rrt_exploration代码解析(一)—— global_rrt_detector.cpp
rrt_exploration代码解析(二)—— local_rrt_detector.cpp
rrt_exploration代码解析(三)—— filter.py
rrt_exploration代码解析(四)—— assigner.py
assigner.py
前面三篇博客提到的文件都是在获取地图数据后对数据进行处理获取到的当前地图的吧边界点,都没有设计robot的运动控制部分,而assigner.py的主要功能就是将之前处理好的边界点信息通过一定的评价标准设置优先级,并将边界点作为movebase的目标点发布,安排多个robot对未知地图进行探索。
回调函数获取边界点和地图数据。
def callBack(data):
global frontiers
frontiers=[]
for point in data.points:
frontiers.append(array([point.x,point.y]))
def mapCallBack(data):
global mapData
mapData=data
主节点node()用于分配各个robot需要探索的边界点,首先进行参数初始化。
global frontiers,mapData,global1,global2,global3,globalmaps
rospy.init_node('assigner', anonymous=False)
# fetching all parameters
map_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 accurate
info_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 range
hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region
frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points')
n_robots = rospy.get_param('~n_robots',1)
namespace = rospy.get_param('~n