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); } /**