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_pub
    markerArray = MarkerArray() #目标点标记数组
    markerArray_number = MarkerArray() #目标点标记数组
    count = 0
    index = 0
    try_again = 1
    sendflagPublisher = 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=1
    sendflagPublisher.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 = 0
            index = 0
            try_again = 1

            markerArray = MarkerArray() 
            marker = Marker()
            marker.header.frame_id = 'map' 
            marker.type = marker.TEXT_VIEW_FACING 
            marker.action = marker.DELETEALL 
            marker.text = '' 
            markerArray.markers.append(marker) 

            for m in markerArray_number.markers:    
                m.action = marker.DELETEALL

            mark_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 = termios.tcgetattr(fd)
    new_settings[3]=new_settings[3] | termios.ECHO
    termios.tcsetattr(fd, termios.TCSADRAIN, new_settings)

if __name__ == '__main__':
    settings = termios.tcgetattr(sys.stdin) #获取键值初始化
    rospy.on_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, count

    print('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坐标为原点
    marker.type = marker.ARROW #一直面向屏幕的字符格式
    marker.action = marker.ADD #添加marker
    marker.scale.x = 0.2 #marker大小
    marker.scale.y = 0.05 #marker大小
    marker.scale.z = 0.05 #marker大小，对于字符只有z起作用
    marker.color.a = 1 #字符透明度
    marker.color.r = 1 #字符颜色R(红色)通道
    marker.color.g = 0 #字符颜色G(绿色)通道
    marker.color.b = 0 #字符颜色B(蓝色)通道
    marker.pose.position.x = msg.point.x #字符位置
    marker.pose.position.y = msg.point.y #字符位置
    marker.pose.orientation.z = 0 #字符位置
    marker.pose.orientation.w = 1 #字符位置
    markerArray.markers.append(marker) #添加元素进数组

    marker_number = Marker()      #创建marker对象
    marker_number.header.frame_id = 'map' #以哪一个TF坐标为原点
    marker_number.type = marker_number.TEXT_VIEW_FACING #一直面向屏幕的字符格式
    marker_number.action = marker_number.ADD #添加marker
    marker_number.scale.x = 0.5 #marker大小
    marker_number.scale.y = 0.5 #marker大小
    marker_number.scale.z = 0.5 #marker大小，对于字符只有z起作用
    marker_number.color.a = 1 #字符透明度
    marker_number.color.r = 1 #字符颜色R(红色)通道
    marker_number.color.g = 0 #字符颜色G(绿色)通道
    marker_number.color.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_number.pose.orientation.z = 0 #字符位置
    marker_number.pose.orientation.w = 1 #字符位置
    marker_number.text = str(count) #字符内容
    markerArray_number.markers.append(marker_number) #添加元素进数组

    #markers的id不能一样，否则rviz只会识别最后一个元素
    id = 0
    for m in markerArray.markers:    #遍历marker分别给id赋值
        m.id = id
        id += 1

    for m in markerArray_number.markers:    #遍历marker分别给id赋值
        m.id = id
        id += 1
    
    mark_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 = rospy.Time.now()
        pose.pose.position.x = msg.point.x #目标点位置
        pose.pose.position.y = msg.point.y #目标点位置
        pose.pose.orientation.z = 0 #四元数，到达目标点后小车的方向，z=sin(angle/2)
        pose.pose.orientation.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, index
    if 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].pose.orientation.z)+
                ', w:'+str(markerArray.markers[index-1].pose.orientation.w))   

            if count > 1:
                print('Complete instructions!') #只有一个目标点不算巡航
                index = 0
            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index].pose.position.x
            pose.pose.position.y = markerArray.markers[index].pose.position.y
            pose.pose.orientation.z = markerArray.markers[index].pose.orientation.z
            pose.pose.orientation.w = markerArray.markers[index].pose.orientation.w
            goal_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].pose.orientation.z)+
                ', w:'+str(markerArray.markers[index-1].pose.orientation.w)) 

            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index].pose.position.x
            pose.pose.position.y = markerArray.markers[index].pose.position.y
            pose.pose.orientation.z = markerArray.markers[index].pose.orientation.z
            pose.pose.orientation.w = markerArray.markers[index].pose.orientation.w
            goal_pub.publish(pose)
            index += 1 #下一次要发布的目标点序号
        
    elif count > 0: #未抵达设定的目标点    
        rospy.logwarn('Can not reach the target point '+str(index-1)+':'+'\r\n'+
                      'x:'+str(markerArray.markers[index-1].pose.position.x)+
                    ', y:'+str(markerArray.markers[index-1].pose.position.y)+
                    ', z:'+str(markerArray.markers[index-1].pose.orientation.z)+
                    ', w:'+str(markerArray.markers[index-1].pose.orientation.w)) 

        #如果未尝试过前往尚未抵达的目标点，则尝试前往尚未抵达的目标点
        if try_again == 1:
            rospy.logwarn('trying reach the target point '+str(index-1)+' again!'+'\r\n'+
                          'x:'+str(markerArray.markers[index-1].pose.position.x)+
                        ', y:'+str(markerArray.markers[index-1].pose.position.y)+
                        ', z:'+str(markerArray.markers[index-1].pose.orientation.z)+
                        ', w:'+str(markerArray.markers[index-1].pose.orientation.w)) 

            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index - 1].pose.position.x           
            pose.pose.position.y = markerArray.markers[index - 1].pose.position.y
            pose.pose.orientation.z = markerArray.markers[index-1].pose.orientation.z
            pose.pose.orientation.w = markerArray.markers[index-1].pose.orientation.w
            goal_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:'+'\r\n'+
                          'x:'+str(markerArray.markers[index-1].pose.position.x)+
                        ', y:'+str(markerArray.markers[index-1].pose.position.y)+
                        ', z:'+str(markerArray.markers[index-1].pose.orientation.z)+
                        ', w:'+str(markerArray.markers[index-1].pose.orientation.w)) 

            if index==count: index=0 #如果下一个目标点序号为count，说明当前目标点为最后一个目标点，下一个目标点序号应该设置为0
            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index].pose.position.x      
            pose.pose.position.y = markerArray.markers[index].pose.position.y
            pose.pose.orientation.z = markerArray.markers[index].pose.orientation.z
            pose.pose.orientation.w = markerArray.markers[index].pose.orientation.w
            goal_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）