From ecadb68f558e7745ccbaf51ab9365e1c0f1df115 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 4 Mar 2026 22:37:35 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BD=BF=E7=94=A8=20Action=20=E6=8E=A5?= =?UTF-8?q?=E5=8F=A3=E4=B8=8E=20Nav2=20=E9=80=9A=E4=BF=A1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 问题: 之前使用话题发布目标，Nav2 无法响应 原因: Nav2 使用 Action 接口 /navigate_to_pose，不是简单话题 修复: - 添加 rclcpp_action 和 nav2_msgs 依赖 - 使用 Action Client 发送导航目标 - NAV 模式现在可以正确触发导航系统 Co-Authored-By: Claude Opus 4.6 (1M context) --- src/rm_nav/rm_simple_move/CMakeLists.txt | 4 +- .../include/rm_simpal_move/simple_move.hpp | 9 ++- src/rm_nav/rm_simple_move/package.xml | 2 + src/rm_nav/rm_simple_move/src/simple_move.cpp | 69 +++++++++++++++---- 4 files changed, 68 insertions(+), 16 deletions(-) diff --git a/src/rm_nav/rm_simple_move/CMakeLists.txt b/src/rm_nav/rm_simple_move/CMakeLists.txt index a7b86b1..f4fa575 100644 --- a/src/rm_nav/rm_simple_move/CMakeLists.txt +++ b/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") diff --git a/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp b/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp index fdc6608..9e67349 100644 --- a/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp +++ b/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -9,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +51,9 @@ struct PIDController { class RMSimpleMove : public rclcpp::Node { public: + using NavigateToPose = nav2_msgs::action::NavigateToPose; + using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle; + RMSimpleMove(const rclcpp::NodeOptions &options); ~RMSimpleMove() override; @@ -77,11 +82,13 @@ private: rclcpp::Subscription::SharedPtr cmd_vel_nav_sub_; // Publishers - rclcpp::Publisher::SharedPtr nav_goal_pub_; rclcpp::Publisher::SharedPtr cmd_vel_move_pub_; rclcpp::Publisher::SharedPtr nav_status_pub_; rclcpp::Publisher::SharedPtr current_pose_pub_; + // Action Client + rclcpp_action::Client::SharedPtr nav_action_client_; + // State std::atomic control_mode_; // 0: PID, 1: NAV std::atomic running_; diff --git a/src/rm_nav/rm_simple_move/package.xml b/src/rm_nav/rm_simple_move/package.xml index 85d52c3..45e8d66 100644 --- a/src/rm_nav/rm_simple_move/package.xml +++ b/src/rm_nav/rm_simple_move/package.xml @@ -10,10 +10,12 @@ rclcpp rclcpp_components + rclcpp_action tf2_ros tf2_geometry_msgs geometry_msgs nav_msgs + nav2_msgs launch launch_ros rm_msgs diff --git a/src/rm_nav/rm_simple_move/src/simple_move.cpp b/src/rm_nav/rm_simple_move/src/simple_move.cpp index d7e7bcc..47cec7b 100644 --- a/src/rm_nav/rm_simple_move/src/simple_move.cpp +++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp @@ -102,16 +102,18 @@ namespace rm_simpal_move ); // 发布者 - nav_goal_pub_ = this->create_publisher("/goal_pose", 10); cmd_vel_move_pub_ = this->create_publisher("/cmd_vel_move", 10); nav_status_pub_ = this->create_publisher("/nav_status", 10); current_pose_pub_ = this->create_publisher("/current_pose", 10); + // Action Client + nav_action_client_ = rclcpp_action::create_client(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(); - goal_msg.header.stamp = this->get_clock()->now(); - goal_msg.header.frame_id = "map"; - goal_msg.pose.position.x = goal_pose_.x; - goal_msg.pose.position.y = goal_pose_.y; - goal_msg.pose.position.z = 0.0; + // 检查 Action Server 是否可用 + if (!nav_action_client_->wait_for_action_server(std::chrono::seconds(1))) { + RCLCPP_WARN(this->get_logger(), "导航 Action Server 不可用"); + return; + } + + // 创建 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.orientation.y = q.y(); - goal_msg.pose.orientation.z = q.z(); - goal_msg.pose.orientation.w = q.w(); + goal_msg.pose.pose.orientation.x = q.x(); + goal_msg.pose.pose.orientation.y = q.y(); + goal_msg.pose.pose.orientation.z = q.z(); + goal_msg.pose.pose.orientation.w = q.w(); - nav_goal_pub_->publish(goal_msg); + // 发送 Action Goal + auto send_goal_options = rclcpp_action::Client::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); } /**