系统架构
move_base本身不支持一次性发送多个目标点并自动按顺序导航，使用nav_msgs/Path消息类型发布多个路径点，然后让机器人按顺序依次到达每个路径点。
- 发布一个包含多个路径点的Path消息（可选，用于在RVIZ中显示路径）。
- 按顺序将每个路径点作为MoveBaseGoal发送给move_base动作服务器。
- 等待当前目标点完成后，再发送下一个目标点。
[路径规划节点] | 发布v
[/path_waypoints] (nav_msgs/Path)|v
[轨迹跟踪控制器] ---> [动作客户端] ---> [move_base 服务器]| || 订阅 | 发布v v
[机器人状态] [控制命令]
1. 路径点
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import tfclass MultTarget:def __init__(self):rospy.init_node('path_planner')# 创建路径发布者self.path_pub = rospy.Publisher('/target_points', Path, queue_size=10)# 定义路径点 (x, y, theta)self.points = [(-3.0, 1.0, 0.0), # 点1(7.0, 1.5, 1.57), # 点2 (90度)(1.5, 2.5, 3.14), # 点3 (180度)(-7.0, 1.5, -1.57), # 点4 (-90度)(1.0, 0.5, 0.0) # 返回起点]def create_path(self):"""创建包含所有路径点的Path消息"""path = Path()path.header.frame_id = "map"path.header.stamp = rospy.Time.now()for i, (x, y, yaw) in enumerate(self.points):pose = PoseStamped()pose.header.frame_id = "map"pose.header.seq = ipose.header.stamp = rospy.Time.now()pose.pose.position.x = xpose.pose.position.y = y# 将偏航角转换为四元数quat = tf.transformations.quaternion_from_euler(0, 0, yaw)pose.pose.orientation.x = quat[0]pose.pose.orientation.y = quat[1]pose.pose.orientation.z = quat[2]pose.pose.orientation.w = quat[3]path.poses.append(pose)return pathdef run(self):rate = rospy.Rate(1) # 每秒发布一次while not rospy.is_shutdown():path_msg = self.create_path()self.path_pub.publish(pat
)
