本文已参与「新人创作礼」活动，一起开启掘金创作之路。
rrt_exploration有3种类型的节点;
- 用于检测占用网格图中的边界的节点
RRT Detector - 用于过滤检测到的点的节点
Filter Node - 用于将点分配给机器人的节点
Assigner Node
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参考博客：www.cnblogs.com/lc1217/p/69…
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(生长树步长)！
