ROS actionlib 发布MoveBaseAction消息实现多点导航，从csv文件读取位姿列表，比用PoseStamped清爽 python实现

在这个程序上改的 https://github.com/cristian-frincu/navigation_waypoints_scripts

上代码 

#!/usr/bin/env python

import rospy
import actionlib

#move_base_msgs
from move_base_msgs.msg import *
from geometry_msgs.msg import *

# form https://github.com/cristian-frincu/navigation_waypoints_scripts

def simple_move(x,y,w,z):


    sac = actionlib.SimpleActionClient('move_base', MoveBaseAction )

    #create goal
    goal = MoveBaseGoal()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.orientation.w = w
    goal.target_pose.pose.orientation.z = z
    goal.target_pose.header.frame_id = 'map'
    goal.target_pose.header.stamp = rospy.Time.now()

    #start listner
    sac.wait_for_server()
    #send goal
    sac.send_goal(goal)
    print ("Sending goal:",x,y,w,z)
    #finish
    sac.wait_for_result()
    #print result
    print (sac.get_result())

def talker(coordinates):
    #publishing as PoseArray, as a reference for user
    f = open('file.csv','r')
    array = PoseArray()
    array.header.frame_id = 'map'
    array.header.stamp = rospy.Time.now()

    for goal in f:
        coordinates = goal.split(",")
        pose = Pose()
        pose.position.x = float(coordinates[0])
        pose.position.y = float(coordinates[1])
        pose.orientation.w = float(coordinates[2])
        pose.orientation.z = float(coordinates[3])
        array.poses.append(pose)

        pub = rospy.Publisher('simpleNavPoses', PoseArray, queue_size=100)
        rate = rospy.Rate(1) # 1hz

        #To not have to deal with threading, Im gonna publish just a couple times in the begging, and then continue with telling the robot to go to the points
    count = 0
    while count<1:
        rate.sleep()	
        pub.publish(array)
        count +=1


if __name__ == '__main__':
    try:
        rospy.init_node('simple_move')

        #actually sending commands for the robot to travel
        f = open('file.csv','r')
        for goal in f:
            coordinates = goal.split(",")
            simple_move(float(coordinates[0]),float(coordinates[1]),float(coordinates[2]),float(coordinates[3]))
            talker(coordinates)
            rospy.loginfo("goal reached")
            rospy.sleep(10)
    except rospy.ROSInterruptException:
        print ("Keyboard Interrupt")

csv 文件内容

12.731,-9.136,1.000, 0.004
9.915,-8.788,-0.715, 0.699
9.878,-9.889,-0.018, 1.000
12.484,-9.954,0.670, 0.742

 注意 1、有两处需要修改文件路径       2、csv文件中前两个是位置x y，后两个是四元数表示的方向的w 和 z  顺序不是z w ，关于如何把角度转换成四元数参考ROS 发布PoseWithCovarianceStamped 消息重新初始化位姿python-CSDN博客 中的代码