基于采样的方法（一） RRT

BornChow

已于 2022-04-11 18:37:01 修改

阅读量1k

点赞数

分类专栏：从零开始学习机器人路径规划文章标签：自动驾驶

于 2022-04-11 18:35:56 首次发布

本文链接：https://blog.csdn.net/simonchow11/article/details/124105461

版权

从零开始学习机器人路径规划专栏收录该内容

2 篇文章 0 订阅

订阅专栏

专栏首页：写在前面的话

文章目录

简介
伪代码分析
功能实现
工程构建
参考

简介

基本的简介这里就不做说明了，如果不了解的同学推荐查看文章后面的链接。

简单而言，基于快速扩展随机树（RRT / rapidly exploring random tree）的路径规划算法，是一种基于采样的路径规划算法，常用于移动机器人路径规划，适合解决高维空间和复杂约束下的路径规划问题。

伪代码分析

其伪代码如下：

rrt

分析伪代码可知，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_id
    map.header.frame_id="map";
    map.header.stamp = ros::Time::now(); 
    map.info.resolution = 0.05;          // float32
    // 栅格地图分辨率对应栅格地图中一小格的长和宽
    map.info.width      = 10/map.info.resolution;           // uint32
    map.info.height     = 10/map.info.resolution;           // uint32

    // 设置地图起始坐标　默认为0
    map.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 parm
    int 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 algorithm
    shared_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 RRTSearch

class 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 tree
            rrt_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 tree
                rrt_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路径规划算法

有哪些应用于移动机器人路径规划的算法？

BornChow

关注

0
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
基于采样的方法（一） RRT

专栏首页：写在前面的话文章目录简介伪代码分析功能实现地图RRT基础表示采样随机点Near()Steer()CollisionFree()工程构建算法启动文件 rrt_group_search.cpp :RRT基类 RRTGroupSearch.hpp算法实现文件 RRTSearch.hpplaunch文件简介基本的简介这里就不做说明了，如果不了解的同学推荐查看下面的文章：RRT路径规划算法【机器人路径规划】快速扩展随机树(RRT)算法RRT算法__yuan_的博客-CSDN博客_rrt算法
复制链接

扫一扫