实现ros多点导航的Python代码

以下是一个简单的Python代码示例，用于在ROS中实现多点导航：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib import SimpleActionClient
from tf.transformations import quaternion_from_euler

def movebase_client(x, y, yaw):
    client = SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()

    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    q = quaternion_from_euler(0, 0, yaw)
    goal.target_pose.pose.orientation.x = q[0]
    goal.target_pose.pose.orientation.y = q[1]
    goal.target_pose.pose.orientation.z = q[2]
    goal.target_pose.pose.orientation.w = q[3]

    client.send_goal(goal)
    wait = client.wait_for_result()

    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        return client.get_result()

if __name__ == '__main__':
    try:
        rospy.init_node('multi_point_navigation')
        # 设置导航点列表
        points = [(1.0, 2.0, 0.0), (3.0, 4.0, 1.57), (5.0, 6.0, 3.14)]
        for point in points:
            result = movebase_client(point[0], point[1], point[2])
            if result:
                rospy.loginfo("Goal execution done!")
            else:
                rospy.loginfo("Goal execution failed!")
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation interrupted!")

上述代码使用move_base的SimpleActionClient来实现多点导航。在`movebase_client`函数中，设置了每个导航点的位置和姿态，并将目标发送给move_base节点。然后，使用`client.wait_for_result()`等待move_base节点返回结果。

在`main`函数中，我们定义了一个包含多个导航点的列表，并用循环逐个调用`movebase_client`函数。最后，我们使用`rospy.spin()`保持节点处于活动状态，并处理ROS回调函数。

注意：在运行此代码之前，确保已正确配置导航栈和move_base节点，并运行ROS导航栈。