ros多点导航的Python代码

以下是一个简单的Python脚本，演示如何在ROS中使用多点导航：

#!/usr/bin/env python

import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib

# 定义目标点坐标
goal_positions = [
    [(0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
    [(1.0, 1.0, 0.0), (0.0, 0.0, 0.707, 0.707)],
    [(2.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)]
]

def move_to_goal(x, y, theta):
    # 创建MoveBaseAction客户端
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()

    # 创建MoveBaseGoal对象
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.orientation.z = theta[2]
    goal.target_pose.pose.orientation.w = theta[3]

    # 发送目标点
    client.send_goal(goal)
    client.wait_for_result()

if __name__ == '__main__':
    rospy.init_node('multi_point_navigation')

    # 依次移动到目标点
    for i, goal in enumerate(goal_positions):
        rospy.loginfo("Moving to Goal {}".format(i+1))
        move_to_goal(*goal[0], goal[1])

上述代码实现了如下功能：

1. 定义了三个目标点，每个目标点包括位置和姿态信息。
2. 创建了MoveBaseAction客户端，并等待服务器启动。
3. 依次移动到每个目标点，并在到达后等待服务器响应。

注意，以上代码中需要使用move_base节点提供的MoveBaseAction接口，因此需要确保move_base节点已经启动。例如，可以使用以下命令启动move_base节点：

roslaunch turtlebot3_navigation turtlebot3_navigation.launch

另外，还需要将上述代码保存为Python文件，并确保文件具有可执行权限。例如，可以使用以下命令将文件保存为multi_point_nav.py，并赋予可执行权限：

chmod +x multi_point_nav.py

最后，运行该脚本即可开始多点导航：

rosrun your_package_name multi_point_nav.py