MOVE_AI/src/rm_nav/rm_simple_move/README.md

rm_simple_move

一个用于 ROS2 的导航控制桥接节点，作为决策层和控制层的连接。支持两种控制模式：PID 直接控制和 Navigation 导航。

---

功能特性

双模式控制

• PID 模式 (mode=0): 直接根据当前位置和目标位置进行 PID 控制
• NAV 模式 (mode=1): 将目标转发给 Navigation 系统进行路径规划和导航

统一接口

• 单一输入话题：/nav_goal (包含模式和目标点)
• 标准输出：/cmd_vel (标准 ROS2 速度控制)
• 状态反馈：当前位置和导航状态

---

安装依赖

1. 克隆仓库

git clone https://github.com/goldenfishs/rm_simple_move.git

2. 安装 rm_msgs

git clone https://github.com/goldenfishs/rm_msgs.git

---

话题接口

输入话题

/nav_goal (rm_msgs/NavGoal)

导航目标 (合并了模式和目标点)

• uint8 mode: 控制模式 (0=PID, 1=NAV)
• float32 target_x: 目标 x 坐标
• float32 target_y: 目标 y 坐标
• float32 target_angle: 目标角度 (-π ~ π)
• float32 max_speed: 最大速度 (仅 PID 模式使用)
• float32 tolerance: 到达容差 (仅 PID 模式使用)

输出话题

/cmd_vel_move (geometry_msgs/Twist)

统一的速度控制指令输出（底盘订阅此话题）

• linear.x: x 方向线速度
• linear.y: y 方向线速度
• angular.z: z 轴角速度

/nav_status (rm_msgs/NavStatus)

导航状态

• uint8 status: 状态码 (0=IDLE, 1=NAVIGATING, 2=REACHED, 3=FAILED)
• float32 distance: 距离目标点的距离
• string message: 状态描述

/current_pose (geometry_msgs/PoseStamped)

当前位置 (map 坐标系)

/goal_pose (geometry_msgs/PoseStamped)

导航目标 (NAV 模式下发布，供导航系统使用)

---

参数配置

参数名	类型	默认值	说明
pid_linear_kp	double	0.8	线性 PID 比例系数
pid_linear_ki	double	0.0	线性 PID 积分系数
pid_linear_kd	double	0.1	线性 PID 微分系数
pid_angular_kp	double	1.5	角度 PID 比例系数
pid_angular_ki	double	0.0	角度 PID 积分系数
pid_angular_kd	double	0.2	角度 PID 微分系数

---

使用方法

1. 编译

cd ~/MOVE_AI
colcon build --packages-select rm_msgs rm_simpal_move

2. 启动节点

source install/setup.bash
ros2 launch rm_simpal_move simple_move.launch.py

3. 测试

使用测试脚本

cd src/rm_nav/rm_simple_move/test
python3 move_test_new.py

测试命令：

# PID 模式导航
>>> pid 1.0 2.0 0.0

# PID 模式导航 (带速度和容差)
>>> pid 1.0 2.0 0.0 1.5 0.2

# NAV 模式导航
>>> nav 1.0 2.0 0.0

# 查看状态
>>> status

# 退出
>>> quit

使用命令行

PID 模式：

ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
  mode: 0,
  target_x: 1.0,
  target_y: 2.0,
  target_angle: 0.0,
  max_speed: 2.0,
  tolerance: 0.1
}" --once

NAV 模式：

ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
  mode: 1,
  target_x: 1.0,
  target_y: 2.0,
  target_angle: 0.0,
  max_speed: 0.0,
  tolerance: 0.0
}" --once

查看输出：

# 查看速度控制指令（底盘订阅此话题）
ros2 topic echo /cmd_vel_move

# 查看导航状态
ros2 topic echo /nav_status

# 查看当前位置
ros2 topic echo /current_pose

---

工作原理

PID 模式

1. 接收目标点 /nav_goal (mode=0)
2. 通过 TF 获取当前位置 (map -> base_link)
3. 计算目标点在 base_link 坐标系下的相对位置
4. 使用 PID 控制器计算速度指令
5. 发布到 /cmd_vel_move

NAV 模式

1. 接收目标点 /nav_goal (mode=1)
2. 转换为 geometry_msgs/PoseStamped 格式
3. 发布到 /goal_pose 给导航系统
4. 导航系统输出 /cmd_vel
5. rm_simple_move 订阅 /cmd_vel 并转发到 /cmd_vel_move

注意: 导航系统内部流程为：

/goal_pose → Nav2 → /cmd_vel_nav → velocity_smoother → /cmd_vel

状态发布

• 实时发布当前位置到 /current_pose
• 实时发布导航状态到 /nav_status
• 包含距离、状态码和描述信息

---

架构说明

决策层 (Decision Layer)
    ↓ /nav_goal (mode + target)

rm_simple_move (Bridge)
    ├─ PID 模式: 直接计算 → /cmd_vel_move
    └─ NAV 模式: /goal_pose → Navigation Stack
                                    ↓
                              /cmd_vel_nav → velocity_smoother → /cmd_vel
                                                                      ↓
                                                    rm_simple_move 转发 → /cmd_vel_move

底盘驱动 (Chassis Driver)
    ← /cmd_vel_move (统一的速度指令)

话题流向:

• PID 模式: rm_simple_move 直接计算并发布到 /cmd_vel_move
• NAV 模式: rm_simple_move 发布 /goal_pose → Nav2 处理后输出 /cmd_vel → rm_simple_move 转发到 /cmd_vel_move
• 底盘驱动: 只订阅 /cmd_vel_move，避免冲突

---

Python 示例

import rclpy
from rclpy.node import Node
from rm_msgs.msg import NavGoal, NavStatus

class MyNode(Node):
    def __init__(self):
        super().__init__('my_node')

        # 发布目标
        self.goal_pub = self.create_publisher(NavGoal, '/nav_goal', 10)

        # 订阅状态
        self.status_sub = self.create_subscription(
            NavStatus, '/nav_status', self.status_callback, 10)

    def send_goal_pid(self, x, y, angle):
        """PID 模式"""
        msg = NavGoal()
        msg.mode = 0  # PID
        msg.target_x = x
        msg.target_y = y
        msg.target_angle = angle
        msg.max_speed = 2.0
        msg.tolerance = 0.1
        self.goal_pub.publish(msg)

    def send_goal_nav(self, x, y, angle):
        """NAV 模式"""
        msg = NavGoal()
        msg.mode = 1  # NAV
        msg.target_x = x
        msg.target_y = y
        msg.target_angle = angle
        self.goal_pub.publish(msg)

    def status_callback(self, msg):
        if msg.status == 2:  # REACHED
            self.get_logger().info('目标已到达!')

完整示例见 test/decision_example.py

---

注意事项

1. 坐标系要求: 需要正确配置 map -> base_link 的 TF 变换
2. PID 参数调优: 根据实际机器人调整 PID 参数
3. 模式切换: 每次发送目标时可以切换模式
4. 导航系统: NAV 模式需要配合 Nav2 或其他导航系统使用
5. 标准接口: 输出使用标准 /cmd_vel，兼容所有 ROS2 底盘驱动

---

License

MIT License