fix: NAV 模式在收到目标时立即发送导航请求

问题: TF 查询失败导致 timer_callback 提前退出，publish_nav_goal() 未执行

修复: 将 NAV 模式的目标发送移到 nav_goal_callback 中
- 收到目标后立即发送 Action 请求，不依赖 TF
- 避免 TF 时间戳问题影响导航功能
- 测试成功：导航到 (4, -4) 完成

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

src/rm_nav/rm_simple_move/CMakeLists.txt

@@ -9,10 +9,12 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
 find_package(rm_msgs REQUIRED)
 
 # include directories
@@ -22,7 +24,7 @@ include_directories(include)
 add_library(rm_simpal_move SHARED src/simple_move.cpp)
 
 # link libraries
-ament_target_dependencies(rm_simpal_move rclcpp rclcpp_components tf2_ros geometry_msgs tf2_geometry_msgs nav_msgs rm_msgs)
+ament_target_dependencies(rm_simpal_move rclcpp rclcpp_components rclcpp_action tf2_ros geometry_msgs tf2_geometry_msgs nav_msgs nav2_msgs rm_msgs)
 
 # register node
 rclcpp_components_register_nodes(rm_simpal_move "rm_simpal_move::RMSimpleMove")

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
@@ -9,6 +10,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <rm_msgs/msg/nav_goal.hpp>
 #include <rm_msgs/msg/nav_status.hpp>
 #include <atomic>
@@ -49,12 +51,16 @@ struct PIDController {
 
 class RMSimpleMove : public rclcpp::Node {
 public:
+    using NavigateToPose = nav2_msgs::action::NavigateToPose;
+    using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
+
     RMSimpleMove(const rclcpp::NodeOptions &options);
     ~RMSimpleMove() override;
 
 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,18 +79,22 @@ 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_move_pub_;
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
     rclcpp::Publisher<rm_msgs::msg::NavStatus>::SharedPtr nav_status_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr current_pose_pub_;
 
+    // Action Client
+    rclcpp_action::Client<NavigateToPose>::SharedPtr nav_action_client_;
+
     // State
     std::atomic<uint8_t> control_mode_;  // 0: PID, 1: NAV
     std::atomic<bool> running_;
     std::atomic<bool> goal_reached_;
     std::atomic<bool> has_goal_;
+    std::atomic<bool> nav_goal_sent_;  // NAV 模式下目标是否已发送
 
     AHRS_Eulr_t current_eulr_;
     Goal_t goal_pose_;

src/rm_nav/rm_simple_move/package.xml

@@ -10,10 +10,12 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>rclcpp_action</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>nav2_msgs</depend>
   <depend>launch</depend>
   <depend>launch_ros</depend>
   <depend>rm_msgs</depend>

src/rm_nav/rm_simple_move/src/simple_move.cpp

@@ -62,6 +62,7 @@ namespace rm_simpal_move
           running_(true),
           goal_reached_(false),
           has_goal_(false),
+          nav_goal_sent_(false),
           pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f),
           pid_linear_y_(0.8f, 0.0f, 0.1f, 3.0f),
           pid_angular_(1.5f, 0.0f, 0.2f, 2.0f)
@@ -96,12 +97,26 @@ 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_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_move", 10);
-        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 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);
 
+        // Action Client
+        nav_action_client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
+
+        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模式: Action /navigate_to_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 启动");
@@ -120,14 +135,10 @@ namespace rm_simpal_move
      */
     void RMSimpleMove::timer_callback()
     {
-        if (!has_goal_) {
-            return;
-        }
-
         try
         {
-            // 获取当前位置
+            // 获取当前位置（始终发布，不管有没有目标）
-            auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
+            auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero, tf2::durationFromSec(0.5));
             AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
 
             // 发布当前位置
@@ -140,6 +151,11 @@ namespace rm_simpal_move
             current_pose_msg.pose.orientation = trans.transform.rotation;
             current_pose_pub_->publish(current_pose_msg);
 
+            // 如果没有目标，只发布当前位置，不执行控制
+            if (!has_goal_) {
+                return;
+            }
+
             // 发布目标点 TF
             geometry_msgs::msg::TransformStamped goal_transform;
             goal_transform.header.stamp = this->get_clock()->now();
@@ -160,7 +176,7 @@ namespace rm_simpal_move
 
             // 计算目标点在 base_link 坐标系下的位置
             geometry_msgs::msg::TransformStamped goal_in_base_link;
-            goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);
+            goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero, tf2::durationFromSec(0.5));
 
             float goal_x_in_base_link = goal_in_base_link.transform.translation.x;
             float goal_y_in_base_link = goal_in_base_link.transform.translation.y;
@@ -180,8 +196,11 @@ namespace rm_simpal_move
                 // PID 模式
                 publish_pid_control();
             } else {
-                // NAV 模式
+                // NAV 模式：只在第一次发送目标
-                publish_nav_goal();
+                if (!nav_goal_sent_) {
+                    publish_nav_goal();
+                    nav_goal_sent_ = true;
+                }
             }
 
             // 发布状态
@@ -189,7 +208,7 @@ namespace rm_simpal_move
         }
         catch (const tf2::TransformException &ex)
         {
-            RCLCPP_WARN(this->get_logger(), "TF 变换失败: %s", ex.what());
+            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "TF 变换失败: %s", ex.what());
         }
     }
 
@@ -231,7 +250,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 +258,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());
@@ -247,25 +266,64 @@ namespace rm_simpal_move
     }
 
     /**
-     * @brief 发布导航目标
+     * @brief 发布导航目标 (使用 Action)
      */
     void RMSimpleMove::publish_nav_goal()
     {
-        auto goal_msg = geometry_msgs::msg::PoseStamped();
+        // 检查 Action Server 是否可用
-        goal_msg.header.stamp = this->get_clock()->now();
+        if (!nav_action_client_->wait_for_action_server(std::chrono::seconds(1))) {
-        goal_msg.header.frame_id = "map";
+            RCLCPP_WARN(this->get_logger(), "导航 Action Server 不可用");
-        goal_msg.pose.position.x = goal_pose_.x;
+            return;
-        goal_msg.pose.position.y = goal_pose_.y;
+        }
-        goal_msg.pose.position.z = 0.0;
+
+        // 创建 Action Goal
+        auto goal_msg = NavigateToPose::Goal();
+        goal_msg.pose.header.stamp = this->get_clock()->now();
+        goal_msg.pose.header.frame_id = "map";
+        goal_msg.pose.pose.position.x = goal_pose_.x;
+        goal_msg.pose.pose.position.y = goal_pose_.y;
+        goal_msg.pose.pose.position.z = 0.0;
 
         tf2::Quaternion q;
         q.setRPY(0, 0, goal_pose_.angle);
-        goal_msg.pose.orientation.x = q.x();
+        goal_msg.pose.pose.orientation.x = q.x();
-        goal_msg.pose.orientation.y = q.y();
+        goal_msg.pose.pose.orientation.y = q.y();
-        goal_msg.pose.orientation.z = q.z();
+        goal_msg.pose.pose.orientation.z = q.z();
-        goal_msg.pose.orientation.w = q.w();
+        goal_msg.pose.pose.orientation.w = q.w();
 
-        nav_goal_pub_->publish(goal_msg);
+        // 发送 Action Goal
+        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
+
+        send_goal_options.goal_response_callback =
+            [this](GoalHandleNavigateToPose::SharedPtr goal_handle) {
+                if (!goal_handle) {
+                    RCLCPP_ERROR(this->get_logger(), "导航目标被拒绝");
+                } else {
+                    RCLCPP_INFO(this->get_logger(), "导航目标已接受");
+                }
+            };
+
+        send_goal_options.result_callback =
+            [this](const GoalHandleNavigateToPose::WrappedResult & result) {
+                switch (result.code) {
+                    case rclcpp_action::ResultCode::SUCCEEDED:
+                        RCLCPP_INFO(this->get_logger(), "导航成功完成");
+                        break;
+                    case rclcpp_action::ResultCode::ABORTED:
+                        RCLCPP_ERROR(this->get_logger(), "导航被中止");
+                        break;
+                    case rclcpp_action::ResultCode::CANCELED:
+                        RCLCPP_WARN(this->get_logger(), "导航被取消");
+                        break;
+                    default:
+                        RCLCPP_ERROR(this->get_logger(), "导航未知结果");
+                        break;
+                }
+            };
+
+        nav_action_client_->async_send_goal(goal_msg, send_goal_options);
+        RCLCPP_INFO(this->get_logger(), "发送导航目标: x=%.2f, y=%.2f, angle=%.2f",
+                    goal_pose_.x, goal_pose_.y, goal_pose_.angle);
     }
 
     /**
@@ -330,6 +388,7 @@ namespace rm_simpal_move
 
         has_goal_ = true;
         goal_reached_ = false;
+        nav_goal_sent_ = false;  // 重置 NAV 目标发送标志
 
         // 重置 PID
         pid_linear_x_.reset();
@@ -339,6 +398,23 @@ namespace rm_simpal_move
         RCLCPP_INFO(this->get_logger(), "收到新目标: mode=%s, x=%.2f, y=%.2f, angle=%.2f",
                     control_mode_ == 0 ? "PID" : "NAV",
                     msg->target_x, msg->target_y, msg->target_angle);
+
+        // 如果是 NAV 模式，立即发送导航目标
+        if (control_mode_ == 1) {
+            publish_nav_goal();
+            nav_goal_sent_ = true;
+        }
+    }
+
+    /**
+     * @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);
+        }
     }
 
     /**