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.