机器人路径规划之RRT(rrt_exploration)算法源码

rrt_exploration有3种类型的节点;

1. 用于检测占用网格图中的边界的节点 RRT Detector
2. 用于过滤检测到的点的节点 Filter Node
3. 用于将点分配给机器人的节点 Assigner Node

参考文章：rrt_exploration github

1.Local RRT Detector / Global RRT Detector

global_rrt_frontier_detector节点获取一个栅格并在其中找到边界点（即探索目标）。
它发布检测到的点，以便Filter节点可以处理。

local_rrt_frontier_detector节点类似于global_rrt_frontier_detector。
但是，它的工作方式有所不同，因为每次检测到边界点时，树都会不断重置。
该节点旨在沿着global_rrt_frontier_detector节点运行，它负责快速检测位于机器人附近的边界点。运行本地边界检测器的其他实例可以提高边界点检测的速度。

所有检测器将发布到同一topic上（/ detected_points）。

// 获取地图数据
ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack);	
// 添加目标点 point
ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack);	
// 发布探测点 detected points
ros::Publisher targetspub = nh.advertise<geometry_msgs::PointStamped>("/detected_points", 10);
// 可视化
ros::Publisher pub = nh.advertise<visualization_msgs::Marker>(ns+"_shapes", 10);

在回调函数中，将获取得到的地图赋值到全局变量中，rviz中获取的目标点添加到point中。
clicked_point是节点收到定义该区域的五个点。前四点是四个定义要探索的正方形区域，最后一个点是树的起点
在发表了关于这个话题的五点后，RRT将开始检测边界

void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
	mapData=*msg;
}
void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg)
{ 
	geometry_msgs::Point p;  
	p.x=msg->point.x;
	p.y=msg->point.y;
	p.z=msg->point.z;
	
	points.points.push_back(p);
}

RRT中设置至少添加5个点，才会运行。

while(points.points.size()<5)
{
	ros::spinOnce();
	pub.publish(points) ;
}

以下代码用来计算RRT的初始点

std::vector<float> temp1;
temp1.push_back(points.points[0].x);
temp1.push_back(points.points[0].y);	
std::vector<float> temp2; 
temp2.push_back(points.points[2].x);
temp2.push_back(points.points[0].y);
init_map_x=Norm(temp1,temp2);
temp1.clear();		
temp2.clear();

temp1.push_back(points.points[0].x);
temp1.push_back(points.points[0].y);
temp2.push_back(points.points[0].x);
temp2.push_back(points.points[2].y);
init_map_y=Norm(temp1,temp2);
temp1.clear();		
temp2.clear();

Xstartx=(points.points[0].x+points.points[2].x)*.5;
Xstarty=(points.points[0].y+points.points[2].y)*.5;

下面为主要的循环过程，首先定义参数：

// 随机采样点,最近的顶点,新的子节点
std::vector<float> x_rand,x_nearest,x_new;
while (ros::ok()){
	...
	}

eta 参数控制用于检测边界点的RRT的增长率，单位为米。
分别生成随机点，在树上找到与xr最近的节点xnearest，以及再根据eta产生一个新的随机点。

// Sample free
x_rand.clear();
xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx;
yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty;
// 产生一个随机点xr
x_rand.push_back( xr ); 
x_rand.push_back( yr );

// Nearest
x_nearest=Nearest(V,x_rand);

// Steer
x_new=Steer(x_nearest,x_rand,eta);

接下来，计算新产生的随机点与最临近点之间的障碍物：

//ObstacleFree   1:free -1:unkown (frontier region) 0:obstacle
char   checking=ObstacleFree(x_nearest,x_new,mapData);

如果当前点与最临近点之间未建图，即未知的情况下，通过发布exploration_goal目标点，探索位置区域。

targetspub.publish(exploration_goal);

如果未检测到障碍物的话，则直接连成线。

p.x=x_new[0]; 
p.y=x_new[1]; 
p.z=0.0;
line.points.push_back(p);
p.x=x_nearest[0]; 
p.y=x_nearest[1]; 
p.z=0.0;
line.points.push_back(p);

2.Filter Node

Filter 节点从所有检测器接收检测到的边界点，对点进行滤波，并将它们传递给 assigner 节点以指令机器人。
过滤包括旧点和无效点的选择，也是冗余点。

goals_topic = rospy.get_param('~goals_topic', '/detected_points')
global_costmap_topic = rospy.get_param(
        '~global_costmap_topic', '/move_base_node/global_costmap/costmap')

如果存在多机器人协同建图，即n_robots>1，需要将多个地图拼接globalmaps
然后订阅当前move_base发出的costmap

  for i in range(0, n_robots):
        globalmaps.append(OccupancyGrid())

    if len(namespace) > 0:
        for i in range(0, n_robots):
            rospy.Subscriber(namespace+str(i+namespace_init_count) +
                             global_costmap_topic, OccupancyGrid, globalMap)
    elif len(namespace) == 0:
        rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)

将当前goals_topic发出的目标点，从全局坐标系(map)下转换到机器人坐标系(base_link)下：

 tfLisn = tf.TransformListener()
    if len(namespace) > 0:
        for i in range(0, n_robots):
            tfLisn.waitForTransform(global_frame[1:], namespace+str(
                i+namespace_init_count)+'/'+robot_frame, rospy.Time(0), rospy.Duration(10.0))
    elif len(namespace) == 0:
        tfLisn.waitForTransform(
            global_frame[1:], '/'+robot_frame, rospy.Time(0), rospy.Duration(10.0))

    rospy.Subscriber(goals_topic, PointStamped, callback=callBack,
                     callback_args=[tfLisn, global_frame[1:]])

将tfLisn和global_frame传到回调函数callBack中，如果存在单个机器人，则global_frame只有一个，也就是base_link
在回调函数中，将变换后的目标点，通过vstack存入到frontiers中

def callBack(data, args):
    global frontiers, min_distance
    transformedPoint = args[0].transformPoint(args[1], data)
    x = [array([transformedPoint.point.x, transformedPoint.point.y])]
    if len(frontiers) > 0:
        frontiers = vstack((frontiers, x))
    else:
        frontiers = x

Mean-shift参考博客：https://www.cnblogs.com/lc1217/p/6963687.html

Mean-shift（即：均值迁移）的基本思想：在数据集中选定一个点，然后以这个点为圆心，r为半径，画一个圆(二维下是圆)，求出这个点到所有点的向量的平均值，而圆心与向量均值的和为新的圆心，然后迭代此过程，直到满足一点的条件结束。(Fukunage在1975年提出)

    while not rospy.is_shutdown():
        # -------------------------------------------------------------------------
        # Clustering frontier points
        centroids = []
        front = copy(frontiers)
        if len(front) > 1:
            ms = MeanShift(bandwidth=0.3)
            ms.fit(front)
            centroids = ms.cluster_centers_  # centroids array is the centers of each cluster
        # if there is only one frontier no need for clustering, i.e. centroids=frontiers
        if len(front) == 1:
            centroids = front
        frontiers = copy(centroids)

首先设置半径(或带宽) bandwidth=0.3，通过cluster_centers_计算出的聚类中心的坐标。
如果仅存在一个边界点，则它就是聚类中心。

清除旧边界点：
边界点都有 occupancy value，即gridValue(globalmaps[i], x)大于阈值threshold的边界点将被视为无效。
占用价值 occupancy value是从costmap中获得的。

# clearing old frontiers
        z = 0
        while z < len(centroids):
            cond = False
            temppoint.point.x = centroids[z][0]
            temppoint.point.y = centroids[z][1]
            for i in range(0, n_robots):
                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius*0.5)) < 0.2):
                centroids = delete(centroids, (z), axis=0)
                z = z-1
            z += 1

除此之外还有一个判断条件informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius*0.5)) < 0.2
在信息熵的计算中，文件functions.py：

  if(mapData.data[i] == -1 and norm(array(point)-point_of_index(mapData, i)) <= r):
       infoGain += 1

如果遍历的附近点i为未知区域，且在所设定的info_radius*0.5范围之内，则返回值infoGain+1

3.Assigner Node

该节点接收目标探测目标，即目标探测目标，即Filter节点发布过滤的边界点，并相应地控制机器人。
Assigner 节点通过move_base_node控制机器人。
这个模块主要是用于寻找机器人下一步需要到达的点。对于每一个过滤后的边界点，对其进行收益计算，意义在于让机器人优先探索附近的边界点。

rrt_exploration中利用centroids边界点个数动态设置搜索范围

在rrt_exploration自探索选点策略中，分为global和local两种方式随机生长树，在不同的场景下，需要适应不同的随机生长步长，以及不适合在大场景下使用该算法！
因此尝试动态设置搜索范围来解决大场景下的问题，思想类比于局部地图和全局地图！
同理也可以动态设置随机生长步长！

首先需要获得里程计odom和边界点centroids的数据：

  ros::param::param<std::string>(ns+"/odom_topic", odom_topic, "/robot_1/odom"); 
  ros::param::param<std::string>(ns+"/centroids_topic", centroids_topic, "/centroids"); 

在回调函数中：

void odomCallBack(const nav_msgs::Odometry::ConstPtr& msg)
{
  odom_x = msg->pose.pose.position.x;
  odom_y = msg->pose.pose.position.y;
}

void centroidsCallBack(const visualization_msgs::Marker::ConstPtr& msg)
{
  double centroidsN = msg->points.size();
  if(centroidsN < 2){
    distanceMax *=1.1;
  }else if(centroidsN > 5 && distanceMax > 2){
    distanceMax /=1.1;
  }
  if(distanceMax < 1) distanceMax = 1;
  if(distanceMax > 50) distanceMax = 50;
  std::cout<<"current distanceMax:"<<distanceMax<<std::endl;
}

其中distanceMax则为距离当前位置的范围，在此范围之外生成的点则为无效点！

// ObstacleFree    1:free     -1:unkown (frontier region)      0:obstacle
char checking = ObstacleFree(x_nearest,x_new,mapData);
double distance = (x_nearest[0]-odom_x)*(x_nearest[0]-odom_x)+(x_nearest[1]-odom_y)*(x_nearest[1]-odom_y);
if(distance < distanceMax){
	// ......
}

也可通过centroidsCallBack动态修改eta(生长树步长)！