fix: 使用 Action 接口与 Nav2 通信

问题: 之前使用话题发布目标，Nav2 无法响应
原因: Nav2 使用 Action 接口 /navigate_to_pose，不是简单话题

修复:
- 添加 rclcpp_action 和 nav2_msgs 依赖
- 使用 Action Client 发送导航目标
- NAV 模式现在可以正确触发导航系统

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/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,6 +51,9 @@ 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;
 
@@ -77,11 +82,13 @@ private:
     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<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_;

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

@@ -102,16 +102,18 @@ namespace rm_simpal_move
         );
 
         // 发布者
-        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);
         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模式: /goal_pose → 导航 → /cmd_vel → 转发到 /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();
@@ -260,25 +262,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);
     }
 
     /**