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>
This commit is contained in:
parent
ebdafcca7c
commit
ecadb68f55
@ -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")
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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();
|
||||
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<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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Loading…
Reference in New Issue
Block a user