rrt_exploration代码解析（四）—— assigner.py

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('~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)

        等待边界点数据和地图数据，同样地图数据涉及多个robot。

# wait if no frontier is received yet 
	while len(frontiers)<1:
		pass
	centroids=copy(frontiers)	
#wait if map is not received yet
	while (len(mapData.data)<1):
		pass

	robots=[]
	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())

        这里实例化了robot对象，robot对象定义在function.py文件中。robot对象包含的方法有，获取当前的位置、获取当前的状态、设置movebase目标、取消movebase目标、规划路径。

class robot:
    goal = MoveBaseGoal()
    start = PoseStamped()
    end = PoseStamped()

    def __init__(self, name):
        self.assigned_point = []
        self.name = name
        self.global_frame = rospy.get_param('~global_frame', '/map')
        self.robot_frame = rospy.get_param('~robot_frame', 'base_link')
        self.plan_service = rospy.get_param(
            '~plan_service', '/move_base_node/NavfnROS/make_plan')
        self.listener = tf.TransformListener()
        self.listener.waitForTransform(
            # self.global_frame, self.name+'/'+self.robot_frame, rospy.Time(0), rospy.Duration(10.0))
            self.global_frame,self.robot_frame, rospy.Time(0), rospy.Duration(10.0))
        cond = 0
        while cond == 0:
            try:
                rospy.loginfo('Waiting for the robot transform')
                (trans, rot) = self.listener.lookupTransform(
                    self.global_frame, self.robot_frame, rospy.Time(0))
                    # self.global_frame, self.name+'/'+self.robot_frame, rospy.Time(0))
                cond = 1
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                cond == 0
        self.position = array([trans[0], trans[1]])
        self.assigned_point = self.position
        self.client = actionlib.SimpleActionClient(
            '/move_base', MoveBaseAction)
            # self.name+'/move_base', MoveBaseAction)
        self.client.wait_for_server()
        robot.goal.target_pose.header.frame_id = self.global_frame
        robot.goal.target_pose.header.stamp = rospy.Time.now()

        rospy.wait_for_service(self.plan_service)
        # rospy.wait_for_service(self.name+self.plan_service)
        self.make_plan = rospy.ServiceProxy(
            self.plan_service, GetPlan)
            # self.name+self.plan_service, GetPlan)
        robot.start.header.frame_id = self.global_frame
        robot.end.header.frame_id = self.global_frame
    
    def getPosition(self):
        cond = 0
        while cond == 0:
            try:
                (trans, rot) = self.listener.lookupTransform(
                    self.global_frame, self.robot_frame, rospy.Time(0))
                    # self.global_frame, self.name+'/'+self.robot_frame, rospy.Time(0))
                cond = 1
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                cond == 0
        self.position = array([trans[0], trans[1]])
        return self.position

    def sendGoal(self, point):
        robot.goal.target_pose.pose.position.x = point[0]
        robot.goal.target_pose.pose.position.y = point[1]
        robot.goal.target_pose.pose.orientation.w = 1.0
        self.client.send_goal(robot.goal)
        self.assigned_point = array(point)

    def cancelGoal(self):
        self.client.cancel_goal()
        self.assigned_point = self.getPosition()

    def getState(self):
        return self.client.get_state()

    def makePlan(self, start, end):
        robot.start.pose.position.x = start[0]
        robot.start.pose.position.y = start[1]
        robot.end.pose.position.x = end[0]
        robot.end.pose.position.y = end[1]
        start = self.listener.transformPose('/map', robot.start)
        end = self.listener.transformPose('/map', robot.end)
        # start = self.listener.transformPose(self.name+'/map', robot.start)
        # end = self.listener.transformPose(self.name+'/map', robot.end)
        plan = self.make_plan(start=start, goal=end, tolerance=0.0)
        return plan.plan.poses

        节点的主循环，判断边界点的优先级。首先获取各个边界点的信息熵，这里利用了informationGain（）函数，该函数定义在function.py文件中。计算出信息熵用于选择目标点。

def informationGain(mapData, point, r):
    infoGain = 0
    # 获取对应点在地图上的索引
    index = index_of_point(mapData, point)
    # 计算给定的半径包含多少个栅格
    r_region = int(r/mapData.info.resolution)
    init_index = index-r_region*(mapData.info.width+1)
    for n in range(0, 2*r_region+1):
        start = n*mapData.info.width+init_index
        end = start+2*r_region
        limit = ((start/mapData.info.width)+2)*mapData.info.width
        for i in range(start, end+1):
            if (i >= 0 and i < limit and i < len(mapData.data)):
                if(mapData.data[i] == -1 and norm(array(point)-point_of_index(mapData, i)) <= r):
                    infoGain += 1
    return infoGain*(mapData.info.resolution**2)

        对于多个的robot，判断各个robot的状态（available or busy）。根据各个机器人当前的位置与边界点集更新信息熵。

        na=[] #available robots
		nb=[] #busy robots
		for i in range(0,n_robots):
			if (robots[i].getState()==1):
				nb.append(i)
			else:
				na.append(i)	
		rospy.loginfo("available robots: "+str(na))	

        for i in nb+na:
            infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius)

   对于多个available的robot情况，计算各个robot到centroids的奖励revenue=information_gain*info_multiplier-cost。这里的代价cost是机器人位置到边界点的欧氏距离，info_multiplier是用户设置的参数，information_gain*=hysteresis_gain，information_gain是信息熵，hysteresis_gain是用户定义的增益（该参数值大于1），这样设置使得当机器人在半径hysteresis_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
				information_gain=infoGain[ip]
				if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius):

					information_gain*=hysteresis_gain
				revenue=information_gain*info_multiplier-cost
				revenue_record.append(revenue)
				centroid_record.append(centroids[ip])
				id_record.append(ir)

        若所有的robot都处于busy状态，则在available状态的基础上加入了当robot部署位置在以边界点位置中心hysteresis_radius为半径的区域内时，信息熵为informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)*hysteresis_gain。

        if 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=1
					information_gain=infoGain[ip]
					if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius):
						information_gain*=hysteresis_gain
				
					if ((norm(centroids[ip]-robots[ir].assigned_point))<hysteresis_radius):
						information_gain=informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)*hysteresis_gain

					revenue=information_gain*info_multiplier-cost
					revenue_record.append(revenue)
					centroid_record.append(centroids[ip])
					id_record.append(ir)

        将奖励存入revenue_record中，将机器人的ID存入id_record中，选择奖励最大的边界点，ID记为winner_id，并给将其表示的边界点分配给对应的robots[id_record[winner_id]]，利用robot对象的sendGoal方法将边界点设置为movebase的目标点，通过在launch文件中启动movebase即可控制robot往设定好的目标点探索。

        对rrt_exploration重要的源码的分析就完成了，仿真过程中使用的launch文件基本就是启动这四个文件对应的节点和movebase、gmapping、rviz等节点。相对来说比较容易理解。

        有问题欢迎交流，祝好。