各文件运行顺序：

1. simple.launch
2. global_rrt_detector.cpp
3. local_rrt_detector.cpp
4. filter.py
5. assigner.py

一、simple.launch

<!-- Launch file for the rrt-detector and the assigner --><launch><arg name="eta" value="1.0"/><arg name="Geta" value="15.0"/><param name="namespace_init_count" value="1"/><node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen"><param name="eta" value="$(arg Geta)"/><param name="map_topic" value="/map"/><!-- <param name="map_topic" value="/move_base/global_costmap/costmap"/> --></node><node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen"><param name="eta" value="$(arg eta)"/><param name="map_topic" value="/map"/><!-- <param name="map_topic" value="/move_base/global_costmap/costmap"/> --><!-- <param name="robot_frame" value="/base_link"/> --><param name="robot_frame" value="base_link"/></node><node pkg="rrt_exploration" type="filter.py" name="filter" output="screen"><param name="map_topic" value="/map"/><!-- <param name="map_topic" value="/move_base/global_costmap/costmap"/> --><param name="info_radius" value="1"/> <param name="costmap_clearing_threshold" value="70"/>   <!-- 清除阈值 --><param name="goals_topic" value="/detected_points"/><param name="namespace" value=""/> <param name="n_robots" value="1"/><param name="rate" value="100"/><param name="global_costmap_topic" value="/move_base/global_costmap/costmap"/><param name="robot_frame" value="base_link"/></node><node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen"><param name="map_topic" value="/map"/><!-- <param name="map_topic" value="/move_base/global_costmap/costmap"/> --><!-- <param name="global_frame" value="/map"/> --><param name="global_frame" value="map"/><param name="info_radius" value="1"/> <param name="info_multiplier" value="3.0"/>   <!-- 信息乘数 --><param name="hysteresis_radius" value="3.0"/>   <!-- 惯性半径 --><param name="hysteresis_gain" value="2.0"/>   <!-- 惯性增益 --><param name="frontiers_topic" value="/filtered_points"/> <param name="n_robots" value="1"/><param name="namespace" value=""/><param name="delay_after_assignement" value="0.5"/> <!-- 分配后的延迟 --><param name="rate" value="100"/></node></launch>

二、global_rrt_detector.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <iostream>
#include <string>
#include <vector>
#include "stdint.h"
#include "functions.h"
#include "mtrand.h"#include "nav_msgs/OccupancyGrid.h"
#include "geometry_msgs/PointStamped.h"
#include "std_msgs/Header.h"
#include "nav_msgs/MapMetaData.h"
#include "geometry_msgs/Point.h"
#include "visualization_msgs/Marker.h"
#include <tf/transform_listener.h>// global variables
nav_msgs::OccupancyGrid mapData;
geometry_msgs::PointStamped clickedpoint;
geometry_msgs::PointStamped exploration_goal;
visualization_msgs::Marker points,line;
float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y;rdm r; // for genrating random numbers

// 1.
int main(int argc, char **argv)
{unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7;	// 初始化随机数生成器MTRand_int32 irand(init, length); // 32位整数生成器// this is an example of initializing by an array// you may use MTRand(seed) with any 32bit integer// as a seed for a simpler initializationMTRand drand; // double in [0, 1) generator, already init	[0, 1)的双精度浮点数生成器，默认已初始化// generate the same numbers as in the original C test program	ros::init(argc, argv, "global_rrt_frontier_detector");	// ROS节点初始化ros::NodeHandle nh;	// 创建ROS节点句柄// fetching all parameters	获取所有参数float eta,init_map_x,init_map_y,range;std::string map_topic,base_frame_topic;std::string ns;ns=ros::this_node::getName();ros::param::param<float>(ns+"/eta", eta, 0.5);ros::param::param<std::string>(ns+"/map_topic", map_topic, "/robot_1/map"); // 创建ROS订阅器和发布器ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack);		// 订阅地图信息ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack);	// 订阅RViz中点击的点ros::Publisher targetspub = nh.advertise<geometry_msgs::PointStamped>("/detected_points", 10);	// 发布探测到的点ros::Publisher pub = nh.advertise<visualization_msgs::Marker>(ns+"_shapes", 10);	// 发布形状标记ros::Rate rate(100); 	// 设置ROS节点的循环频率// wait until map is received, when a map is received, mapData.header.seq will not be < 1  // 等待地图接收，当接收到地图时，mapData.header.seq不会小于1while (mapData.header.seq<1 or mapData.data.size()<1)  {  ros::spinOnce();  ros::Duration(0.1).sleep();}//visualizations  points and lines..	设置可视化标记（visualization markers）的属性，包括点和线的样式、颜色、大小等points.header.frame_id=mapData.header.frame_id;	//将points标记的坐标系设置为地图数据的坐标系。line.header.frame_id=mapData.header.frame_id;	//将line标记的坐标系设置为地图数据的坐标系points.header.stamp=ros::Time(0);	//将points标记的时间戳设置为0，表示最新的时间line.header.stamp=ros::Time(0);		points.ns=line.ns = "markers";	//设置points和line标记的命名空间为"markers"points.id = 0;	//设置points标记的ID为0line.id =1;points.type = points.POINTS;	//将points标记的类型设置为POINTS，表示它是由一系列点组成的标记line.type=line.LINE_LIST;	//将line标记的类型设置为LINE_LIST，表示它是由一系列线段组成的标记。//Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)points.action =points.ADD;	//设置points标记的动作为ADD，表示将添加新标记line.action = line.ADD;	//设置line标记的动作为ADD，表示将添加新标记points.pose.orientation.w =1.0;	//设置points标记的姿态的四元数的w分量为1.0line.pose.orientation.w = 1.0;	//设置line标记的姿态的四元数的w分量为1.0line.scale.x =  0.03;	//设置line标记的线段宽度为0.03line.scale.y= 0.03;points.scale.x=0.3; points.scale.y=0.3; line.color.r =9.0/255.0;	//设置line标记的颜色为红色（R=9）line.color.g= 91.0/255.0;	//绿色（G=91）line.color.b =236.0/255.0;	//蓝色（B=236）points.color.r = 255.0/255.0;points.color.g = 0.0/255.0;points.color.b = 0.0/255.0;points.color.a=1.0;	//设置points标记的颜色的透明度为1.0line.color.a = 1.0;points.lifetime = ros::Duration();	//设置points标记的生命周期为默认值，表示它一直保持可见。line.lifetime = ros::Duration();geometry_msgs::Point p;  //创建一个geometry_msgs::Point类型的变量p，用于表示3D点的坐标。// 等待至少有5个点的可视化while(points.points.size()<5){ros::spinOnce();pub.publish(points) ;}// 对前两个点进行一些计算std::vector<float> temp1;	//创建一个名为temp1的浮点数向量，用于存储points中的第一个点的x和y坐标temp1.push_back(points.points[0].x);	//将points中的第一个点的x坐标加入到temp1中。temp1.push_back(points.points[0].y);	//将points中的第一个点的y坐标加入到temp1中。std::vector<float> temp2; temp2.push_back(points.points[2].x);temp2.push_back(points.points[0].y);init_map_x=Norm(temp1,temp2);	//调用Norm函数计算temp1和temp2表示的两个点之间的距离，并将结果存储在init_map_x中temp1.clear();	//清空temp1和temp2向量，以便重新使用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);	//调用Norm函数计算temp1和temp2表示的两个点之间的距离，并将结果存储在init_map_y中。temp1.clear();		temp2.clear();// 计算起始位置的中点Xstartx=(points.points[0].x+points.points[2].x)*.5;	//计算起始位置的x中点，即前两个点的x坐标之和的一半Xstarty=(points.points[0].y+points.points[2].y)*.5;	//计算起始位置的y中点，即前两个点的y坐标之和的一半geometry_msgs::Point trans;	//创建一个名为 trans 的 geometry_msgs::Point 类型的变量trans=points.points[4];	//将 points 中的第五个点的坐标赋值给 transstd::vector< std::vector<float>  > V; 	//创建一个名为 V 的二维浮点数向量，用于存储探索算法中的一些坐标std::vector<float> xnew;	//创建一个名为 xnew 的一维浮点数向量xnew.push_back(trans.x);  //将 trans 中的 x 和 y 坐标加入到 xnew 中xnew.push_back(trans.y);V.push_back(xnew);	//将 xnew 添加到 V 中，形成一个新的探索坐标points.points.clear();	//清空 points 中的点，准备用于新的可视化pub.publish(points) ;	//发布清空后的 points，以清空可视化效果std::vector<float> frontiers;	//创建一个名为 frontiers 的一维浮点数向量int i=0;float xr,yr;std::vector<float> x_rand,x_nearest,x_new;	//创建三个一维浮点数向量 x_rand、x_nearest 和 x_new。// Main loop 主循环,在 ROS 节点保持运行状态时不断执行while (ros::ok()){// Sample free	采样自由点x_rand.clear();	//清空 x_rand 向量，用于存储采样得到的随机自由点xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx;	//通过随机采样生成 x 轴坐标yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty;x_rand.push_back( xr ); 	//将生成的 x 和 y 坐标加入到 x_rand 向量中，形成一个随机采样的自由点x_rand.push_back( yr );// Nearest	最近点x_nearest=Nearest(V,x_rand);	//找到 V 中离 x_rand 最近的点，将其存储在 x_nearest 中。// Steer 根据最近点 x_nearest 和随机点 x_rand 以及给定的步长 eta，通过 Steer 函数生成新的点，存储在 x_new 中x_new=Steer(x_nearest,x_rand,eta);// 检查从最近点 x_nearest 到新点 x_new 的路径是否在地图中是自由空间。// ObstacleFree    1:free     -1:unkown (frontier region)      0:obstacle	检查是否为自由空间char checking=ObstacleFree(x_nearest,x_new,mapData);if (checking==-1){	//如果路径为未知区域（前沿区域）exploration_goal.header.stamp=ros::Time(0);	//将 exploration_goal 的时间戳设置为当前时间exploration_goal.header.frame_id=mapData.header.frame_id;	//将 exploration_goal 的坐标系设置为地图的坐标系exploration_goal.point.x=x_new[0];	//将 exploration_goal 的 x 坐标设置为新生成点 x_new 的 x 坐标exploration_goal.point.y=x_new[1];	//将 exploration_goal 的 y 坐标设置为新生成点 x_new 的 y 坐标exploration_goal.point.z=0.0;	//将 exploration_goal 的 z 坐标设置为 0p.x=x_new[0]; 	//创建一个用于可视化的点 p，其坐标与 exploration_goal 相同p.y=x_new[1]; p.z=0.0;points.points.push_back(p);	//将点 p 添加到 points 中，用于可视化pub.publish(points) ;	//发布包含新点的可视化信息targetspub.publish(exploration_goal);	//发布包含新点的探索目标信息points.points.clear();	//清空用于可视化的点，以便下一次循环时重新填充}	  		  else if (checking==1){	//如果路径为自由空间V.push_back(x_new);	//将新生成的点 x_new 添加到 V 向量中，以便记录已经访问过的点p.x=x_new[0]; 	//创建一个用于可视化的点 p，其坐标与 x_new 相同p.y=x_new[1]; p.z=0.0;line.points.push_back(p);	//将点 p 添加到 line 中，以便可视化连接到最近点的线p.x=x_nearest[0]; 	//创建另一个用于可视化的点 p，其坐标与最近点 x_nearest 相同p.y=x_nearest[1]; p.z=0.0;line.points.push_back(p);	//将点 p 添加到 line 中，以便可视化连接到最近点的线}pub.publish(line);  //发布路径可视化信息ros::spinOnce();	//处理回调函数rate.sleep();}return 0;
}

Subscribers callback functions---------------------------------------
这两个回调函数是通过 ros::Subscriber 订阅相应的消息主题，当有新消息到达时，系统将自动调用这两个回调函数

// 2.mapCallBack 处理地图消息
void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{mapData=*msg;	//当接收到 nav_msgs::OccupancyGrid 类型的消息时，将其存储在全局变量 mapData 中
}

// 3.rvizCallBack 处理来自RViz工具的点选消息
void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg)
{ //当接收到 geometry_msgs::PointStamped 类型的消息时，将其中的点坐标提取出来，并将点添加到全局的 points 中，用于后续的处理和可视化geometry_msgs::Point p;  p.x=msg->point.x;p.y=msg->point.y;p.z=msg->point.z;points.points.push_back(p);}

三、local_rrt_detector.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <iostream>
#include <string>
#include <vector>
#include "stdint.h"
#include "functions.h"
#include "mtrand.h"#include "nav_msgs/OccupancyGrid.h"
#include "geometry_msgs/PointStamped.h"
#include "std_msgs/Header.h"
#include "nav_msgs/MapMetaData.h"
#include "geometry_msgs/Point.h"
#include "visualization_msgs/Marker.h"
#include <tf/transform_listener.h>// global variables
nav_msgs::OccupancyGrid mapData;
geometry_msgs::PointStamped clickedpoint;
geometry_msgs::PointStamped exploration_goal;
visualization_msgs::Marker points,line;
float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y;rdm r; // for genrating random numbers

// 1.
int main(int argc, char **argv)
{// ...... 和 global_rrt_detector.cpp 一样std::vector<float> frontiers;int i=0;float xr,yr;std::vector<float> x_rand,x_nearest,x_new;tf::TransformListener listener;	//用于监听并获取Transforms（坐标变换）的信息// Main loopwhile (ros::ok()){// Sample freex_rand.clear();xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx;yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty;x_rand.push_back( xr ); x_rand.push_back( yr );// Nearestx_nearest=Nearest(V,x_rand);// Steerx_new=Steer(x_nearest,x_rand,eta);// ObstacleFree    1:free     -1:unkown (frontier region)      0:obstaclechar   checking=ObstacleFree(x_nearest,x_new,mapData);if (checking==-1){exploration_goal.header.stamp=ros::Time(0);exploration_goal.header.frame_id=mapData.header.frame_id;exploration_goal.point.x=x_new[0];exploration_goal.point.y=x_new[1];exploration_goal.point.z=0.0;p.x=x_new[0]; p.y=x_new[1]; p.z=0.0;points.points.push_back(p);pub.publish(points) ;targetspub.publish(exploration_goal);points.points.clear();V.clear();		//清空存储在 V 中的坐标,清空 V 可以在每次探索开始时重置存储的信息，以便重新记录新的路径或位置tf::StampedTransform transform;	//声明一个 tf::StampedTransform 类型的对象 transform，用于存储变换信息，包括平移和旋转int  temp=0;while (temp==0){	//使用一个循环来尝试获取变换信息，直到成功为止try{temp=1;//使用 TransformListener 的 lookupTransform 方法获取从 base_frame_topic到 map_topic 的变换，并将结果存储在 transform 中listener.lookupTransform(map_topic, base_frame_topic , ros::Time(0), transform);}catch (tf::TransformException ex){temp=0;ros::Duration(0.1).sleep();}}x_new[0]=transform.getOrigin().x();	//从变换中获取机器人的 x 和 y 坐标，并将它们存储在 x_new 中x_new[1]=transform.getOrigin().y();V.push_back(x_new);	//将 x_new 添加到 V 中，可能是用于记录机器人的位置line.points.clear();	//清空 line.points，可能是为了清除之前绘制的路径线，以便重新绘制新的路径}else if (checking==1){V.push_back(x_new);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);}pub.publish(line);  ros::spinOnce();rate.sleep();}return 0;
}

Subscribers callback functions---------------------------------------

// 2.mapCallBack 处理地图消息
void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{mapData=*msg;
}

// 3.rvizCallBack 处理来自RViz工具的点选消息
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);}

四、filter.py

# --------Include modules---------------
from copy import copy
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import PointStamped
import tf
from numpy import array, vstack, delete
from functions import gridValue, informationGain
from sklearn.cluster import MeanShift
from rrt_exploration.msg import PointArray# Subscribers' callbacks------------------------------
mapData = OccupancyGrid()
frontiers = []
globalmaps = []

if __name__ == '__main__':try:node()except rospy.ROSInterruptException:pass

# 1.
def node():# 声明了一些全局变量。这些变量在整个脚本中都可以使用global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count# 初始化ROS节点。这里的节点名为 'filter'，并设置 anonymous 参数为 False，表示节点名不允许自动附加一个唯一标识符。rospy.init_node('filter', anonymous=False)# fetching all parametersmap_topic = rospy.get_param('~map_topic', '/map')threshold = rospy.get_param('~costmap_clearing_threshold', 70)  # 地图中用于清除障碍物的阈值# this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurateinfo_radius = rospy.get_param('~info_radius', 1.0)goals_topic = rospy.get_param('~goals_topic', '/detected_points')n_robots = rospy.get_param('~n_robots', 1)namespace = rospy.get_param('~namespace', '')namespace_init_count = rospy.get_param('namespace_init_count', 1)rateHz = rospy.get_param('~rate', 100)# global_costmap_topic = rospy.get_param(#     '~global_costmap_topic', '/move_base_node/global_costmap/costmap')global_costmap_topic = rospy.get_param('~global_costmap_topic', '/move_base/global_costmap/costmap')robot_frame = rospy.get_param('~robot_frame', 'base_link')litraIndx = len(namespace)rate = rospy.Rate(rateHz)# 创建了一个ROS Subscriber（订阅者），用于接收 map_topic 所指定的地图信息rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)# 这部分代码主要涉及全局地图数据的初始化和订阅# 这段代码的目的是初始化全局地图数据结构并订阅全局地图话题，以便在接收到全局地图消息时执行相应的处理for i in range(0, n_robots):    # 为每个机器人创建一个 OccupancyGrid 类型的对象globalmaps.append(OccupancyGrid())  # 创建一个空的 OccupancyGrid 对象，并将其添加到 globalmaps 列表中if len(namespace) > 0:for i in range(0, n_robots):    # 为每个机器人创建一个 ROS Subscriber# 构建了每个机器人的特定命名空间，然后将其与全局地图话题 global_costmap_topic 进行组合。# 每个机器人的全局地图消息都将传递给 globalMap 函数进行处理rospy.Subscriber(namespace+str(i+namespace_init_count) +    global_costmap_topic, OccupancyGrid, globalMap)elif len(namespace) == 0:# 直接为全局地图话题创建一个 ROS Subscriber，并将消息传递给 globalMap 函数进行处理rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)# wait if map is not received yetwhile (len(mapData.data) < 1):  # mapData.data 存储地图的数据。条件检查确保在接收到有效的地图数据之前一直等待rospy.loginfo('Waiting for the map')rospy.sleep(0.1)pass# wait if any of robots' global costmap map is not received yet# 等待所有机器人的全局代价地图（globalmaps）准备好for i in range(0, n_robots):while (len(globalmaps[i].data) < 1):rospy.loginfo('Waiting for the global costmap')rospy.sleep(0.1)passglobal_frame = "/"+mapData.header.frame_idtfLisn = tf.TransformListener()if len(namespace) > 0:for i in range(0, n_robots):    # 通过waitForTransform等待机器人坐标系和全局坐标系之间的转换关系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:]])pub = rospy.Publisher('frontiers', Marker, queue_size=10)   # 发布前沿点（'frontiers'）pub2 = rospy.Publisher('centroids', Marker, queue_size=10)  # 质心（'centroids'）filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)   # 过滤后的点（'filtered_points'）rospy.loginfo("the map and global costmaps are received")# wait if no frontier is received yet# 检查frontiers列表的长度是否小于1，即是否有前沿点数据接收到。直到有前沿点数据接收到，才会跳出循环while len(frontiers) < 1:passpoints = Marker()   # 创建了两个Marker对象points_clust = Marker()# Set the frame ID and timestamp.  See the TF tutorials for information on these.points.header.frame_id = mapData.header.frame_idpoints.header.stamp = rospy.Time.now()points.ns = "markers2"points.id = 0points.type = Marker.POINTS# Set the marker action for latched frontiers.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)points.action = Marker.ADDpoints.pose.orientation.w = 1.0points.scale.x = 0.2points.scale.y = 0.2points.color.r = 255.0/255.0points.color.g = 255.0/255.0points.color.b = 0.0/255.0points.color.a = 1points.lifetime = rospy.Duration()p = Point()p.z = 0pp = []pl = []points_clust.header.frame_id = mapData.header.frame_idpoints_clust.header.stamp = rospy.Time.now()points_clust.ns = "markers3"points_clust.id = 4points_clust.type = Marker.POINTS# Set the marker action for centroids.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)points_clust.action = Marker.ADDpoints_clust.pose.orientation.w = 1.0points_clust.scale.x = 0.2points_clust.scale.y = 0.2points_clust.color.r = 0.0/255.0points_clust.color.g = 255.0/255.0points_clust.color.b = 0.0/255.0points_clust.color.a = 1points_clust.lifetime = rospy.Duration()temppoint = PointStamped()  # 创建了一个PointStamped消息实例，用于发布单个前沿点的信息temppoint.header.frame_id = mapData.header.frame_idtemppoint.header.stamp = rospy.Time(0)temppoint.point.z = 0.0 # 设置为0.0表示平面上的点arraypoints = PointArray()  # 创建了一个PointArray消息实例，用于发布一组前沿点的信息tempPoint = Point() # 创建了一个Point消息实例，用于存储前沿点的坐标信息tempPoint.z = 0.0# -------------------------------------------------------------------------# ---------------------     Main   Loop     -------------------------------# 这部分代码进行前沿点的聚类while not rospy.is_shutdown():# -------------------------------------------------------------------------# Clustering frontier pointscentroids = []front = copy(frontiers) # 复制前沿点的列表if len(front) > 1:  # 如果前沿点的数量大于1，执行以下聚类步骤ms = MeanShift(bandwidth=0.3)   # 创建MeanShift聚类器，指定带宽为0.3ms.fit(front)   # 使用ms.fit(front)对前沿点进行聚类centroids = ms.cluster_centers_  # 将聚类后的中心点存储在centroids中# # 如果只有一个前沿点，将centroids设置为等于frontif len(front) == 1:centroids = frontfrontiers = copy(centroids) # 将frontiers更新为centroids，以便后续的处理使用聚类后的前沿点# 这部分代码负责清理旧的前沿点# clearing old frontiersz = 0while z < len(centroids):cond = Falsetemppoint.point.x = centroids[z][0] # 将当前聚类中心的坐标设置到temppointtemppoint.point.y = centroids[z][1]for i in range(0, n_robots):    # 对于每个机器人的全局地图，检查聚类中心附近是否存在障碍物（通过cond变量判断）transformedPoint = tfLisn.transformPoint(   # 使用tfLisn.transformPoint将聚类中心的坐标从地图坐标系转换到机器人的全局地图坐标系globalmaps[i].header.frame_id, temppoint)x = array([transformedPoint.point.x, transformedPoint.point.y])cond = (gridValue(globalmaps[i], x) > threshold) or cond# 如果聚类中心附近存在障碍物（cond为True）或者聚类中心的信息增益低于阈值，删除该聚类中心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-1z += 1# 这段代码负责发布前沿点、聚类中心和过滤后的前沿点# publishingarraypoints.points = []for i in centroids:tempPoint.x = i[0]tempPoint.y = i[1]arraypoints.points.append(copy(tempPoint))  # 将清理后的前沿点 centroids 转换为 arraypoints 消息，用于发布filterpub.publish(arraypoints)pp = []for q in range(0, len(frontiers)):p.x = frontiers[q][0]p.y = frontiers[q][1]pp.append(copy(p))  # 将原始的前沿点 frontiers 转换为 points 消息，用于发布points.points = pppp = []for q in range(0, len(centroids)):p.x = centroids[q][0]p.y = centroids[q][1]pp.append(copy(p))  # 将聚类中心 centroids 转换为 points_clust 消息，用于发布points_clust.points = pppub.publish(points)pub2.publish(points_clust)rate.sleep()# -------------------------------------------------------------------------

# 2.用作 ROS Subscriber 接收到消息时的回调函数
def mapCallBack(data):global mapData  # 声明了一个全局变量 mapData，用于存储地图数据mapData = data  # 将接收到的地图消息赋值给全局变量 mapData，以便其他部分的程序可以使用这个地图数据

# 3.用于更新全局地图数据的回调函数,主要作用是将接收到的全局地图数据存储在全局变量 globalmaps 中，以供后续使用
def globalMap(data):global global1, globalmaps, litraIndx, namespace_init_count, n_robotsglobal1 = dataif n_robots > 1:    # # 如果有多个机器人，从连接头部分析机器人索引indx = int(data._connection_header['topic'][litraIndx])-namespace_init_countelif n_robots == 1: # 如果只有一个机器人，索引为0indx = 0globalmaps[indx] = data # 将接收到的全局地图数据存储在全局变量中的相应位置

# 4.用于处理前沿点数据的回调函数,主要作用是将接收到的前沿点数据进行坐标变换，然后存储在全局变量 frontiers 中
def callBack(data, args):global frontiers, min_distancetransformedPoint = args[0].transformPoint(args[1], data)    # 从参数中提取tf.TransformListener和目标坐标系x = [array([transformedPoint.point.x, transformedPoint.point.y])]   # 将变换后的点坐标存储在数组中if len(frontiers) > 0:  # 将坐标数组存储在全局变量frontiers中frontiers = vstack((frontiers, x))else:frontiers = x

五、assigner.py

#--------Include modules---------------
from copy import copy
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from nav_msgs.msg import OccupancyGrid
import tf
from rrt_exploration.msg import PointArray
from time import time
from numpy import array
from numpy import linalg as LA
from numpy import all as All
from numpy import inf
from functions import robot,informationGain,discount
from numpy.linalg import norm# Subscribers' callbacks------------------------------
mapData=OccupancyGrid()
frontiers=[]
global1=OccupancyGrid()
global2=OccupancyGrid()
global3=OccupancyGrid()
globalmaps=[]

if __name__ == '__main__':try:node()except rospy.ROSInterruptException:pass

# Node----------------------------------------------
# 1.
def node():global frontiers,mapData,global1,global2,global3,globalmapsrospy.init_node('assigner', anonymous=False)# fetching all parametersmap_topic= rospy.get_param('~map_topic','/map')info_radius= rospy.get_param('~info_radius',1.0)					#this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurateinfo_multiplier=rospy.get_param('~info_multiplier',3.0)		hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0)			#at least as much as the laser scanner rangehysteresis_gain=rospy.get_param('~hysteresis_gain',2.0)				#bigger than 1 (biase robot to continue exploring current regionfrontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points')	n_robots = rospy.get_param('~n_robots',1)namespace = rospy.get_param('~namespace','')namespace_init_count = rospy.get_param('namespace_init_count',1)delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5)rateHz = rospy.get_param('~rate',100)rate = rospy.Rate(rateHz)#-------------------------------------------rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)rospy.Subscriber(frontiers_topic, PointArray, callBack)#---------------------------------------------------------------------------------------------------------------# wait if no frontier is received yet while len(frontiers)<1:passcentroids=copy(frontiers)	#wait if map is not received yetwhile (len(mapData.data)<1):passrobots=[]if len(namespace)>0:for i in range(0,n_robots):robots.append(robot(namespace+str(i+namespace_init_count)))elif len(namespace)==0:robots.append(robot(namespace))for i in range(0,n_robots):robots[i].sendGoal(robots[i].getPosition())#-------------------------------------------------------------------------#---------------------     Main   Loop     -------------------------------#-------------------------------------------------------------------------while not rospy.is_shutdown():centroids=copy(frontiers)	# 将全局变量 frontiers 复制给局部变量 centroids，以确保在循环执行期间 frontiers 不被修改#Get information gain for each frontier pointinfoGain=[]	# 创建一个空列表，用于存储每个前沿点的信息增益for ip in range(0,len(centroids)):	# 遍历 centroids 中的前沿点，计算每个点的信息增益，并将结果添加到 infoGain 列表中infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius))#-------------------------------------------------------------------------			#get number of available/busy robots	分别创建两个空列表，用于存储可用和繁忙的机器人的索引na=[] #available robotsnb=[] #busy robotsfor i in range(0,n_robots):	# 遍历所有机器人，将其状态为 1 的索引添加到 nb 中，其他情况添加到 na 中。if (robots[i].getState()==1):nb.append(i)else:na.append(i)	rospy.loginfo("available robots: "+str(na))		# 记录可用机器人的信息#get dicount and update informationGain	遍历所有机器人，计算折扣并更新信息增益for i in nb+na:	# 这个循环遍历繁忙（nb）和可用（na）的机器人，并通过 discount 函数计算折扣，然后更新信息增益infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius)# 创建三个空列表          revenue_record=[]centroid_record=[]id_record=[]# 在嵌套循环中，对于每个可用机器人和每个前沿点，计算收入、保存前沿点和机器人的信息for ir in na:for ip in range(0,len(centroids)):cost=norm(robots[ir].getPosition()-centroids[ip])	# 计算机器人与前沿点之间的距离	threshold=1	# 设置阈值（此处为1，但未在后续代码中使用）information_gain=infoGain[ip]	# 获取前沿点的信息增益if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius):information_gain*=hysteresis_gain	# 如果机器人与前沿点之间的距离小于等于 hysteresis_radius，则应用滞后增益revenue=information_gain*info_multiplier-cost	revenue_record.append(revenue)	# 计算收入，将其添加到 revenue_recordcentroid_record.append(centroids[ip])	# 前沿点添加到 centroid_recordid_record.append(ir)	# 机器人 ID 添加到 id_recordif len(na)<1:	# 在没有可用机器人的情况下revenue_record=[]centroid_record=[]id_record=[]for ir in nb:for ip in range(0,len(centroids)):cost=norm(robots[ir].getPosition()-centroids[ip])		threshold=1information_gain=infoGain[ip]if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius):information_gain*=hysteresis_gain# 在计算收入之前，检查前沿点是否在当前机器人的分配点附近（在 hysteresis_radius 内），如果是，则应用 hysteresis_gainif ((norm(centroids[ip]-robots[ir].assigned_point))<hysteresis_radius):information_gain=informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)*hysteresis_gainrevenue=information_gain*info_multiplier-costrevenue_record.append(revenue)centroid_record.append(centroids[ip])id_record.append(ir)rospy.loginfo("revenue record: "+str(revenue_record))	rospy.loginfo("centroid record: "+str(centroid_record))	rospy.loginfo("robot IDs record: "+str(id_record))	# 检查是否存在收入记录，并在有记录时选择收入最大的机器人	if (len(id_record)>0):	# 如果 id_record 列表的长度大于零，表示有机器人被分配了任务winner_id=revenue_record.index(max(revenue_record))	# 找到 revenue_record 中最大值对应的索引，这个索引对应于赢家机器人的索引robots[id_record[winner_id]].sendGoal(centroid_record[winner_id])	# 通过 id_record 找到赢家机器人的 ID，并使用该 ID 发送目标位置# 记录分配的信息到 ROS 日志rospy.loginfo(namespace+str(namespace_init_count+id_record[winner_id])+"  assigned to  "+str(centroid_record[winner_id]))	rospy.sleep(delay_after_assignement)rate.sleep()

# 2.处理接收到的地图数据 OccupancyGrid 数据
# 将其赋值给全局变量 mapData。在ROS中，OccupancyGrid 通常表示地图的占用情况，包含有关环境中障碍物的信息
def mapCallBack(data):global mapDatamapData=data

# 3.处理接收到的前沿点 PointArray 数据
# 将其中的每个点的 x 和 y 坐标提取出来，组成一个数组，并将这些数组存储在 frontiers 全局变量中
def callBack(data):global frontiersfrontiers=[]for point in data.points:frontiers.append(array([point.x,point.y]))