diff --git a/src/rm_decision/include/rm_decision/decision_node.hpp b/src/rm_decision/include/rm_decision/decision_node.hpp index d36608c..2024d66 100644 --- a/src/rm_decision/include/rm_decision/decision_node.hpp +++ b/src/rm_decision/include/rm_decision/decision_node.hpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include @@ -37,7 +36,6 @@ private: void data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg); void data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg); void nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg); - void current_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); void cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg); // 定时器回调 @@ -52,7 +50,6 @@ private: rclcpp::Subscription::SharedPtr data_ref_sub_; rclcpp::Subscription::SharedPtr data_aim_sub_; rclcpp::Subscription::SharedPtr nav_status_sub_; - rclcpp::Subscription::SharedPtr current_pose_sub_; rclcpp::Subscription::SharedPtr cmd_vel_move_sub_; // 发布者 @@ -68,7 +65,6 @@ private: rm_msgs::msg::DataRef latest_ref_data_; rm_msgs::msg::DataAim latest_aim_data_; rm_msgs::msg::NavStatus latest_nav_status_; - geometry_msgs::msg::PoseStamped current_pose_; geometry_msgs::msg::Twist latest_cmd_vel_; // 控制状态 diff --git a/src/rm_decision/src/decision_node.cpp b/src/rm_decision/src/decision_node.cpp index f6a3ffe..1796846 100644 --- a/src/rm_decision/src/decision_node.cpp +++ b/src/rm_decision/src/decision_node.cpp @@ -32,10 +32,6 @@ DecisionNode::DecisionNode(const rclcpp::NodeOptions &options) "/nav_status", 10, std::bind(&DecisionNode::nav_status_callback, this, std::placeholders::_1)); - current_pose_sub_ = this->create_subscription( - "/current_pose", 10, - std::bind(&DecisionNode::current_pose_callback, this, std::placeholders::_1)); - cmd_vel_move_sub_ = this->create_subscription( "/cmd_vel_move", 10, std::bind(&DecisionNode::cmd_vel_move_callback, this, std::placeholders::_1)); @@ -61,7 +57,6 @@ DecisionNode::DecisionNode(const rclcpp::NodeOptions &options) RCLCPP_INFO(this->get_logger(), " 订阅: /data_ref (裁判系统)"); RCLCPP_INFO(this->get_logger(), " 订阅: /vision/aim (视觉自瞄)"); RCLCPP_INFO(this->get_logger(), " 订阅: /nav_status (导航状态)"); - RCLCPP_INFO(this->get_logger(), " 订阅: /current_pose (当前位置)"); RCLCPP_INFO(this->get_logger(), " 订阅: /cmd_vel_move (底盘速度)"); RCLCPP_INFO(this->get_logger(), " 发布: /data_ai (综合控制指令)"); RCLCPP_INFO(this->get_logger(), " 发布: /nav_goal (导航目标)"); @@ -100,12 +95,6 @@ void DecisionNode::nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr has_nav_data_ = true; } -void DecisionNode::current_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ - std::lock_guard lock(mutex_); - current_pose_ = *msg; -} - void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg) { std::lock_guard lock(mutex_); 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 3d30a92..894cd1c 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 @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include @@ -67,7 +66,7 @@ private: void publish_nav_goal(); void publish_pid_control(); - void publish_status(); + void publish_status(float current_x, float current_y, float current_angle); // TF tf2_ros::Buffer tf_buffer_; @@ -84,7 +83,6 @@ private: // Publishers 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_; 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 b628440..2360e5d 100644 --- a/src/rm_nav/rm_simple_move/src/simple_move.cpp +++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp @@ -105,7 +105,6 @@ namespace rm_simpal_move // 发布者 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"); @@ -115,7 +114,7 @@ namespace rm_simpal_move 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"); + RCLCPP_INFO(this->get_logger(), " 状态: /nav_status"); last_time_ = this->get_clock()->now(); @@ -140,19 +139,13 @@ namespace rm_simpal_move // 获取当前位置（始终发布，不管有没有目标） auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero, tf2::durationFromSec(0.5)); AHRS_GetEulr(¤t_eulr_, trans.transform.rotation); + const float current_x = trans.transform.translation.x; + const float current_y = trans.transform.translation.y; + const float current_angle = current_eulr_.yaw; - // 发布当前位置 - auto current_pose_msg = geometry_msgs::msg::PoseStamped(); - current_pose_msg.header.stamp = this->get_clock()->now(); - current_pose_msg.header.frame_id = "map"; - current_pose_msg.pose.position.x = trans.transform.translation.x; - current_pose_msg.pose.position.y = trans.transform.translation.y; - current_pose_msg.pose.position.z = trans.transform.translation.z; - current_pose_msg.pose.orientation = trans.transform.rotation; - current_pose_pub_->publish(current_pose_msg); - - // 如果没有目标，只发布当前位置，不执行控制 + // 如果没有目标，仅发布当前位置状态，不执行控制 if (!has_goal_) { + publish_status(current_x, current_y, current_angle); return; } @@ -204,7 +197,7 @@ namespace rm_simpal_move } // 发布状态 - publish_status(); + publish_status(current_x, current_y, current_angle); } catch (const tf2::TransformException &ex) { @@ -329,9 +322,19 @@ namespace rm_simpal_move /** * @brief 发布状态信息 */ - void RMSimpleMove::publish_status() + void RMSimpleMove::publish_status(float current_x, float current_y, float current_angle) { auto status_msg = rm_msgs::msg::NavStatus(); + status_msg.current_x = current_x; + status_msg.current_y = current_y; + status_msg.current_angle = current_angle; + + if (!has_goal_) { + status_msg.status = 0; // 空闲 + status_msg.distance = 0.0f; + nav_status_pub_->publish(status_msg); + return; + } try { auto goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);