robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix decision

Browse Source
This commit is contained in:
Robofish 2026-03-09 16:47:07 +08:00
parent 9b8cfacb01
commit bdcdcd05a4
4 changed files with 19 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/rm_decision/include/rm_decision/decision_node.hpp
View File

@ -7,7 +7,6 @@
#include <rm_msgs/msg/data_aim.hpp>
#include <rm_msgs/msg/nav_goal.hpp>
#include <rm_msgs/msg/nav_status.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <atomic>
#include <mutex>
@ -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<rm_msgs::msg::DataRef>::SharedPtr data_ref_sub_;
rclcpp::Subscription<rm_msgs::msg::DataAim>::SharedPtr data_aim_sub_;
rclcpp::Subscription<rm_msgs::msg::NavStatus>::SharedPtr nav_status_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr current_pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::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_;
// 控制状态

11
src/rm_decision/src/decision_node.cpp
View File

@ -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<geometry_msgs::msg::PoseStamped>(
"/current_pose", 10,
std::bind(&DecisionNode::current_pose_callback, this, std::placeholders::_1));
cmd_vel_move_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"/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<std::mutex> lock(mutex_);
current_pose_ = *msg;
}
void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);

4
src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
View File

@ -7,7 +7,6 @@
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#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>
@ -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<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_;

33
src/rm_nav/rm_simple_move/src/simple_move.cpp
View File

@ -105,7 +105,6 @@ namespace rm_simpal_move
// 发布者
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");
@ -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(&current_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);