多点导航操作
打开导航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