src/rm_nav/rm_simple_move/CMakeLists.txt

@@ -9,12 +9,10 @@ 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
@@ -24,7 +22,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 rclcpp_action tf2_ros geometry_msgs tf2_geometry_msgs nav_msgs nav2_msgs rm_msgs)
+ament_target_dependencies(rm_simpal_move rclcpp rclcpp_components tf2_ros geometry_msgs tf2_geometry_msgs nav_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_move` (geometry_msgs/Twist)
+#### `/cmd_vel` (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_move
+ros2 topic echo /cmd_vel
 
 # 查看导航状态
 ros2 topic echo /nav_status
@@ -166,19 +166,13 @@ ros2 topic echo /current_pose
 2. 通过 TF 获取当前位置 (map -> base_link)
 3. 计算目标点在 base_link 坐标系下的相对位置
 4. 使用 PID 控制器计算速度指令
-5. 发布到 `/cmd_vel_move`
+5. 发布标准 `/cmd_vel` 控制指令
 
 ### 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`
@@ -192,24 +186,13 @@ ros2 topic echo /current_pose
 ```
 决策层 (Decision Layer)
     ↓ /nav_goal (mode + target)
-
 rm_simple_move (Bridge)
-    ├─ PID 模式: 直接计算 → /cmd_vel_move
+    ↓ PID 模式: /cmd_vel (直接控制)
-    └─ NAV 模式: /goal_pose → Navigation Stack
+    ↓ NAV 模式: /goal_pose → Navigation → /cmd_vel
-                                    ↓
+控制层 (Control Layer)
-                              /cmd_vel_nav → velocity_smoother → /cmd_vel
+    ← /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,7 +1,6 @@
 #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>
@@ -10,7 +9,6 @@
 #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>
@@ -51,16 +49,12 @@ 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);
@@ -79,22 +73,18 @@ 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::Twist>::SharedPtr cmd_vel_move_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr nav_goal_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,12 +10,10 @@
 
   <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,7 +62,6 @@ 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)
@@ -97,26 +96,12 @@ 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)
-        );
-
         // 发布者
-        cmd_vel_move_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_move", 10);
+        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);
         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 启动");
@@ -135,10 +120,14 @@ namespace rm_simpal_move
      */
     void RMSimpleMove::timer_callback()
     {
+        if (!has_goal_) {
+            return;
+        }
+
         try
         {
-            // 获取当前位置（始终发布，不管有没有目标）
+            // 获取当前位置
-            auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero, tf2::durationFromSec(0.5));
+            auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
             AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
 
             // 发布当前位置
@@ -151,11 +140,6 @@ 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();
@@ -176,7 +160,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, tf2::durationFromSec(0.5));
+            goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);
 
             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;
@@ -196,11 +180,8 @@ namespace rm_simpal_move
                 // PID 模式
                 publish_pid_control();
             } else {
-                // NAV 模式：只在第一次发送目标
+                // NAV 模式
-                if (!nav_goal_sent_) {
+                publish_nav_goal();
-                    publish_nav_goal();
-                    nav_goal_sent_ = true;
-                }
             }
 
             // 发布状态
@@ -208,7 +189,7 @@ namespace rm_simpal_move
         }
         catch (const tf2::TransformException &ex)
         {
-            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "TF 变换失败: %s", ex.what());
+            RCLCPP_WARN(this->get_logger(), "TF 变换失败: %s", ex.what());
         }
     }
 
@@ -250,7 +231,7 @@ namespace rm_simpal_move
                 vy *= scale;
             }
 
-            // 发布标准 cmd_vel_move
+            // 发布标准 cmd_vel
             auto cmd_vel_msg = geometry_msgs::msg::Twist();
             cmd_vel_msg.linear.x = vx;
             cmd_vel_msg.linear.y = vy;
@@ -258,7 +239,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_move_pub_->publish(cmd_vel_msg);
+            cmd_vel_pub_->publish(cmd_vel_msg);
 
         } catch (const tf2::TransformException &ex) {
             RCLCPP_WARN(this->get_logger(), "PID 控制 TF 失败: %s", ex.what());
@@ -266,64 +247,25 @@ namespace rm_simpal_move
     }
 
     /**
-     * @brief 发布导航目标 (使用 Action)
+     * @brief 发布导航目标
      */
     void RMSimpleMove::publish_nav_goal()
     {
-        // 检查 Action Server 是否可用
+        auto goal_msg = geometry_msgs::msg::PoseStamped();
-        if (!nav_action_client_->wait_for_action_server(std::chrono::seconds(1))) {
+        goal_msg.header.stamp = this->get_clock()->now();
-            RCLCPP_WARN(this->get_logger(), "导航 Action Server 不可用");
+        goal_msg.header.frame_id = "map";
-            return;
+        goal_msg.pose.position.x = goal_pose_.x;
-        }
+        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.pose.orientation.x = q.x();
+        goal_msg.pose.orientation.x = q.x();
-        goal_msg.pose.pose.orientation.y = q.y();
+        goal_msg.pose.orientation.y = q.y();
-        goal_msg.pose.pose.orientation.z = q.z();
+        goal_msg.pose.orientation.z = q.z();
-        goal_msg.pose.pose.orientation.w = q.w();
+        goal_msg.pose.orientation.w = q.w();
 
-        // 发送 Action Goal
+        nav_goal_pub_->publish(goal_msg);
-        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);
     }
 
     /**
@@ -388,7 +330,6 @@ namespace rm_simpal_move
 
         has_goal_ = true;
         goal_reached_ = false;
-        nav_goal_sent_ = false;  // 重置 NAV 目标发送标志
 
         // 重置 PID
         pid_linear_x_.reset();
@@ -398,23 +339,6 @@ 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);
-        }
     }
 
     /**