fix: 避免 PID 和 NAV 模式的 cmd_vel 冲突

改动:
- 统一输出到 /cmd_vel_move (底盘订阅此话题)
- PID 模式: 直接计算并发布到 /cmd_vel_move
- NAV 模式: 订阅导航的 /cmd_vel 并转发到 /cmd_vel_move
- 避免两种模式同时发布到 /cmd_vel 导致冲突

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

src/rm_nav/rm_simple_move/README.md

@@ -46,8 +46,8 @@ git clone https://github.com/goldenfishs/rm_msgs.git
 
 ### 输出话题
 
-#### `/cmd_vel` (geometry_msgs/Twist)
+#### `/cmd_vel_move` (geometry_msgs/Twist)
-标准速度控制指令
+统一的速度控制指令输出（底盘订阅此话题）
 - `linear.x`: x 方向线速度
 - `linear.y`: y 方向线速度
 - `angular.z`: z 轴角速度
@@ -147,8 +147,8 @@ ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
 
 **查看输出：**
 ```bash
-# 查看速度控制指令
+# 查看速度控制指令（底盘订阅此话题）
-ros2 topic echo /cmd_vel
+ros2 topic echo /cmd_vel_move
 
 # 查看导航状态
 ros2 topic echo /nav_status
@@ -166,13 +166,19 @@ ros2 topic echo /current_pose
 2. 通过 TF 获取当前位置 (map -> base_link)
 3. 计算目标点在 base_link 坐标系下的相对位置
 4. 使用 PID 控制器计算速度指令
-5. 发布标准 `/cmd_vel` 控制指令
+5. 发布到 `/cmd_vel_move`
 
 ### NAV 模式
 1. 接收目标点 `/nav_goal` (mode=1)
 2. 转换为 `geometry_msgs/PoseStamped` 格式
 3. 发布到 `/goal_pose` 给导航系统
-4. 导航系统负责路径规划并发布 `/cmd_vel`
+4. 导航系统输出 `/cmd_vel`
+5. `rm_simple_move` 订阅 `/cmd_vel` 并转发到 `/cmd_vel_move`
+
+**注意**: 导航系统内部流程为：
+```
+/goal_pose → Nav2 → /cmd_vel_nav → velocity_smoother → /cmd_vel
+```
 
 ### 状态发布
 - 实时发布当前位置到 `/current_pose`
@@ -186,13 +192,24 @@ ros2 topic echo /current_pose
 ```
 决策层 (Decision Layer)
     ↓ /nav_goal (mode + target)
+
 rm_simple_move (Bridge)
-    ↓ PID 模式: /cmd_vel (直接控制)
+    ├─ PID 模式: 直接计算 → /cmd_vel_move
-    ↓ NAV 模式: /goal_pose → Navigation → /cmd_vel
+    └─ NAV 模式: /goal_pose → Navigation Stack
-控制层 (Control Layer)
+                                    ↓
-    ← /cmd_vel (标准速度指令)
+                              /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 示例

src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp

@@ -55,6 +55,7 @@ public:
 private:
     void timer_callback();
     void nav_goal_callback(const rm_msgs::msg::NavGoal::SharedPtr msg);
+    void cmd_vel_nav_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
 
     int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const geometry_msgs::msg::Quaternion &quat);
     bool is_goal_reached(float x, float y, float tolerance);
@@ -73,10 +74,11 @@ private:
 
     // Subscribers
     rclcpp::Subscription<rm_msgs::msg::NavGoal>::SharedPtr nav_goal_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_nav_sub_;
 
     // Publishers
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr nav_goal_pub_;
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_move_pub_;
     rclcpp::Publisher<rm_msgs::msg::NavStatus>::SharedPtr nav_status_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr current_pose_pub_;
 

src/rm_nav/rm_simple_move/src/simple_move.cpp

@@ -96,12 +96,24 @@ namespace rm_simpal_move
             std::bind(&RMSimpleMove::nav_goal_callback, this, std::placeholders::_1)
         );
 
+        cmd_vel_nav_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+            "/cmd_vel", 10,
+            std::bind(&RMSimpleMove::cmd_vel_nav_callback, this, std::placeholders::_1)
+        );
+
         // 发布者
         nav_goal_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/goal_pose", 10);
-        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
+        cmd_vel_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_move", 10);
         nav_status_pub_ = this->create_publisher<rm_msgs::msg::NavStatus>("/nav_status", 10);
         current_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/current_pose", 10);
 
+        RCLCPP_INFO(this->get_logger(), "话题配置:");
+        RCLCPP_INFO(this->get_logger(), "  输入: /nav_goal");
+        RCLCPP_INFO(this->get_logger(), "  输出: /cmd_vel_move (统一输出)");
+        RCLCPP_INFO(this->get_logger(), "  PID模式: 直接计算发布到 /cmd_vel_move");
+        RCLCPP_INFO(this->get_logger(), "  NAV模式: /goal_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move");
+        RCLCPP_INFO(this->get_logger(), "  状态: /nav_status, /current_pose");
+
         last_time_ = this->get_clock()->now();
 
         RCLCPP_INFO(this->get_logger(), "RMSimpleMove 启动");
@@ -231,7 +243,7 @@ namespace rm_simpal_move
                 vy *= scale;
             }
 
-            // 发布标准 cmd_vel
+            // 发布标准 cmd_vel_move
             auto cmd_vel_msg = geometry_msgs::msg::Twist();
             cmd_vel_msg.linear.x = vx;
             cmd_vel_msg.linear.y = vy;
@@ -239,7 +251,7 @@ namespace rm_simpal_move
             cmd_vel_msg.angular.x = 0.0;
             cmd_vel_msg.angular.y = 0.0;
             cmd_vel_msg.angular.z = wz;
-            cmd_vel_pub_->publish(cmd_vel_msg);
+            cmd_vel_move_pub_->publish(cmd_vel_msg);
 
         } catch (const tf2::TransformException &ex) {
             RCLCPP_WARN(this->get_logger(), "PID 控制 TF 失败: %s", ex.what());
@@ -341,6 +353,17 @@ namespace rm_simpal_move
                     msg->target_x, msg->target_y, msg->target_angle);
     }
 
+    /**
+     * @brief 导航 cmd_vel 回调 (NAV 模式下转发)
+     */
+    void RMSimpleMove::cmd_vel_nav_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
+    {
+        // 只在 NAV 模式下转发
+        if (control_mode_ == 1) {
+            cmd_vel_move_pub_->publish(*msg);
+        }
+    }
+
     /**
      * @brief 判断是否到达目标
      */