專欄首頁：寫在前面的話

---

文章目錄

• 簡介
• 偽代碼分析
• 功能實現
• • 地圖
  • RRT基礎表示
  • 采樣隨機點
  • Near()
  • Steer()
  • CollisionFree()
• 工程構建
• • 算法啟動文件 rrt_group_search.cpp :
  • RRT基類 RRTGroupSearch.hpp
  • 算法實現文件 RRTSearch.hpp
  • launch文件
• 參考

---

簡介

基本的簡介這里就不做說明了，如果不了解的同學推薦查看文章后面的鏈接。

簡單而言，基于快速擴展隨機樹（RRT / rapidly exploring random tree）的路徑規劃算法，是一種基于采樣的路徑規劃算法，常用于移動機器人路徑規劃，適合解決高維空間和復雜約束下的路徑規劃問題。

偽代碼分析

其偽代碼如下：

分析偽代碼可知，rrt算法的主要步驟包括

Sample rand point(在地圖上隨機采樣一個點 $p_{rand}$，如下圖a );

Near(計算rrt_tree上離隨機點最近的點 $p_{nearest}$, 如下圖b)；

Steer(以 $p_{nearest}$為起點，以 $p_{nearest}$->$p_{rand}$射線為方向，前進StepSize的距離,如下圖c)；

CollisionFree(檢查步驟3中生成路徑的碰撞狀態)

• 該代碼還有一個簡單的改進

例如在 Sample rand point 步驟中可以加入概率P， 由概率P決定是隨機點$P_{rand}$是在地圖上隨機采樣還是目標點$P_{goal}$

功能實現

這里我們只進行基礎rrt的實現

地圖

進行路徑規劃首先要建立地圖，導航地圖有很多種，常見的有二維珊格地圖、三維八叉樹地圖、三維點云地圖等。這里我們使用了簡單的二維珊格地圖(即ros中的nav_msgs::OccupancyGrid)，同時我們假設忽略機器人的尺寸，即認為該二維珊格地圖表征的是機器人的C-Space（何為C-space？）

這里我們直接貼上地圖生成的代碼

2d_map_creater.cpp

二維地圖生成，該程序發布一個名為 “/gridMap” 的二維珊格地圖(分辨率0.05, 長寬為10)， 可以通過參數 obstacle_num 和 obstacle_len 來修改地圖中障礙物的數量與長度

#include "ros/ros.h" #include "nav_msgs/OccupancyGrid.h" #include <iostream>using namespace std;int main(int argc, char * argv[]) {ros::init(argc, argv, "gridMap");ros::NodeHandle nh;ros::NodeHandle nh_private("~");ros::Publisher pub = nh_private.advertise<nav_msgs::OccupancyGrid>("/gridMap", 1);int obstacle_num, obstacle_len;nh_private.param<int>("obstacle_num", obstacle_num, 50);nh_private.param<int>("obstacle_len", obstacle_len, 30);nav_msgs::OccupancyGrid map;// 向Rviz發送的數據中一定要包含frame_idmap.header.frame_id="map";map.header.stamp = ros::Time::now(); map.info.resolution = 0.05; // float32// 柵格地圖分辨率對應柵格地圖中一小格的長和寬map.info.width = 10/map.info.resolution; // uint32map.info.height = 10/map.info.resolution; // uint32// 設置地圖起始坐標　默認為0map.info.origin.position.x = -(map.info.width*map.info.resolution)/2;map.info.origin.position.y = -(map.info.height*map.info.resolution)/2;int p[map.info.width*map.info.height] = {-1}; // [0,100] 只初始化了第一個元素p[21] = 100; //(1, 1) map坐標p[20] = 100; //(1, 0)p[30] = 100;p[31] = -1;p[32] = -1;// rid_map [y*map.info.width+x]生成隨機障礙for(int n=0; n<obstacle_num;n++){int y = obstacle_len + rand()%(map.info.height-obstacle_len*2+1);int x = obstacle_len + rand()%(map.info.width-obstacle_len*2+1);int xy = rand()%(2);if(xy == 0){for(int i=0; i<obstacle_len; i++){p[(y+i)*map.info.width+x] = 100;p[(y+i)*map.info.width+x+1] = 100;}}else{for(int i=0; i<obstacle_len; i++){p[y*map.info.width+x+i] = 100;p[(y+1)*map.info.width+x+i] = 100;}}}// p[int(floor((x-map.info.origin.position.x)/map.info.resolution) + floor((y-map.info.origin.position.y)/map.info.resolution))] = 100;std::vector<signed char> a(p, p+map.info.width*map.info.height);map.data = a;while (ros::ok()){pub.publish(map);cout << "origin: " << map.info.origin.position.x << " " << map.info.origin.position.y << endl;}// ros::shutdown();return 0; }

RRT基礎表示

使用 ros中的 geometry_msgs::Point 來表征點；

使用C++ 中的 unordered_map 表征 RRT樹 (即RRT擴展出來的集合)；

RRT樹中儲存 子節點，父節點對， 方便后續的回溯；

//為geometry_msgs::Point定義一個比較函數與相等函數 struct HashFunc {std::size_t operator()(const geometry_msgs::Point &p) const {using std::size_t;using std::hash;return ((hash<int>()(p.x)^ (hash<int>()(p.y) << 1)) >> 1)^ (hash<int>()(p.z) << 1);} };struct EqualKey {bool operator () (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2) const{return p1.x == p2.x&& p1.y == p2.y&& p1.z == p2.z;} };// 子節點， 父節點 typedef unordered_map<geometry_msgs::Point, geometry_msgs::Point, HashFunc, EqualKey> PointPair;// 定義空rrt樹 PointPair rrt_tree_set_;

使用 visualization_msgs::Marker 中的 POINTS 來可視化采樣出來的點；

visualization_msgs::Marker points_; points_.header.frame_id = mapData_.header.frame_id; points_.header.stamp=ros::Time(0); points_.ns = "points"; points_.id = 0; points_.type=points_.POINTS; points_.action=points_.ADD; points_.pose.orientation.w = 1.0; points_.scale.x=mapData_.info.resolution; points_.scale.y=mapData_.info.resolution; points_.color.r = 0.0/255.0; points_.color.g = 0.0/255.0; points_.color.b = 255.0/255.0; points_.color.a=1.0; points_.lifetime = ros::Duration();

使用visualization_msgs::Marker中的 LINE_LIST 來可視化rrt樹；

visualization_msgs::Marker line_, final_line_; // 灰色線段表示rrt line_.header.frame_id=mapData_.header.frame_id; line_.header.stamp=ros::Time(0); line_.ns = "lines"; line_.id = 1; line_.type=line_.LINE_LIST; line_.action=line_.ADD; line_.pose.orientation.w = 1.0; line_.scale.x = 0.03; line_.scale.y= 0.03; line_.color.r =155.0/255.0; line_.color.g= 155.0/255.0; line_.color.b =155.0/255.0; line_.color.a = 1.0; line_.lifetime = ros::Duration();//紅色線段表示最終搜索出來的路徑 final_line_.header.frame_id=mapData_.header.frame_id; final_line_.header.stamp=ros::Time(0); final_line_.ns = "line_back"; final_line_.id = 1; final_line_.type=final_line_.LINE_LIST; final_line_.action=final_line_.ADD; final_line_.pose.orientation.w = 1.0; final_line_.scale.x = 0.04; final_line_.scale.y = 0.04; final_line_.color.r =255.0/255.0; final_line_.color.g= 0.0/255.0; final_line_.color.b =0.0/255.0; final_line_.color.a = 1.0; final_line_.lifetime = ros::Duration();

采樣隨機點

// 返回 min max之間的隨機數 double getRandData(int min,int max){double m1=(double)(rand()%101)/101; // 計算 0，1之間的隨機小數,得到的值域近似為(0,1)min++; //將 區間變為(min+1,max),double m2=(double)((rand()%(max-min+1))+min); //計算 min+1,max 之間的隨機整數，得到的值域為[min+1,max]m2=m2-1; //令值域為[min,max-1]return m1+m2; //返回值域為(min,max),為所求隨機浮點數 }

Near()

// 計算并返回rrt_tree上離隨機點最近的點 geometry_msgs::Point Near(PointPair tree, geometry_msgs::Point p){geometry_msgs::Point nearst_point;float temp;float min = 100000;for (auto pt : tree){temp = Norm(pt.first, p); //返回兩點之前的歐式距離if (temp < min){min = temp;nearst_point = pt.first;}}return nearst_point; }

Steer()

// 以nearest_point為起點，以nearest_point->rand_point射線為方向，前進eta的距離 geometry_msgs::Point Steer(geometry_msgs::Point nearest_point, geometry_msgs::Point rand_point, float eta){geometry_msgs::Point new_point;float dis = Norm(nearest_point, rand_point);if (dis <= eta){new_point = rand_point;}else{new_point.x = nearest_point.x + eta * (rand_point.x - nearest_point.x) / dis;new_point.y = nearest_point.y + eta * (rand_point.y - nearest_point.y) / dis; }return new_point; }

根據相似三角形：

$$\frac{eta}{dis}=\frac{p_{new}?p_{nearest}}{p_{rand}?p_{nearest}}$$

CollisionFree()

關于碰撞檢測有兩種方法增量法(圖左)或者等分法(圖右)

這里采用增量法, 每步的增量為地圖分辨率的一半，針對連線上采樣到的節點，檢查其在地圖上占據狀態

// 占據狀態　1:占據　0:未占據　-1:未知 int CollisionFree(geometry_msgs::Point nearest_point, geometry_msgs::Point new_point, nav_msgs::OccupancyGrid map){std::vector<signed char> Data = map.data;float re = map.info.resolution;float Pstartx = map.info.origin.position.x;float Pstarty = map.info.origin.position.y;float width = map.info.width;float step_length = map.info.resolution * 0.5;// //ceil(x)返回的是大于x的最小整數int step_number = ceil(Norm(nearest_point, new_point)/step_length);geometry_msgs::Point step_point = nearest_point;int state, out;for (int i = 0; i < step_number; i++){step_point = Steer(step_point, new_point, step_length);// floor(x)返回的是小于或等于x的最大整數float index = floor((step_point.y - Pstarty)/re)*width + floor((step_point.x - Pstartx)/re);out = Data[int(index)];if (out == 100) // 被占據{state = 1;break;}if (out == -1) //未知{state = -1;break;}if (out == 0){state = 0;continue;}}return state;}

---

工程構建

前面講解了rrt算法的基礎函數實現，現在來看看如何在ros中構建工程，

這里我們需要寫三個源文件和一個luanch文件

算法啟動文件 rrt_group_search.cpp :

負責選取rrt算法類型、 支持算法可視化(rviz)、與rviz交互,包括獲取地圖信息與起始點信息(使用的是rviz中的 /clicked_point)；

該程序有兩個線程：一個線程負責執行rrt搜索， 另一個線程負責實時可視化搜索結果

這樣做有利于把冗雜的可視化代碼從算法的主體代碼中剝離出來

/*************************************************************************** rrt_group_search.cpp* * @Author： bornchow* * @Description:* 本程序為rrt系列路徑搜索算法的啟動文件，提供以下功能：* 1. 選取rrt算法類型* 2. 支持算法可視化(rviz)* 3. 與rviz交互,包括獲取地圖信息與起始點信息* * ****************************************************/ #include "ros/ros.h" #include "nav_msgs/OccupancyGrid.h" #include "geometry_msgs/Point.h" #include "geometry_msgs/PointStamped.h" #include "visualization_msgs/Marker.h" #include <iostream> #include <vector> #include <thread> #include "RRTSearch.hpp"class RRTStart { private:ros::NodeHandle n;ros::NodeHandle n_;ros::Subscriber global_map_sub_;ros::Subscriber goal_sub_;ros::Publisher rrt_node_pub_;ros::Publisher rrt_tree_pub_;ros::Publisher rrt_final_tree_pub_;// ros parmint rrt_algorithm_;float eta_;int iter_step_;int iter_max_;float search_radius_;nav_msgs::OccupancyGrid mapData_;visualization_msgs::Marker points_goal_, points_, line_, final_line_;// rrt algorithmshared_ptr<RRTGroupSearch> rrt_group_search_;private:void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg);void goalCallBack(const geometry_msgs::PointStamped::ConstPtr& msg);public:RRTStart(ros::NodeHandle nh, ros::NodeHandle nh_);void visRRTProcessThread();void run();~RRTStart(); };RRTStart::RRTStart(ros::NodeHandle nh, ros::NodeHandle nh_) : n(nh), n_(nh_) {//ros 參數nh_.param<int>("rrt_algorithm", rrt_algorithm_, 0); // 選擇rrt算法nh_.param<float>("eta", eta_, 0.3); //rrt每次擴展時，延伸的長度nh_.param<int>("iter_step", iter_step_, 3100); // 為了更好的顯示，每隔iter_step步，需要鍵入值以繼續// ros訂閱與發布global_map_sub_ = n.subscribe("/gridMap", 100, &RRTStart::mapCallBack, this);goal_sub_ = n.subscribe("/clicked_point", 100, &RRTStart::goalCallBack, this);rrt_node_pub_ = nh.advertise<visualization_msgs::Marker>("rrt_node", 10);rrt_tree_pub_ = nh.advertise<visualization_msgs::Marker>("rrt_tree", 10);rrt_final_tree_pub_ = nh.advertise<visualization_msgs::Marker>("rrt_final_tree", 100);// wait until map is received, when a map is received, mapData.header.seq will not be < 1 while (mapData_.header.seq<1 or mapData_.data.size()<1){ROS_INFO("waiting for map !");ros::spinOnce(); ros::Duration(0.1).sleep();}//定義可視化參數// 起始點可視化points_goal_.header.frame_id = mapData_.header.frame_id;points_goal_.header.stamp=ros::Time(0);points_goal_.ns = "points_goal";points_goal_.id = 0;points_goal_.type=points_goal_.POINTS;points_goal_.action=points_goal_.ADD;points_goal_.pose.orientation.w = 1.0;points_goal_.scale.x=mapData_.info.resolution; points_goal_.scale.y=mapData_.info.resolution;points_goal_.color.r = 0.0/255.0;points_goal_.color.g = 255.0/255.0;points_goal_.color.b = 255.0/255.0;points_goal_.color.a=1.0;points_goal_.lifetime = ros::Duration();points_.header.frame_id = mapData_.header.frame_id;points_.header.stamp=ros::Time(0);points_.ns = "points";points_.id = 0;points_.type=points_.POINTS;points_.action=points_.ADD;points_.pose.orientation.w = 1.0;points_.scale.x=mapData_.info.resolution; points_.scale.y=mapData_.info.resolution;points_.color.r = 0.0/255.0;points_.color.g = 0.0/255.0;points_.color.b = 255.0/255.0;points_.color.a=1.0;points_.lifetime = ros::Duration();line_.header.frame_id=mapData_.header.frame_id;line_.header.stamp=ros::Time(0);line_.ns = "lines";line_.id = 1;line_.type=line_.LINE_LIST;line_.action=line_.ADD;line_.pose.orientation.w = 1.0;line_.scale.x = 0.03;line_.scale.y= 0.03;line_.color.r =155.0/255.0;line_.color.g= 155.0/255.0;line_.color.b =155.0/255.0;line_.color.a = 1.0;line_.lifetime = ros::Duration();final_line_.header.frame_id=mapData_.header.frame_id;final_line_.header.stamp=ros::Time(0);final_line_.ns = "line_back";final_line_.id = 1;final_line_.type=final_line_.LINE_LIST;final_line_.action=final_line_.ADD;final_line_.pose.orientation.w = 1.0;final_line_.scale.x = 0.04;final_line_.scale.y = 0.04;final_line_.color.r =255.0/255.0;final_line_.color.g= 0.0/255.0;final_line_.color.b =0.0/255.0;final_line_.color.a = 1.0;final_line_.lifetime = ros::Duration();switch (rrt_algorithm_){case 0:{rrt_group_search_ = make_shared<RRTSearch>(eta_, iter_step_);break;}default:break;}}RRTStart::~RRTStart() { }void RRTStart::visRRTProcessThread(){ros::Rate rate(10); geometry_msgs::Point p_temp;std::map<int, std::queue<geometry_msgs::Point> > iter_vis_info;std::queue<geometry_msgs::Point> p_final_que;int count;while (ros::ok()){rrt_group_search_->getVisInfo(iter_vis_info, p_final_que, count);if (count == 0) //迭代未開始{continue;}else{for (auto it : iter_vis_info){points_.points.push_back(it.second.front());it.second.pop();rrt_node_pub_.publish(points_);while (!it.second.empty()){line_.points.push_back(it.second.front());it.second.pop();line_.points.push_back(it.second.front());it.second.pop();rrt_tree_pub_.publish(line_);}}if (rrt_group_search_->getSearchState()){// 發布最后的rrt if (!p_final_que.empty()){std::cout << "final line size: " << final_line_.points.size() << std::endl;final_line_.points.clear();std::cout << "final line size after: " << final_line_.points.size() << std::endl;rrt_final_tree_pub_.publish(final_line_);final_line_.header.frame_id=mapData_.header.frame_id;final_line_.header.stamp=ros::Time(0);final_line_.ns = "line_back";final_line_.id = count;final_line_.type=final_line_.LINE_LIST;final_line_.action=final_line_.ADD;final_line_.pose.orientation.w = 1.0;final_line_.scale.x = 0.04;final_line_.scale.y = 0.04;final_line_.color.r =255.0/255.0;final_line_.color.g= 0.0/255.0;final_line_.color.b =0.0/255.0;final_line_.color.a = 1.0;final_line_.lifetime = ros::Duration();std::cout << " --------- " << p_final_que.size() << std::endl;p_temp = p_final_que.front();p_final_que.pop();while (!p_final_que.empty()){final_line_.points.push_back(p_temp);p_temp = p_final_que.front();final_line_.points.push_back(p_temp);p_final_que.pop();rrt_final_tree_pub_.publish(final_line_);sleep(0.1);}std::cout << "final line size add: " << final_line_.points.size() << std::endl;}else{continue;}//break;}}rate.sleep();}}void RRTStart::run(){// 獲取起始點double last_command = ros::Time::now().toSec();while (points_goal_.points.size()<3){double now = ros::Time::now().toSec();if (points_goal_.points.size()<1 && now - last_command > 2){ROS_INFO("set {start point} in rviz");last_command = ros::Time::now().toSec();}else if (points_goal_.points.size()<2 && now - last_command > 2){ROS_INFO("set {goal point} in rviz");last_command = ros::Time::now().toSec();}else if (points_goal_.points.size()<3 && now - last_command > 2){ROS_INFO("clicked point to start!!!");last_command = ros::Time::now().toSec();}rrt_node_pub_.publish(points_goal_);ros::spinOnce();}//加載地圖rrt_group_search_->updateMap(mapData_);ROS_INFO("map info origin [%f, %f] -- size [%d, %d]", mapData_.info.origin.position.x, mapData_.info.origin.position.y, mapData_.info.width, mapData_.info.height);geometry_msgs::Point p_start, p_end;p_start.x = points_goal_.points[0].x; p_start.y = points_goal_.points[0].y;p_end.x = points_goal_.points[1].x; p_end.y = points_goal_.points[1].y;rrt_group_search_->search(p_start, p_end);}void RRTStart::mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg){mapData_ = *msg; }void RRTStart::goalCallBack(const geometry_msgs::PointStamped::ConstPtr& msg){points_goal_.points.push_back(msg->point); } class end; int main(int argc, char** argv){ros::init(argc, argv, "rrt_group_search");ros::NodeHandle nh;ros::NodeHandle nh_("~");ros::Rate rate(10);RRTStart rrt_start(nh, nh_);std::thread visRRTThread(&RRTStart::visRRTProcessThread, &rrt_start);rrt_start.run();visRRTThread.join();}

RRT基類 RRTGroupSearch.hpp

為啥要搞個RRT基類尼，直接寫個RRT類不就好了嗎？

當然直接寫也可以，但是RRT系列中還有后面的RRT- Connect 、 RRT-Star等等，里面有很多功能都是重復的；前面提到的四個基礎的功能實現相差不大。所以為了方便，先寫個基類，把這些基礎的定義與公用的功能實現，然后具體的RRT算法都繼承于此。

#pragma once #include "nav_msgs/OccupancyGrid.h" #include "geometry_msgs/Point.h" #include "geometry_msgs/PointStamped.h" #include "visualization_msgs/Marker.h" #include "ros/ros.h" #include <iostream> #include <vector> #include <unordered_map> #include <map> #include <queue>using namespace std;//為geometry_msgs::Point定義一個比較函數與相等函數 struct HashFunc {std::size_t operator()(const geometry_msgs::Point &p) const {using std::size_t;using std::hash;return ((hash<int>()(p.x)^ (hash<int>()(p.y) << 1)) >> 1)^ (hash<int>()(p.z) << 1);} };struct EqualKey {bool operator () (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2) const{return p1.x == p2.x&& p1.y == p2.y&& p1.z == p2.z;} };struct NodeCost{geometry_msgs::Point this_node;geometry_msgs::Point father_node;float cost; };// 子節點， 父節點 typedef unordered_map<geometry_msgs::Point, geometry_msgs::Point, HashFunc, EqualKey> PointPair;// 基類定義class RRTGroupSearch { private:/* data */ public:RRTGroupSearch(/* args */);virtual void updateMap(nav_msgs::OccupancyGrid map_data){std::cout << " this is updateMap function in base " << std::endl;}virtual void search(geometry_msgs::Point p_start, geometry_msgs::Point p_end){std::cout << " this is search function in base " << std::endl;}virtual bool getSearchState(){;}virtual void getVisInfo(std::map<int, std::queue<geometry_msgs::Point> >& iter_vis_info,std::queue<geometry_msgs::Point> &p_final_q,int &iter){std::cout << " this is getVisInfo function in base " << std::endl;}~RRTGroupSearch();public:geometry_msgs::Point Steer(geometry_msgs::Point nearest_point, geometry_msgs::Point rand_point, float eta);geometry_msgs::Point Near(PointPair tree, geometry_msgs::Point p);float Norm(geometry_msgs::Point p1, geometry_msgs::Point p2);double getRandData(int min,int max);bool nearGoal(geometry_msgs::Point new_point, geometry_msgs::Point end_point);int CollisionFree(geometry_msgs::Point nearest_point, geometry_msgs::Point new_point, nav_msgs::OccupancyGrid map);};RRTGroupSearch::RRTGroupSearch(/* args */) {std::cout << "this is rrt group method " << std::endl; }RRTGroupSearch::~RRTGroupSearch() { }// 返回 min max之間的隨機數 double RRTGroupSearch::getRandData(int min,int max){double m1=(double)(rand()%101)/101; // 計算 0，1之間的隨機小數,得到的值域近似為(0,1)min++; //將 區間變為(min+1,max),double m2=(double)((rand()%(max-min+1))+min); //計算 min+1,max 之間的隨機整數，得到的值域為[min+1,max]m2=m2-1; //令值域為[min,max-1]return m1+m2; //返回值域為(min,max),為所求隨機浮點數 }// 計算rrt_tree上離隨機點最近的點 geometry_msgs::Point RRTGroupSearch::Near(PointPair tree, geometry_msgs::Point p){geometry_msgs::Point nearst_point;float temp;float min = 100000;for (auto pt : tree){temp = Norm(pt.first, p);if (temp < min){min = temp;nearst_point = pt.first;}}return nearst_point; }//　返回兩點的歐式距離 float RRTGroupSearch::Norm(geometry_msgs::Point p1, geometry_msgs::Point p2){return pow( pow(p1.x-p2.x, 2) + pow(p1.y-p2.y,2), 0.5); }// 以nearest_point為起點，以nearest_point->rand_point射線為方向，前進eta的距離 geometry_msgs::Point RRTGroupSearch::Steer(geometry_msgs::Point nearest_point, geometry_msgs::Point rand_point, float eta){geometry_msgs::Point new_point;float dis = Norm(nearest_point, rand_point);if (dis <= eta){new_point = rand_point;}else{new_point.x = nearest_point.x + eta * (rand_point.x - nearest_point.x) / dis;new_point.y = nearest_point.y + eta * (rand_point.y - nearest_point.y) / dis; }return new_point; }bool RRTGroupSearch::nearGoal(geometry_msgs::Point new_point, geometry_msgs::Point end_point){if(Norm(new_point, end_point) < 0.5){return true;}else{return false;} }// 占據狀態　1:占據　0:未占據　-1:未知 int RRTGroupSearch::CollisionFree(geometry_msgs::Point nearest_point, geometry_msgs::Point new_point, nav_msgs::OccupancyGrid map){std::vector<signed char> Data = map.data;float re = map.info.resolution;float Pstartx = map.info.origin.position.x;float Pstarty = map.info.origin.position.y;float width = map.info.width;float step_length = map.info.resolution * 0.5;// //ceil(x)返回的是大于x的最小整數int step_number = ceil(Norm(nearest_point, new_point)/step_length);geometry_msgs::Point step_point = nearest_point;int state, out;for (int i = 0; i < step_number; i++){step_point = Steer(step_point, new_point, step_length);// floor(x)返回的是小于或等于x的最大整數float index = floor((step_point.y - Pstarty)/re)*width + floor((step_point.x - Pstartx)/re);out = Data[int(index)];if (out == 100) // 被占據{state = 1;break;}if (out == -1) //未知{state = -1;break;}if (out == 0){state = 0;continue;}}return state;}

算法實現文件 RRTSearch.hpp

好了，千呼萬喚始出來，終于寫主體文件了。由于前面已經把基礎的功能函數都在基類中實現了一遍，

所以主體文件只需要實現算法的流程search()函數，然后把需要可視化的東西反饋給啟動文件就可以了getVisInfo()函數。

/*************************************************************************** RRTSearch.hpp* * @Author： bornchow* * @Description:* 本程序為rrt系列路徑搜索算法中原始rrt的實現* 1. 實現原始選取rrt算法* 2. 將需要顯示的信息反饋* * ****************************************************/ #include "RRTGroupSearch.hpp" #include "tic_toc.h" #include <mutex>// class RRTSearchclass RRTSearch : public RRTGroupSearch { private:PointPair rrt_tree_set_; nav_msgs::OccupancyGrid map_;bool map_updated_;bool search_finished_;int iter_; // 計數迭代次數geometry_msgs::Point p_new_, p_rand_, p_nearest_;geometry_msgs::Point p_start_, p_end_;std::map<int, std::queue<geometry_msgs::Point> > iter_vis_set_;std::queue<geometry_msgs::Point> p_final_que_;std::mutex vis_mut_;// 參數float eta_;int iter_step_;public:RRTSearch(float eta, int iter_step); virtual void updateMap(nav_msgs::OccupancyGrid map_data);virtual void search(geometry_msgs::Point p_start, geometry_msgs::Point p_end);virtual bool getSearchState();virtual void getVisInfo(std::map<int, std::queue<geometry_msgs::Point> >& iter_vis_info,std::queue<geometry_msgs::Point> &p_final_q,int &iter);~RRTSearch(); };//****************************************************** // 【輸入】 - eta : 每隔迭代中rrt樹擴展的距離 // 【輸入】 - iter_step : 每隔多少步，需要鍵入以繼續 RRTSearch::RRTSearch(float eta, int iter_step) : eta_(eta), iter_step_(iter_step) {iter_ = 0;map_updated_ = false;search_finished_ = false;ROS_INFO("\033[1;32m----> this is a [2D] [rrt] search method!! .\033[0m"); }RRTSearch::~RRTSearch() { }void RRTSearch::updateMap(nav_msgs::OccupancyGrid map_data){map_ = map_data;map_updated_ = true; }void RRTSearch::search(geometry_msgs::Point p_start, geometry_msgs::Point p_end){int a, checking;p_start_ = p_start;p_end_ = p_end;rrt_tree_set_.insert(make_pair(p_start_,p_start_));std::queue<geometry_msgs::Point> vis_point_que;// std::cout << " ------ " << std::endl;// for(auto &p : rrt_tree_set_){// std::cout << "{--- " << p.first << " : " << p.second << "---} " << std::endl;// }// std::cout << " ------ " << std::endl;while (!search_finished_){if (iter_ != 0) // 每次迭代結束處理{vis_mut_.lock();iter_vis_set_.insert(make_pair(iter_, vis_point_que));vis_mut_.unlock();}iter_++;while (!vis_point_que.empty()){vis_point_que.pop();}TicToc search_time;if (!map_updated_){ROS_INFO("no map info !! use updateMap function first");break;}if(iter_ % iter_step_ == 0){std::cout << "input any to go on!!" << std::endl;a = cin.get();}// step 1: 隨機采樣一個點p_rand_.x = getRandData(map_.info.origin.position.x, map_.info.origin.position.x + map_.info.width*map_.info.resolution);p_rand_.y = getRandData(map_.info.origin.position.y, map_.info.origin.position.y + map_.info.height*map_.info.resolution);vis_point_que.push(p_rand_); // 儲存p_rand用于顯示ROS_INFO("rand sample a point [ %d ] -- (x %f , y %f)", iter_, p_rand_.x, p_rand_.y);// setp 2: 計算rrt_tree上離隨機點(p_rand_)最近的點p_nearest_p_nearest_ = Near(rrt_tree_set_, p_rand_);ROS_INFO("nearest point [ %d ]-- (x %f , y %f)", iter_, p_nearest_.x, p_nearest_.y);// step 3: 沿著p_nearest_ --> p_rand_方向， 以eta為步長，生成新的樹節點 p_new_p_new_ = Steer(p_nearest_, p_rand_, eta_);ROS_INFO("new point [ %d ]-- (x %f , y %f)", iter_, p_new_.x, p_new_.y);// step 4: 碰撞檢測 1:占據　0:未占據　-1:未知checking = CollisionFree(p_nearest_, p_new_, map_);ROS_INFO("the CollisionFree is : %d", checking);if (checking == 1){ROS_INFO("collision");continue;}if (checking == -1){ROS_INFO("get a unknown area!");continue;}if (checking == 0){ROS_INFO("get a new point!");//將p_new_加入rrt treerrt_tree_set_.insert(make_pair(p_new_, p_nearest_));// 將 p_new_， p_nearest_ 儲存用于顯示vis_point_que.push(p_new_);vis_point_que.push(p_nearest_);//判斷p_new_是否與終點接近if (nearGoal(p_new_, p_end_)){//將p_end_加入rrt treerrt_tree_set_.insert(make_pair(p_end_, p_new_));ROS_INFO("you have got the goal!----");// 回溯一條rrt路徑vis_mut_.lock();p_final_que_.push(p_end_);while (Norm(p_start_, p_final_que_.back()) > 0.05){auto find_it = rrt_tree_set_.find(p_final_que_.back());if ( find_it != rrt_tree_set_.end()){p_final_que_.push(find_it->second);}else{ROS_INFO("point not in rrt_tree");break;}std::cout << " dis form start: " << Norm(p_start_, p_final_que_.back()) << std::endl;}if (Norm(p_start_, p_final_que_.back()) < 0.05){ROS_INFO("back tree sucess!");}vis_mut_.unlock();search_finished_ = true;ROS_INFO("Time : %f", search_time.toc());}}}}//**************************************** // 【返回】 std::map<int, std::queue<geometry_msgs::Point> >& iter_vis_info // iter_vis_info.first 為 算法迭代次數 // iter_vis_info.second 為 需要顯示的點, 存儲順序為 p_rand, p_new, p_nearest, p_new_2, p_nearest_2, ... // 【返回】std::queue<geometry_msgs::Point> &p_final_q 最終的路徑 void RRTSearch::getVisInfo(std::map<int, std::queue<geometry_msgs::Point> >& iter_vis_info,std::queue<geometry_msgs::Point> &p_final_q,int &iter){vis_mut_.lock();if (!iter_vis_set_.empty()){copy(iter_vis_set_.begin(), iter_vis_set_.end(), inserter(iter_vis_info, iter_vis_info.begin()));iter_vis_set_.clear();}if (search_finished_){while (!p_final_que_.empty()){p_final_q.push(p_final_que_.front());p_final_que_.pop();}}vis_mut_.unlock();iter = iter_; }bool RRTSearch::getSearchState(){return search_finished_; }

launch文件

這里的navi_algorithm 是我的ROS工作空間

<launch><!-- 啟動二維地圖 --><node pkg="navi_algorithm" type="2d_map_creater" name="map_creater_node" ><param name="obstacle_num" value="60" /><param name="obstacle_len" value="20" /></node><!--rrt 算法--><!-- 0- rrt --><node pkg="navi_algorithm" type="rrt_group_search" name="rrt_search" output="screen"><param name="rrt_algorithm" value="0" /> <!--選擇rrt算法--><param name="eta" value="0.3" /> <!--rrt每次擴展時，延伸的長度 --><param name="iter_step" value="3100" /> <!-- 為了更好的顯示，每隔iter_step步，需要鍵入值以繼續 --></node><!-- RViz --><node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find navi_algorithm)/rviz/2d_map.rviz"/></launch>

最后貼一張結果：

參考

RRT路徑規劃算法

【機器人路徑規劃】快速擴展隨機樹(RRT)算法

RRT算法__yuan_的博客-CSDN博客_rrt算法

RRT路徑規劃算法_unique_jie的博客-CSDN博客_rrt路徑規劃算法

有哪些應用于移動機器人路徑規劃的算法？

總結

以上是生活随笔為你收集整理的基于采样的方法（一） RRT的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。