导航与中央节点初步实现（未测试）

CMakeLists.txt

@@ -43,12 +43,32 @@ ament_target_dependencies(imu_receiver_node
   sensor_msgs
 )
 
+# ==================== 中央节点 ====================
+add_executable(central_node src/central_node.cpp)
+
+ament_target_dependencies(central_node 
+  rclcpp 
+  std_msgs
+  geometry_msgs
+)
+
+# ==================== 导航节点 ====================
+add_executable(navigation_node src/navigation_node.cpp)
+
+ament_target_dependencies(navigation_node 
+  rclcpp 
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+)
+
 # ==================== 安装目标 ====================
 install(TARGETS 
   uart_transmitter_node
   imu_receiver_node
-  DESTINATION lib/${PROJECT_NAME}
+  central_node
-)
+  navigation_node
+  DESTINATION lib/${PROJECT_NAME})
 
 # 安装 launch 文件
 install(DIRECTORY launch

README.md

@@ -23,7 +23,7 @@
 
 ## 目标
 
-感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。
+感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26，整车决策和节点图系统为自有设计。
 
 ## 软件架构
 
@@ -44,7 +44,7 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
 - 订阅：中央节点 -> 控制指令
 - 发送：CH340.TX -> 控制指令
 - 发布：裁判系统数据（血量、比赛当前状态）
-        CH340 、IMU 连接 OK 标志
+        CH340 连接 OK 标志
 
 ### IMU 收发节点
 
@@ -87,3 +87,9 @@ stateDiagram-v2
 - 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪
         中央节点 -> 状态
 - 发布：导航运动指令
+
+导航思想简单描述：
+0. IMU 被安装在哨兵云台的 YAW 转盘上，坐标轴系向前为 IMU 接口中对应 X 轴，向右为 Y 轴，向下为 Z 轴。由于底盘作动在代码中设置为跟随云台朝向，因此控制量可以直接同步到 IMU 的这个坐标系中观察作用。
+1. 将本届 RMUL 地图简化为一个 2D 平面地图，整体轮廓为矩形，面积 $(12\times 8)  m^2$。设置原点为矩形左下角，矩形长（X 轴） 12 米，高（Y 轴）8 米，基地范围为矩形 \[(0,6),(1.5,8)\]，有两墙在图中近似为矩形，一个位于 \[(2.5,1.5),(4.5,8)\] 另一个位于其相对场地的几何中心的中心对称位置，不可跨越，不可通过。“增益点”范围为矩形 \[(4.5,2.5),(7,9.5)\]。
+2. 将机器人简化为一个中心点圆，IMU 位于中心点，圆直径为 0.75 米。开局时，因为没有对地图的重定位手段，人为放置机器人，使得机器人中心点圆与基地矩形下、右边缘相切，使云台朝向场地的 Y 轴方向，至此机器人与场地的 X、Y 轴坐标系映射完成。
+3. 后续依据此映射和 IMU 的数据，计算出机器人的当前位置，并根据当前状态规划一个绕过墙，前往“增益点”内的路径。目前的方法是直接计算出能使机器人中心点圆通过的路径并一直按该路径行动（设置几个路径点，中心点圆覆盖到路径点时认为路径部分完成）

launch/amadeus_26.launch.py

@@ -0,0 +1,84 @@
+"""
+Amadeus 26 哨兵机器人启动文件
+启动所有核心节点：IMU接收、中央控制、导航、UART收发
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    # 声明启动参数
+    enemy_color_arg = DeclareLaunchArgument(
+        'enemy_color',
+        default_value='red',
+        description='敌方装甲板颜色 (red/blue)'
+    )
+    
+    # IMU接收节点
+    imu_receiver_node = Node(
+        package='amadeus_26',
+        executable='imu_receiver_node',
+        name='imu_receiver_node',
+        output='screen',
+        parameters=[{
+            'serial_port': '/dev/ttyIMU',
+            'baudrate': 921600,
+            'verbose': False,
+        }]
+    )
+    
+    # 中央节点
+    central_node = Node(
+        package='amadeus_26',
+        executable='central_node',
+        name='central_node',
+        output='screen',
+        parameters=[{
+            'enemy_color': LaunchConfiguration('enemy_color'),
+            'max_hp': 100,
+            'low_hp_threshold': 50,
+            'control_freq': 50.0,
+        }]
+    )
+    
+    # 导航节点
+    navigation_node = Node(
+        package='amadeus_26',
+        executable='navigation_node',
+        name='navigation_node',
+        output='screen',
+        parameters=[{
+            'map_width': 12.0,
+            'map_height': 8.0,
+            'robot_radius': 0.375,
+            'max_speed': 2.0,
+            'nav_freq': 50.0,
+            'init_x': 0.375,
+            'init_y': 6.375,
+            'init_heading': 0.0,
+        }]
+    )
+    
+    # UART收发节点
+    uart_transmitter_node = Node(
+        package='amadeus_26',
+        executable='uart_transmitter_node',
+        name='uart_transmitter_node',
+        output='screen',
+        parameters=[{
+            'serial_port': '/dev/ttyCH340',
+            'baudrate': 115200,
+            'send_frequency': 50.0,
+        }]
+    )
+    
+    return LaunchDescription([
+        enemy_color_arg,
+        imu_receiver_node,
+        central_node,
+        navigation_node,
+        uart_transmitter_node,
+    ])

src/central_node.cpp

@@ -0,0 +1,433 @@
+/**
+ * @file central_node.cpp
+ * @brief 中央节点 - 综合各类信息判断状态与转移，是控制指令发送的唯一来源
+ *
+ * 功能：
+ * - 订阅：自瞄节点 -> 是否有装甲板、自瞄移动指令、开火指令
+ *         收发节点 -> 裁判系统.血量、裁判系统.比赛进程、USB2CAN连接状态
+ * - 发布：当前状态、敌方装甲板颜色、控制指令
+ *
+ * 状态机：
+ * 等待 -> 进点 -> 索敌 -> 攻击
+ *         ↘ 回家 ↗
+ */
+
+#include <chrono>
+#include <cmath>
+#include <geometry_msgs/msg/twist.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/string.hpp>
+
+namespace amadeus_26 {
+
+// 机器人状态枚举
+enum class RobotState : int32_t {
+  WAITING = 0,       // 等待
+  MOVE_TO_POINT = 1, // 进点
+  SEARCHING = 2,     // 索敌
+  ATTACKING = 3,     // 攻击
+  RETURN_HOME = 4    // 回家
+};
+
+// 状态名称转换
+const char *stateToString(RobotState state) {
+  switch (state) {
+  case RobotState::WAITING:
+    return "等待";
+  case RobotState::MOVE_TO_POINT:
+    return "进点";
+  case RobotState::SEARCHING:
+    return "索敌";
+  case RobotState::ATTACKING:
+    return "攻击";
+  case RobotState::RETURN_HOME:
+    return "回家";
+  default:
+    return "未知";
+  }
+}
+
+// 控制指令消息结构
+struct ControlCommand {
+  int16_t x_move = 0;       // 平动左右 [-660, 660]
+  int16_t y_move = 0;       // 平动前后 [-660, 660]
+  int16_t yaw = 0;          // 云台偏航 [-660, 660]
+  int16_t pitch = 0;        // 云台俯仰 [-660, 660]
+  int16_t feed = 0;         // 拨弹轮 [-660, 660]
+  uint8_t left_switch = 0;  // 左拨杆 [1, 3]
+  uint8_t right_switch = 0; // 右拨杆 [1, 3]
+  bool fire = false;        // 开火标志
+};
+
+class CentralNode : public rclcpp::Node {
+public:
+  CentralNode() : Node("central_node") {
+    // 声明参数
+    this->declare_parameter("enemy_color", "red");   // 敌方颜色 red/blue
+    this->declare_parameter("max_hp", 100);          // 最大血量
+    this->declare_parameter("low_hp_threshold", 50); // 低血量阈值
+    this->declare_parameter("control_freq", 50.0);   // 控制发布频率 Hz
+
+    // 获取参数
+    enemy_color_ = this->get_parameter("enemy_color").as_string();
+    max_hp_ = this->get_parameter("max_hp").as_int();
+    low_hp_threshold_ = this->get_parameter("low_hp_threshold").as_int();
+    control_freq_ = this->get_parameter("control_freq").as_double();
+
+    RCLCPP_INFO(this->get_logger(), "=================================");
+    RCLCPP_INFO(this->get_logger(), "中央节点启动");
+    RCLCPP_INFO(this->get_logger(), "敌方颜色: %s", enemy_color_.c_str());
+    RCLCPP_INFO(this->get_logger(), "血量阈值: %d/%d", low_hp_threshold_,
+                max_hp_);
+    RCLCPP_INFO(this->get_logger(), "控制频率: %.1f Hz", control_freq_);
+    RCLCPP_INFO(this->get_logger(), "=================================");
+
+    // 创建发布者
+    state_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("robot/state", 10);
+    color_pub_ =
+        this->create_publisher<std_msgs::msg::String>("robot/armor_color", 10);
+
+    // 控制指令发布 - 使用自定义消息格式
+    control_x_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("control/x_move", 10);
+    control_y_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("control/y_move", 10);
+    control_yaw_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("control/yaw", 10);
+    control_pitch_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("control/pitch", 10);
+    control_feed_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("control/feed", 10);
+    control_switch_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("control/switch", 10);
+    control_fire_pub_ =
+        this->create_publisher<std_msgs::msg::Bool>("control/fire", 10);
+
+    // 创建订阅者
+    // 导航指令订阅
+    nav_x_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "navigation/x_move", 10,
+        std::bind(&CentralNode::navXCallback, this, std::placeholders::_1));
+    nav_y_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "navigation/y_move", 10,
+        std::bind(&CentralNode::navYCallback, this, std::placeholders::_1));
+
+    // 自瞄节点订阅（预留接口）
+    auto_aim_detected_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "auto_aim/detected", 10,
+        std::bind(&CentralNode::autoAimDetectedCallback, this,
+                  std::placeholders::_1));
+    auto_aim_yaw_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "auto_aim/yaw", 10,
+        std::bind(&CentralNode::autoAimYawCallback, this,
+                  std::placeholders::_1));
+    auto_aim_pitch_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "auto_aim/pitch", 10,
+        std::bind(&CentralNode::autoAimPitchCallback, this,
+                  std::placeholders::_1));
+    auto_aim_fire_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "auto_aim/fire", 10,
+        std::bind(&CentralNode::autoAimFireCallback, this,
+                  std::placeholders::_1));
+
+    // 收发节点订阅（预留接口）
+    referee_hp_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "referee/hp", 10,
+        std::bind(&CentralNode::refereeHpCallback, this,
+                  std::placeholders::_1));
+    referee_game_status_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "referee/game_status", 10,
+        std::bind(&CentralNode::refereeGameStatusCallback, this,
+                  std::placeholders::_1));
+    transmitter_connected_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "transmitter/connection_status", 10,
+        std::bind(&CentralNode::transmitterConnectedCallback, this,
+                  std::placeholders::_1));
+
+    // 创建定时器
+    auto period = std::chrono::duration<double>(1.0 / control_freq_);
+    control_timer_ = this->create_wall_timer(
+        std::chrono::duration_cast<std::chrono::milliseconds>(period),
+        std::bind(&CentralNode::controlLoop, this));
+
+    state_timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&CentralNode::stateMachineUpdate, this));
+
+    status_timer_ = this->create_wall_timer(
+        std::chrono::seconds(1), std::bind(&CentralNode::publishStatus, this));
+
+    // 发布敌方颜色
+    auto color_msg = std_msgs::msg::String();
+    color_msg.data = enemy_color_;
+    color_pub_->publish(color_msg);
+
+    RCLCPP_INFO(this->get_logger(), "中央节点初始化完成");
+  }
+
+private:
+  // ===== 回调函数 =====
+  void navXCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    nav_cmd_.x_move = msg->data;
+  }
+
+  void navYCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    nav_cmd_.y_move = msg->data;
+  }
+
+  void autoAimDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg) {
+    armor_detected_ = msg->data;
+  }
+
+  void autoAimYawCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    auto_aim_cmd_.yaw = msg->data;
+  }
+
+  void autoAimPitchCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    auto_aim_cmd_.pitch = msg->data;
+  }
+
+  void autoAimFireCallback(const std_msgs::msg::Bool::SharedPtr msg) {
+    auto_aim_cmd_.fire = msg->data;
+  }
+
+  void refereeHpCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    current_hp_ = msg->data;
+  }
+
+  void refereeGameStatusCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    // 游戏状态: 0=未开始, 1=准备, 2=进行中
+    game_started_ = (msg->data == 2);
+  }
+
+  void transmitterConnectedCallback(const std_msgs::msg::Bool::SharedPtr msg) {
+    communication_ok_ = msg->data;
+  }
+
+  // ===== 状态机更新 =====
+  void stateMachineUpdate() {
+    auto prev_state = current_state_;
+
+    switch (current_state_) {
+    case RobotState::WAITING:
+      // 等待 -> 进点: 通信OK 且 比赛开始 且 血量=100%
+      if (communication_ok_ && game_started_ && current_hp_ >= max_hp_) {
+        current_state_ = RobotState::MOVE_TO_POINT;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 等待 -> 进点");
+      }
+      break;
+
+    case RobotState::MOVE_TO_POINT:
+      // 进点 -> 索敌: 到达点内
+      if (reached_point_) {
+        current_state_ = RobotState::SEARCHING;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 进点 -> 索敌");
+      }
+      break;
+
+    case RobotState::SEARCHING:
+      // 索敌 -> 攻击: 搜寻到装甲板
+      if (armor_detected_) {
+        current_state_ = RobotState::ATTACKING;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 索敌 -> 攻击");
+      }
+      // 索敌 -> 回家: 血量 < 50%
+      if (current_hp_ < low_hp_threshold_) {
+        current_state_ = RobotState::RETURN_HOME;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 索敌 -> 回家 (低血量)");
+      }
+      break;
+
+    case RobotState::ATTACKING:
+      // 攻击 -> 索敌: 无装甲板
+      if (!armor_detected_) {
+        current_state_ = RobotState::SEARCHING;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 攻击 -> 索敌");
+      }
+      // 攻击 -> 回家: 血量 < 50%
+      if (current_hp_ < low_hp_threshold_) {
+        current_state_ = RobotState::RETURN_HOME;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 攻击 -> 回家 (低血量)");
+      }
+      break;
+
+    case RobotState::RETURN_HOME:
+      // 回家 -> 等待: 到达基地
+      if (reached_home_) {
+        current_state_ = RobotState::WAITING;
+        RCLCPP_INFO(this->get_logger(), "状态转移: 回家 -> 等待");
+      }
+      break;
+    }
+
+    // 发布状态
+    auto state_msg = std_msgs::msg::Int32();
+    state_msg.data = static_cast<int32_t>(current_state_);
+    state_pub_->publish(state_msg);
+
+    if (prev_state != current_state_) {
+      RCLCPP_INFO(this->get_logger(), "当前状态: %s",
+                  stateToString(current_state_));
+    }
+  }
+
+  // ===== 控制循环 =====
+  void controlLoop() {
+    ControlCommand cmd;
+
+    switch (current_state_) {
+    case RobotState::WAITING:
+      // 等待状态: 静止
+      cmd = createStopCommand();
+      break;
+
+    case RobotState::MOVE_TO_POINT:
+      // 进点状态: 使用导航指令
+      cmd = nav_cmd_;
+      // 允许自瞄控制云台
+      if (armor_detected_) {
+        cmd.yaw = auto_aim_cmd_.yaw;
+        cmd.pitch = auto_aim_cmd_.pitch;
+      }
+      break;
+
+    case RobotState::SEARCHING:
+      // 索敌状态: 使用导航指令 + 自动扫描
+      cmd = nav_cmd_;
+      // TODO: 添加扫描逻辑
+      break;
+
+    case RobotState::ATTACKING:
+      // 攻击状态: 自瞄优先
+      cmd = nav_cmd_; // 底盘可以移动
+      cmd.yaw = auto_aim_cmd_.yaw;
+      cmd.pitch = auto_aim_cmd_.pitch;
+      cmd.fire = auto_aim_cmd_.fire;
+      cmd.feed = auto_aim_cmd_.fire ? 500 : 0; // 开火时拨弹轮转动
+      break;
+
+    case RobotState::RETURN_HOME:
+      // 回家状态: 使用导航回家指令
+      cmd = nav_cmd_; // 导航节点会发布回家路径
+      break;
+    }
+
+    // 发布控制指令
+    publishControlCommand(cmd);
+  }
+
+  ControlCommand createStopCommand() {
+    ControlCommand cmd;
+    cmd.x_move = 0;
+    cmd.y_move = 0;
+    cmd.yaw = 0;
+    cmd.pitch = 0;
+    cmd.feed = 0;
+    cmd.left_switch = 1;
+    cmd.right_switch = 1;
+    cmd.fire = false;
+    return cmd;
+  }
+
+  void publishControlCommand(const ControlCommand &cmd) {
+    auto x_msg = std_msgs::msg::Int32();
+    x_msg.data = cmd.x_move;
+    auto y_msg = std_msgs::msg::Int32();
+    y_msg.data = cmd.y_move;
+    auto yaw_msg = std_msgs::msg::Int32();
+    yaw_msg.data = cmd.yaw;
+    auto pitch_msg = std_msgs::msg::Int32();
+    pitch_msg.data = cmd.pitch;
+    auto feed_msg = std_msgs::msg::Int32();
+    feed_msg.data = cmd.feed;
+    auto switch_msg = std_msgs::msg::Int32();
+    switch_msg.data = (cmd.left_switch << 4) | cmd.right_switch;
+    auto fire_msg = std_msgs::msg::Bool();
+    fire_msg.data = cmd.fire;
+
+    control_x_pub_->publish(x_msg);
+    control_y_pub_->publish(y_msg);
+    control_yaw_pub_->publish(yaw_msg);
+    control_pitch_pub_->publish(pitch_msg);
+    control_feed_pub_->publish(feed_msg);
+    control_switch_pub_->publish(switch_msg);
+    control_fire_pub_->publish(fire_msg);
+  }
+
+  void publishStatus() {
+    RCLCPP_INFO_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000,
+        "状态: %s | HP: %d/%d | 通信: %s | 比赛: %s | 装甲板: %s",
+        stateToString(current_state_), current_hp_, max_hp_,
+        communication_ok_ ? "OK" : "NG", game_started_ ? "进行中" : "未开始",
+        armor_detected_ ? "检测到" : "未检测");
+  }
+
+  // ===== 成员变量 =====
+  // 参数
+  std::string enemy_color_;
+  int max_hp_;
+  int low_hp_threshold_;
+  double control_freq_;
+
+  // 当前状态
+  RobotState current_state_ = RobotState::WAITING;
+
+  // 传感器数据
+  int current_hp_ = 100;
+  bool game_started_ = false;
+  bool communication_ok_ = false;
+  bool armor_detected_ = false;
+
+  // 位置标志（由导航节点设置，通过参数或其他方式）
+  bool reached_point_ = false; // TODO: 需要从导航节点获取
+  bool reached_home_ = false;  // TODO: 需要从导航节点获取
+
+  // 控制指令
+  ControlCommand nav_cmd_;      // 导航指令
+  ControlCommand auto_aim_cmd_; // 自瞄指令
+
+  // 发布者
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr state_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr color_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr control_x_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr control_y_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr control_yaw_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr control_pitch_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr control_feed_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr control_switch_pub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr control_fire_pub_;
+
+  // 订阅者
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr nav_x_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr nav_y_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr auto_aim_detected_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr auto_aim_yaw_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr auto_aim_pitch_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr auto_aim_fire_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr referee_hp_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr
+      referee_game_status_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr
+      transmitter_connected_sub_;
+
+  // 定时器
+  rclcpp::TimerBase::SharedPtr control_timer_;
+  rclcpp::TimerBase::SharedPtr state_timer_;
+  rclcpp::TimerBase::SharedPtr status_timer_;
+};
+
+} // namespace amadeus_26
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<amadeus_26::CentralNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

src/navigation_node.cpp

@@ -0,0 +1,493 @@
+/**
+ * @file navigation_node.cpp
+ * @brief 导航节点 - 根据IMU数据计算位置，根据状态决定目标位置并发布导航指令
+ *
+ * 功能：
+ * - 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪
+ *         中央节点 -> 状态
+ * - 发布：导航运动指令
+ *
+ * 地图说明：
+ * - 地图尺寸: 12m x 8m
+ * - 原点: 左下角 (0, 0)
+ * - 基地范围: [(0,6), (1.5,8)]
+ * - 墙1: [(2.5,1.5), (4.5,8)]
+ * - 墙2: 对称位置 [(7.5,0), (9.5,6.5)]
+ * - 增益点: [(4.5,2.5), (7,9.5)]  (注意：y上限超过8，实际是到7.5)
+ * - 机器人直径: 0.75m
+ */
+
+#include <chrono>
+#include <cmath>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/int32.hpp>
+
+namespace amadeus_26 {
+
+// 机器人状态
+enum class RobotState : int32_t {
+  WAITING = 0,
+  MOVE_TO_POINT = 1,
+  SEARCHING = 2,
+  ATTACKING = 3,
+  RETURN_HOME = 4
+};
+
+// 2D位置结构
+struct Position2D {
+  double x = 0.0;
+  double y = 0.0;
+
+  Position2D() = default;
+  Position2D(double x_, double y_) : x(x_), y(y_) {}
+
+  double distanceTo(const Position2D &other) const {
+    return std::sqrt(std::pow(x - other.x, 2) + std::pow(y - other.y, 2));
+  }
+};
+
+// 矩形区域
+struct Rectangle {
+  double x_min, y_min, x_max, y_max;
+
+  bool contains(const Position2D &pos) const {
+    return pos.x >= x_min && pos.x <= x_max && pos.y >= y_min && pos.y <= y_max;
+  }
+
+  Position2D center() const {
+    return Position2D((x_min + x_max) / 2.0, (y_min + y_max) / 2.0);
+  }
+};
+
+// 路径点
+struct Waypoint {
+  Position2D position;
+  double tolerance = 0.3; // 到达容忍距离(m)
+};
+
+class NavigationNode : public rclcpp::Node {
+public:
+  NavigationNode() : Node("navigation_node") {
+    // 声明参数
+    this->declare_parameter("map_width", 12.0);     // 地图宽度(m)
+    this->declare_parameter("map_height", 8.0);     // 地图高度(m)
+    this->declare_parameter("robot_radius", 0.375); // 机器人半径(m)
+    this->declare_parameter("max_speed", 2.0);      // 最大速度(m/s)
+    this->declare_parameter("nav_freq", 50.0);      // 导航频率(Hz)
+    this->declare_parameter("init_x", 0.375);       // 初始X位置
+    this->declare_parameter("init_y", 6.375);       // 初始Y位置
+    this->declare_parameter("init_heading",
+                            0.0); // 初始朝向(弧度，0为X轴正方向)
+
+    // 获取参数
+    map_width_ = this->get_parameter("map_width").as_double();
+    map_height_ = this->get_parameter("map_height").as_double();
+    robot_radius_ = this->get_parameter("robot_radius").as_double();
+    max_speed_ = this->get_parameter("max_speed").as_double();
+    nav_freq_ = this->get_parameter("nav_freq").as_double();
+
+    // 初始化位置
+    current_pos_.x = this->get_parameter("init_x").as_double();
+    current_pos_.y = this->get_parameter("init_y").as_double();
+    current_heading_ = this->get_parameter("init_heading").as_double();
+
+    RCLCPP_INFO(this->get_logger(), "=================================");
+    RCLCPP_INFO(this->get_logger(), "导航节点启动");
+    RCLCPP_INFO(this->get_logger(), "地图尺寸: %.1fm x %.1fm", map_width_,
+                map_height_);
+    RCLCPP_INFO(this->get_logger(), "机器人半径: %.3fm", robot_radius_);
+    RCLCPP_INFO(this->get_logger(), "初始位置: (%.3f, %.3f)", current_pos_.x,
+                current_pos_.y);
+    RCLCPP_INFO(this->get_logger(), "=================================");
+
+    // 初始化地图区域
+    initMapRegions();
+
+    // 创建发布者
+    nav_x_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("navigation/x_move", 10);
+    nav_y_pub_ =
+        this->create_publisher<std_msgs::msg::Int32>("navigation/y_move", 10);
+    pos_x_pub_ =
+        this->create_publisher<std_msgs::msg::Float32>("navigation/pos_x", 10);
+    pos_y_pub_ =
+        this->create_publisher<std_msgs::msg::Float32>("navigation/pos_y", 10);
+    reached_point_pub_ = this->create_publisher<std_msgs::msg::Bool>(
+        "navigation/reached_point", 10);
+    reached_home_pub_ = this->create_publisher<std_msgs::msg::Bool>(
+        "navigation/reached_home", 10);
+
+    // 创建订阅者
+    imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
+        "imu/ahrs", 10,
+        std::bind(&NavigationNode::imuCallback, this, std::placeholders::_1));
+    state_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "robot/state", 10,
+        std::bind(&NavigationNode::stateCallback, this, std::placeholders::_1));
+
+    // 创建定时器
+    auto period = std::chrono::duration<double>(1.0 / nav_freq_);
+    nav_timer_ = this->create_wall_timer(
+        std::chrono::duration_cast<std::chrono::milliseconds>(period),
+        std::bind(&NavigationNode::navigationLoop, this));
+
+    status_timer_ = this->create_wall_timer(
+        std::chrono::seconds(1),
+        std::bind(&NavigationNode::publishStatus, this));
+
+    RCLCPP_INFO(this->get_logger(), "导航节点初始化完成");
+  }
+
+private:
+  void initMapRegions() {
+    // 基地范围 [(0,6), (1.5,8)]
+    home_region_ = {0.0, 6.0, 1.5, 8.0};
+
+    // 增益点范围 [(4.5,2.5), (7,7.5)] (限制在地图内)
+    point_region_ = {4.5, 2.5, 7.0, 7.5};
+
+    // 墙1 [(2.5,1.5), (4.5,8)]
+    wall1_ = {2.5, 1.5, 4.5, 8.0};
+
+    // 墙2 对称位置 [(7.5,0), (9.5,6.5)]
+    wall2_ = {7.5, 0.0, 9.5, 6.5};
+
+    // 规划路径点（从基地到增益点，绕过墙1）
+    // 路径：基地 -> 路径点1 -> 路径点2 -> 增益点中心
+    waypoints_to_point_.clear();
+    waypoints_to_point_.push_back({Position2D(2.0, 7.0), 0.3}); // 出基地
+    waypoints_to_point_.push_back({Position2D(2.0, 2.0), 0.3}); // 绕过墙1左侧
+    waypoints_to_point_.push_back(
+        {Position2D(5.75, 2.0), 0.3}); // 到达增益点下方
+    waypoints_to_point_.push_back({Position2D(5.75, 5.0), 0.5}); // 增益点中心
+
+    // 回家路径（从增益点返回基地）
+    waypoints_to_home_.clear();
+    waypoints_to_home_.push_back({Position2D(2.0, 2.0), 0.3}); // 绕过墙1左侧
+    waypoints_to_home_.push_back({Position2D(0.75, 7.0), 0.3}); // 进入基地
+  }
+
+  void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    // 提取IMU数据
+    // IMU坐标系: X向前, Y向右, Z向下
+    // 地图坐标系: X向右(长边), Y向上(短边)
+
+    // 获取角速度
+    angular_velocity_z_ = msg->angular_velocity.z; // 偏航角速度 (rad/s)
+
+    // 获取朝向（从四元数计算偏航角）
+    double qx = msg->orientation.x;
+    double qy = msg->orientation.y;
+    double qz = msg->orientation.z;
+    double qw = msg->orientation.w;
+
+    // 四元数转偏航角
+    current_heading_ =
+        std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
+
+    // 记录时间用于积分
+    rclcpp::Time current_time = msg->header.stamp;
+    if (last_imu_time_.nanoseconds() > 0) {
+      double dt = (current_time - last_imu_time_).seconds();
+      if (dt > 0 && dt < 0.1) { // 有效时间间隔
+        // 使用角速度积分更新朝向
+        current_heading_ += angular_velocity_z_ * dt;
+
+        // 归一化到 [-pi, pi]
+        while (current_heading_ > M_PI)
+          current_heading_ -= 2 * M_PI;
+        while (current_heading_ < -M_PI)
+          current_heading_ += 2 * M_PI;
+      }
+    }
+    last_imu_time_ = current_time;
+  }
+
+  void stateCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    auto new_state = static_cast<RobotState>(msg->data);
+    if (new_state != current_state_) {
+      current_state_ = new_state;
+      current_waypoint_idx_ = 0; // 重置路径点索引
+
+      RCLCPP_INFO(this->get_logger(), "导航状态切换: %d", msg->data);
+    }
+  }
+
+  void navigationLoop() {
+    // 根据当前状态计算目标位置和速度指令
+    Position2D target_pos;
+    bool has_target = false;
+
+    switch (current_state_) {
+    case RobotState::WAITING:
+      // 等待状态：停止
+      publishNavigationCommand(0, 0);
+      return;
+
+    case RobotState::MOVE_TO_POINT:
+      // 进点状态：沿路径点前往增益点
+      has_target = getNextWaypoint(waypoints_to_point_, target_pos);
+      break;
+
+    case RobotState::SEARCHING:
+    case RobotState::ATTACKING:
+      // 索敌/攻击状态：在增益点内巡逻
+      has_target = patrolInPointRegion(target_pos);
+      break;
+
+    case RobotState::RETURN_HOME:
+      // 回家状态：沿路径点返回基地
+      has_target = getNextWaypoint(waypoints_to_home_, target_pos);
+      break;
+    }
+
+    if (has_target) {
+      // 计算速度指令
+      auto [vx, vy] = calculateVelocity(target_pos);
+      publishNavigationCommand(vx, vy);
+    } else {
+      // 无目标，停止
+      publishNavigationCommand(0, 0);
+    }
+
+    // 发布当前位置
+    publishPosition();
+
+    // 检查是否到达区域
+    checkReachedRegions();
+  }
+
+  bool getNextWaypoint(const std::vector<Waypoint> &waypoints,
+                       Position2D &target) {
+    if (waypoints.empty())
+      return false;
+
+    // 检查是否到达当前路径点
+    if (current_waypoint_idx_ < waypoints.size()) {
+      const auto &wp = waypoints[current_waypoint_idx_];
+      double dist = current_pos_.distanceTo(wp.position);
+
+      if (dist < wp.tolerance) {
+        // 到达当前路径点，前往下一个
+        RCLCPP_INFO(this->get_logger(), "到达路径点 %zu: (%.2f, %.2f)",
+                    current_waypoint_idx_, wp.position.x, wp.position.y);
+        current_waypoint_idx_++;
+      }
+    }
+
+    // 返回当前目标路径点
+    if (current_waypoint_idx_ < waypoints.size()) {
+      target = waypoints[current_waypoint_idx_].position;
+      return true;
+    }
+
+    // 所有路径点已完成，返回最终目标
+    if (!waypoints.empty()) {
+      target = waypoints.back().position;
+      return true;
+    }
+
+    return false;
+  }
+
+  bool patrolInPointRegion(Position2D &target) {
+    // 在增益点区域内巡逻
+    // 简单的巡逻策略：在增益点中心和边界之间移动
+    static bool going_to_center = true;
+
+    Position2D center = point_region_.center();
+    double dist_to_center = current_pos_.distanceTo(center);
+
+    if (going_to_center && dist_to_center < 0.5) {
+      going_to_center = false;
+    } else if (!going_to_center && dist_to_center > 1.5) {
+      going_to_center = true;
+    }
+
+    if (going_to_center) {
+      target = center;
+    } else {
+      // 巡逻到区域边缘
+      target = Position2D(point_region_.x_max - 0.5, point_region_.y_max - 0.5);
+    }
+
+    return true;
+  }
+
+  std::pair<int16_t, int16_t> calculateVelocity(const Position2D &target) {
+    // 计算到目标的方向和距离
+    double dx = target.x - current_pos_.x;
+    double dy = target.y - current_pos_.y;
+    double distance = std::sqrt(dx * dx + dy * dy);
+
+    // 目标方向（地图坐标系）
+    double target_angle = std::atan2(dy, dx);
+
+    // 转换到机器人坐标系（考虑当前朝向）
+    double angle_diff = target_angle - current_heading_;
+
+    // 归一化角度差
+    while (angle_diff > M_PI)
+      angle_diff -= 2 * M_PI;
+    while (angle_diff < -M_PI)
+      angle_diff += 2 * M_PI;
+
+    // 计算速度（前向和侧向）
+    double v_forward = max_speed_ * std::cos(angle_diff); // 前进速度
+    double v_lateral = max_speed_ * std::sin(angle_diff); // 侧向速度
+
+    // 距离减速
+    if (distance < 1.0) {
+      double scale = std::max(0.1, distance);
+      v_forward *= scale;
+      v_lateral *= scale;
+    }
+
+    // 避障检查
+    if (checkObstacleAhead()) {
+      v_forward *= 0.5;
+      v_lateral = max_speed_ * 0.5; // 尝试侧向移动
+    }
+
+    // 转换为控制指令 [-660, 660]
+    // 注意坐标系转换：IMU X向前对应地图Y，IMU Y向右对应地图X
+    int16_t x_cmd =
+        static_cast<int16_t>(v_lateral / max_speed_ * 660); // 左右平动
+    int16_t y_cmd =
+        static_cast<int16_t>(v_forward / max_speed_ * 660); // 前后平动
+
+    // 限制范围
+    x_cmd = std::max<int16_t>(-660, std::min<int16_t>(660, x_cmd));
+    y_cmd = std::max<int16_t>(-660, std::min<int16_t>(660, y_cmd));
+
+    return {x_cmd, y_cmd};
+  }
+
+  bool checkObstacleAhead() {
+    // 简化的避障检查：检查前方是否有墙
+    // 在实际应用中可能需要更复杂的算法
+
+    // 预测前方0.5m的位置
+    double check_dist = robot_radius_ + 0.5;
+    Position2D front_pos;
+    front_pos.x = current_pos_.x + check_dist * std::cos(current_heading_);
+    front_pos.y = current_pos_.y + check_dist * std::sin(current_heading_);
+
+    // 检查是否与墙碰撞
+    if (wall1_.contains(front_pos) || wall2_.contains(front_pos)) {
+      return true;
+    }
+
+    // 检查是否超出地图边界
+    if (front_pos.x < robot_radius_ ||
+        front_pos.x > map_width_ - robot_radius_ ||
+        front_pos.y < robot_radius_ ||
+        front_pos.y > map_height_ - robot_radius_) {
+      return true;
+    }
+
+    return false;
+  }
+
+  void publishNavigationCommand(int16_t x, int16_t y) {
+    auto x_msg = std_msgs::msg::Int32();
+    x_msg.data = x;
+    auto y_msg = std_msgs::msg::Int32();
+    y_msg.data = y;
+
+    nav_x_pub_->publish(x_msg);
+    nav_y_pub_->publish(y_msg);
+  }
+
+  void publishPosition() {
+    auto x_msg = std_msgs::msg::Float32();
+    x_msg.data = static_cast<float>(current_pos_.x);
+    auto y_msg = std_msgs::msg::Float32();
+    y_msg.data = static_cast<float>(current_pos_.y);
+
+    pos_x_pub_->publish(x_msg);
+    pos_y_pub_->publish(y_msg);
+  }
+
+  void checkReachedRegions() {
+    // 检查是否到达增益点
+    bool in_point = point_region_.contains(current_pos_);
+    auto point_msg = std_msgs::msg::Bool();
+    point_msg.data = in_point;
+    reached_point_pub_->publish(point_msg);
+
+    // 检查是否到达基地
+    bool in_home = home_region_.contains(current_pos_);
+    auto home_msg = std_msgs::msg::Bool();
+    home_msg.data = in_home;
+    reached_home_pub_->publish(home_msg);
+  }
+
+  void publishStatus() {
+    RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
+                         "位置: (%.2f, %.2f) | 朝向: %.2f | 路径点: %zu",
+                         current_pos_.x, current_pos_.y, current_heading_,
+                         current_waypoint_idx_);
+  }
+
+  // ===== 成员变量 =====
+  // 参数
+  double map_width_;
+  double map_height_;
+  double robot_radius_;
+  double max_speed_;
+  double nav_freq_;
+
+  // 当前状态
+  RobotState current_state_ = RobotState::WAITING;
+
+  // 位置和朝向
+  Position2D current_pos_;
+  double current_heading_ = 0.0; // 弧度
+
+  // IMU数据
+  double angular_velocity_z_ = 0.0;
+  rclcpp::Time last_imu_time_;
+
+  // 地图区域
+  Rectangle home_region_;
+  Rectangle point_region_;
+  Rectangle wall1_;
+  Rectangle wall2_;
+
+  // 路径点
+  std::vector<Waypoint> waypoints_to_point_;
+  std::vector<Waypoint> waypoints_to_home_;
+  size_t current_waypoint_idx_ = 0;
+
+  // 发布者
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr nav_x_pub_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr nav_y_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pos_x_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pos_y_pub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr reached_point_pub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr reached_home_pub_;
+
+  // 订阅者
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_sub_;
+
+  // 定时器
+  rclcpp::TimerBase::SharedPtr nav_timer_;
+  rclcpp::TimerBase::SharedPtr status_timer_;
+};
+
+} // namespace amadeus_26
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<amadeus_26::NavigationNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

src/uart_transmitter_node.cpp

@@ -9,11 +9,13 @@
  * bytes|1 byte| 1byte| 0xEE |
  */
 
+#include <atomic>
 #include <cstring>
 #include <errno.h>
 #include <fcntl.h>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/int32.hpp>
 #include <termios.h>
 #include <thread>
 #include <unistd.h>
@@ -37,15 +39,6 @@ public:
     this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
     this->declare_parameter("send_frequency", 50.0); // Hz
 
-    // 控制参数
-    this->declare_parameter("x_move", 0);       // 平动左右 [-660, 660]
-    this->declare_parameter("y_move", 0);       // 平动前后 [-660, 660]
-    this->declare_parameter("yaw", 0);          // 云台偏航 [-660, 660]
-    this->declare_parameter("pitch", 0);        // 云台俯仰 [-660, 660]
-    this->declare_parameter("feed", 0);         // 拨弹轮 [-660, 660]
-    this->declare_parameter("left_switch", 0);  // 左拨杆 [1, 3]
-    this->declare_parameter("right_switch", 0); // 右拨杆 [1, 3]
-
     // 获取参数
     serial_port_ = this->get_parameter("serial_port").as_string();
     baudrate_ = this->get_parameter("baudrate").as_int();
@@ -69,6 +62,38 @@ public:
     connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
         "transmitter/connection_status", 10);
 
+    // 创建订阅者 - 订阅中央节点的控制指令
+    control_x_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "control/x_move", 10,
+        std::bind(&UartTransmitterNode::controlXCallback, this,
+                  std::placeholders::_1));
+    control_y_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "control/y_move", 10,
+        std::bind(&UartTransmitterNode::controlYCallback, this,
+                  std::placeholders::_1));
+    control_yaw_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "control/yaw", 10,
+        std::bind(&UartTransmitterNode::controlYawCallback, this,
+                  std::placeholders::_1));
+    control_pitch_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "control/pitch", 10,
+        std::bind(&UartTransmitterNode::controlPitchCallback, this,
+                  std::placeholders::_1));
+    control_feed_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "control/feed", 10,
+        std::bind(&UartTransmitterNode::controlFeedCallback, this,
+                  std::placeholders::_1));
+    control_switch_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "control/switch", 10,
+        std::bind(&UartTransmitterNode::controlSwitchCallback, this,
+                  std::placeholders::_1));
+    control_fire_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "control/fire", 10,
+        std::bind(&UartTransmitterNode::controlFireCallback, this,
+                  std::placeholders::_1));
+
+    RCLCPP_INFO(this->get_logger(), "已订阅控制指令话题");
+
     // 创建定时器 - 发送数据
     auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
     send_timer_ = this->create_wall_timer(
@@ -184,6 +209,29 @@ private:
     }
   }
 
+  // 控制指令回调函数
+  void controlXCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    control_cmd_.x_move = msg->data;
+  }
+  void controlYCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    control_cmd_.y_move = msg->data;
+  }
+  void controlYawCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    control_cmd_.yaw = msg->data;
+  }
+  void controlPitchCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    control_cmd_.pitch = msg->data;
+  }
+  void controlFeedCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    control_cmd_.feed = msg->data;
+  }
+  void controlSwitchCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    control_cmd_.switch_value = msg->data;
+  }
+  void controlFireCallback(const std_msgs::msg::Bool::SharedPtr msg) {
+    control_cmd_.fire = msg->data;
+  }
+
   void sendControlFrame() {
     if (serial_fd_ < 0) {
       RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
@@ -191,20 +239,15 @@ private:
       return;
     }
 
-    // 获取控制参数
+    // 使用订阅的控制指令
-    int16_t x_move =
+    int16_t x_move = control_cmd_.x_move;
-        static_cast<int16_t>(this->get_parameter("x_move").as_int());
+    int16_t y_move = control_cmd_.y_move;
-    int16_t y_move =
+    int16_t yaw = control_cmd_.yaw;
-        static_cast<int16_t>(this->get_parameter("y_move").as_int());
+    int16_t pitch = control_cmd_.pitch;
-    int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
+    int16_t feed = control_cmd_.feed;
-    int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
+    uint8_t switch_value = control_cmd_.switch_value;
-    int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
+    uint8_t left_switch = (switch_value >> 4) & 0x0F;
-    uint8_t left_switch =
+    uint8_t right_switch = switch_value & 0x0F;
-        static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) &
-        0x0F;
-    uint8_t right_switch =
-        static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) &
-        0x0F;
 
     // 构建数据帧
     uint8_t frame[FRAME_LENGTH];
@@ -371,6 +414,17 @@ private:
     connection_status_pub_->publish(msg);
   }
 
+  // 控制指令结构
+  struct ControlCommand {
+    int16_t x_move = 0;
+    int16_t y_move = 0;
+    int16_t yaw = 0;
+    int16_t pitch = 0;
+    int16_t feed = 0;
+    uint8_t switch_value = 0;
+    bool fire = false;
+  };
+
   // 成员变量
   std::string serial_port_;
   int baudrate_;
@@ -381,7 +435,16 @@ private:
   bool running_ = true;
   std::thread receive_thread_;
 
+  ControlCommand control_cmd_;
+
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_x_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_y_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_yaw_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_pitch_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_feed_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr control_switch_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr control_fire_sub_;
   rclcpp::TimerBase::SharedPtr send_timer_;
   rclcpp::TimerBase::SharedPtr status_timer_;
 };