ROS小车研究笔记3/11/2023：多点导航及其源码实现

ROS小车研究笔记3/11/2023：多点导航及其源码实现

多点导航操作

打开导航launch文件

roslaunch turn_on_wheeltec_robot navigation.launch
rviz

在rviz里，选择publish point在地图上点击标记目标点。在标记多个目标点后小车会按标记顺序依次在各个目标点中往返

多点导航对于话题MarkerArray。需要在rviz中使markerArray订阅小车发布的path_point话题

多点导航源码分析

在导航节点 turn_on_wheeltec_robot navigation.launch文件中，启动了如下节点用于多点导航

 <!-- MarkerArray功能节点> --><node name='send_mark' pkg="turn_on_wheeltec_robot" type="send_mark.py"></node>

找到turn_on_wheeltec_robot下的send_mark.py。该节点即为多点导航控制节点

1 主方法

def send_mark():global markerArray, markerArray_number, count, index, try_again, mark_pub, goal_pubmarkerArray = MarkerArray() #目标点标记数组markerArray_number = MarkerArray() #目标点标记数组count = 0index = 0try_again = 1sendflagPublisher = rospy.Publisher('/send_flag', Int8, queue_size =1)rospy.init_node('path_point_demo') #初始化节点mark_pub    = rospy.Publisher('/path_point', MarkerArray, queue_size = 100) #用于发布目标点标记navGoal_sub = rospy.Subscriber('/send_mark_goal', PoseStamped, navGoal_callback) #订阅rviz内标记按下的位置click_sub   = rospy.Subscriber('/clicked_point', PointStamped, click_callback) #订阅rviz内标记按下的位置goal_pub    = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size = 1) #用于发布目标点send_flag=Int8()send_flag.data=1sendflagPublisher.publish(send_flag)rospy.sleep(1.)sendflagPublisher.publish(send_flag)rospy.loginfo('a=%d',send_flag.data)print("11111111111111111111111111")goal_status_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, pose_callback) #用于订阅是否到达目标点状态while not rospy.is_shutdown():key = getKey() #获取键值if(key=='c'): #键值为c是清空目标点count = 0index = 0try_again = 1markerArray = MarkerArray() marker = Marker()marker.header.frame_id = 'map' pe = marker.TEXT_VIEW_FACING marker.action = marker. = '' markerArray.markers.append(marker) for m in markerArray_number.markers:    m.action = marker.DELETEALLmark_pub.publish(markerArray) mark_pub.publish(markerArray_number) markerArray = MarkerArray() markerArray_number = MarkerArray() elif (key == 'x03'): #ctrl+c退出break
def breakkey():fd = sys.stdin.fileno()new_settings = attr(fd)new_settings[3]=new_settings[3] | setattr(fd, termios.TCSADRAIN, new_settings)if __name__ == '__main__':settings = attr(sys.stdin) #获取键值初始化_shutdown(breakkey)#退出前执行键值初始化send_mark()

对于创建的变量，markerArray是一个数组用于保存所有目标的。count为目标点总数，index为下一个要前往的目标点，try_again为在前往目标点失败后，在重新选择前往下一个目标点前尝试前往该目标点的次数。

后面定义了几个发布者和订阅者

mark_pub = rospy.Publisher(‘/path_point’, MarkerArray, queue_size = 100) #用于发布目标点标记
navGoal_sub = rospy.Subscriber(‘/send_mark_goal’, PoseStamped, navGoal_callback) #订阅rviz内标记按下的位置
click_sub = rospy.Subscriber(‘/clicked_point’, PointStamped, click_callback) #订阅rviz内标记按下的位置
goal_pub = rospy.Publisher(‘/move_base_simple/goal’, PoseStamped, queue_size = 1) #用于发布目标点
send_flag=Int8()

这里我们得到rviz里发布的clicked_point和send_mark_goal代表得到的目标点。并发布小车的目标点

click_sub的回调函数click_callback

#rviz内标记按下的回调函数，输入参数：按下的位置[x, y, z=0]
def click_callback(msg):           global index, countprint('Add a new target point '+str(count)+':')print('x:'+str(msg.point.x)+', y:'+str(msg.point.y)+', z:0'+', w:1') marker = Marker()      #创建marker对象marker.header.frame_id = 'map' #以哪一个TF坐标为原点pe = marker.ARROW #一直面向屏幕的字符格式marker.action = marker.ADD #添加markermarker.scale.x = 0.2 #marker大小marker.scale.y = 0.05 #marker大小marker.scale.z = 0.05 #marker大小，对于字符只有z起作用lor.a = 1 #字符透明度lor.r = 1 #字符颜色R(红色)通道lor.g = 0 #字符颜色G(绿色)通道lor.b = 0 #字符颜色B(蓝色)通道marker.pose.position.x = msg.point.x #字符位置marker.pose.position.y = msg.point.y #字符位置ientation.z = 0 #字符位置ientation.w = 1 #字符位置markerArray.markers.append(marker) #添加元素进数组marker_number = Marker()      #创建marker对象marker_number.header.frame_id = 'map' #以哪一个TF坐标为原点pe = marker_number.TEXT_VIEW_FACING #一直面向屏幕的字符格式marker_number.action = marker_number.ADD #添加markermarker_number.scale.x = 0.5 #marker大小marker_number.scale.y = 0.5 #marker大小marker_number.scale.z = 0.5 #marker大小，对于字符只有z起作用lor.a = 1 #字符透明度lor.r = 1 #字符颜色R(红色)通道lor.g = 0 #字符颜色G(绿色)通道lor.b = 0 #字符颜色B(蓝色)通道marker_number.pose.position.x = msg.point.x #字符位置marker_number.pose.position.y = msg.point.y #字符位置marker_number.pose.position.z = 0.1 #字符位置marker_ientation.z = 0 #字符位置marker_ientation.w = 1 #字符位置 = str(count) #字符内容markerArray_number.markers.append(marker_number) #添加元素进数组#markers的id不能一样，否则rviz只会识别最后一个元素id = 0for m in markerArray.markers:    #遍历marker分别给id赋值m.id = idid += 1for m in markerArray_number.markers:    #遍历marker分别给id赋值m.id = idid += 1mark_pub.publish(markerArray) #发布markerArray，rviz订阅并进行可视化mark_pub.publish(markerArray_number) #发布markerArray，rviz订阅并进行可视化#第一次添加marker时直接发布目标点if count == 0:pose = PoseStamped() #创建目标点对象pose.header.frame_id = 'map' #以哪一个TF坐标为原点pose.header.stamp = w()pose.pose.position.x = msg.point.x #目标点位置pose.pose.position.y = msg.point.y #目标点位置ientation.z = 0 #四元数，到达目标点后小车的方向，z=sin(angle/2)ientation.w = 1 #四元数，到达目标点后小车的方向，w=cos(angle/2)goal_pub.publish(pose)index += 1 #下一次要发布的目标点序号count += 1 #有几个目标点

click_callback实现了如下功能
1 在终端打出得到的目标点位置和编号

2 创建Marker对象（rviz上的标记点），并设置标记面向屏幕，大小，透明度，颜色，位置，方向

3 设置marker的id为标记的顺序值。默认情况下marker id都为0，这会导致当有多个id相同的marker时rviz只显示最后一个

4 发布markerArray，由rviz订阅以进行可视化

5 在第一次添加marker时会发布目标点消息，类型为PoseStamped。在之后会让count加一

疑点：这里我们创建了marker和marker_number，并对这两个对象进行了完全一样的操作，设置这一个完全相同的marker_number的目的是什么

navGoal_sub的回调函数pose_callback

#到达目标点成功或失败的回调函数，输入参数：[3：成功， 其它：失败](4：ABORTED)
def pose_callback(msg):global try_again, index, try_again, indexif msg.status.status == 3 and count>0 :  #成功到达任意目标点，前往下一目标点try_again = 1 #允许再次尝试前往尚未抵达的该目标点#count表示当前目标点计数，index表示已完成的目标点计数if index == count:                   #当index等于count时，表示所有目标点完成，重新开始巡航print ('Reach the target point '+str(index-1)+':')print('x:'+str(markerArray.markers[index-1].pose.position.x)+', y:'+str(markerArray.markers[index-1].pose.position.y)+', z:'+str(markerArray.markers[index-1].ientation.z)+', w:'+str(markerArray.markers[index-1].ientation.w))   if count > 1:print('Complete instructions!') #只有一个目标点不算巡航index = 0pose = PoseStamped()pose.header.frame_id = 'map'pose.header.stamp = w()pose.pose.position.x = markerArray.markers[index].pose.position.xpose.pose.position.y = markerArray.markers[index].pose.position.ientation.z = markerArray.markers[index].ientation.ientation.w = markerArray.markers[index].ientation.wgoal_pub.publish(pose)index += 1 #下一次要发布的目标点序号elif index < count:                   #当index小于count时，表示还未完成所有目标点，目标巡航未完成print ('Reach the target point '+str(index-1)+':')    print('x:'+str(markerArray.markers[index-1].pose.position.x)+', y:'+str(markerArray.markers[index-1].pose.position.y)+', z:'+str(markerArray.markers[index-1].ientation.z)+', w:'+str(markerArray.markers[index-1].ientation.w)) pose = PoseStamped()pose.header.frame_id = 'map'pose.header.stamp = w()pose.pose.position.x = markerArray.markers[index].pose.position.xpose.pose.position.y = markerArray.markers[index].pose.position.ientation.z = markerArray.markers[index].ientation.ientation.w = markerArray.markers[index].ientation.wgoal_pub.publish(pose)index += 1 #下一次要发布的目标点序号elif count > 0: #未抵达设定的目标点    rospy.logwarn('Can not reach the target point '+str(index-1)+':'+'rn'+'x:'+str(markerArray.markers[index-1].pose.position.x)+', y:'+str(markerArray.markers[index-1].pose.position.y)+', z:'+str(markerArray.markers[index-1].ientation.z)+', w:'+str(markerArray.markers[index-1].ientation.w)) #如果未尝试过前往尚未抵达的目标点，则尝试前往尚未抵达的目标点if try_again == 1:rospy.logwarn('trying reach the target point '+str(index-1)+' again!'+'rn'+'x:'+str(markerArray.markers[index-1].pose.position.x)+', y:'+str(markerArray.markers[index-1].pose.position.y)+', z:'+str(markerArray.markers[index-1].ientation.z)+', w:'+str(markerArray.markers[index-1].ientation.w)) pose = PoseStamped()pose.header.frame_id = 'map'pose.header.stamp = w()pose.pose.position.x = markerArray.markers[index - 1].pose.position.x           pose.pose.position.y = markerArray.markers[index - 1].pose.position.ientation.z = markerArray.markers[index-1].ientation.ientation.w = markerArray.markers[index-1].ientation.wgoal_pub.publish(pose)try_again = 0 #不允许再次尝试前往尚未抵达的该目标点#如果已经尝试过前往尚未抵达的目标点，则前往下一个目标点elif index < len(markerArray.markers):      #若还未完成目标点rospy.logwarn('try reach the target point '+str(index-1)+' failed! reach next point:'+'rn'+'x:'+str(markerArray.markers[index-1].pose.position.x)+', y:'+str(markerArray.markers[index-1].pose.position.y)+', z:'+str(markerArray.markers[index-1].ientation.z)+', w:'+str(markerArray.markers[index-1].ientation.w)) if index==count: index=0 #如果下一个目标点序号为count，说明当前目标点为最后一个目标点，下一个目标点序号应该设置为0pose = PoseStamped()pose.header.frame_id = 'map'pose.header.stamp = w()pose.pose.position.x = markerArray.markers[index].pose.position.x      pose.pose.position.y = markerArray.markers[index].pose.position.ientation.z = markerArray.markers[index].ientation.ientation.w = markerArray.markers[index].ientation.wgoal_pub.publish(pose)index += 1 #下一次要发布的目标点序号try_again = 1 #允许再次尝试前往尚未抵达的该目标点

函数内部执行的操作均为封装PoseStamped话题并通过goal_pub发布给小车运动控制节点进行执行。这里我们主要看该函数的导航逻辑

1

if msg.status.status == 3 and count>0 : #成功到达任意目标点，前往下一目标点

在我们受到的参数msg中，status为3代表成功到达目标点，4（或者任何其他值）代表失败。判断count > 0是为了确定小车处于多点导航模式。如果count为0代表小车只是执行了一次自主导航，而没有开启多点导航，故不执行后面程序

2

if index == count: #当index等于count时，表示所有目标点完成，重新开始巡航
print (‘Reach the target point ‘+str(index-1)+’:’)

index代表下一个目标点，count代表目标点总数。因此当两者相等时代表本轮巡航彻底完成，开始新一轮巡航。

这时我们将index重设为0，并发布话题让小车回到0点

3

elif index < count: #当index小于count时，表示还未完成所有目标点，目标巡航未完成

index小于count时代表还没有完成所有目标点，此时发布话题指挥小车前往下一个目标点并使index + 1。这里注意区分index - 1代表了当前小车所在目标点，而index 代表小车要去的下一个目标点

4

elif count > 0: #未抵达设定的目标点

此时代表小车处于多点导航状态，但是没有收到成功信息3，代表小车没有成功到达目标点。这时如果try_again为1，代表小车有一次再次尝试机会，控制小车再次前往目标点（前往当前目标点index - 1）。如果尝试后依然失败，则让小车前往下一个目标点（index）