去除部分冗余代码，更新 README

2026-03-25 01:32:55 +08:00
parent 7f4aaf78cd
commit 2cec9bb7e1
4 changed files with 44 additions and 154 deletions
--- a/README.md
+++ b/README.md
@@ -21,10 +21,6 @@
 - MVSDK (工业相机 SDK)
 - 待补充...

-## 目标
-
-感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。
-
 ## 软件架构

 ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为例说明。
@@ -49,9 +45,9 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
 ### IMU 收发节点

 - 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据
- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1`
+- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyIMU`
 - 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据
- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
+- 发布：IMU 数据（加速度、陀螺仪）

 ### 中央节点

@@ -84,6 +80,14 @@ stateDiagram-v2
 ### 导航节点

 - 功能：根据 IMU 数据计算出当前位置，根据状态决定目标位置并发出移动到该位置所需的指令
- 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪
+- 接收：收发节点 -> IMU.加速度, IMU.六轴陀螺仪(AHRS)
        中央节点 -> 状态
 - 发布：导航运动指令
+
+考虑到 IMU 的实际情况，导航建模思路如下：
+- 地图范围：一个 $8 \times 12 m$ 的矩形，机器人运动的范围。以左下角为坐标原点 $(0,0)$，地图对角为 $(12,8)$，坐标单位为米。
+- 障碍区：不可跨越，不可侵入，由地图中两个矩形组成，第一个矩形的两个角点为 $(2.5,1.5) ~ (4.5,8)$，第二个为 $(7.5,0) ~ (9.5,6.5)$。
+- 基地：同样为场地中角点描述的矩形，我方基地为 $(0,6) ~ (1.5,8)$。
+- 增益点：同样为场地中角点描述的矩形，范围为 $(4.5,2.5) ~ (7.5,5.5)$.
+- 机器人：视为一直径 $0.75 m$ 的圆，圆心位置为 IMU 安装位置。开始时，圆心位于 $(1.235,6.265)$。
+- 机器人的朝向和位置计算：IMU 安装于机器人圆心，开始时，IMU X 轴正方向（机器人朝向）与场地 Y 轴负方向相同，Y 轴正方向与场地 X 轴负方向相同，IMU Z 轴则朝向地面。当机器人朝向发生改变时，IMU 轴系的朝向也会相应发生改变，因此需要结合陀螺仪数据才能正确反向解算加速度计带来的加速度信息，从而完成惯性导航。IMU 安装并不完全平整，且机器人运动有颠簸，因此需要将 Z 轴数据结合解算出在二维平面上的加速度和位移。
--- a/launch/uart_transmitter.launch.py
+++ b/launch/uart_transmitter.launch.py
@@ -1,102 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-
-
-def generate_launch_description():
-    # 声明启动参数
-    serial_port_arg = DeclareLaunchArgument(
-        'serial_port',
-        default_value='/dev/ttyCH340',
-        description='CH340 串口设备路径'
-    )
-    
-    baudrate_arg = DeclareLaunchArgument(
-        'baudrate',
-        default_value='115200',
-        description='串口波特率'
-    )
-    
-    send_frequency_arg = DeclareLaunchArgument(
-        'send_frequency',
-        default_value='10.0',
-        description='发送频率 (Hz)'
-    )
-    
-    # 控制参数
-    x_move_arg = DeclareLaunchArgument(
-        'x_move',
-        default_value='0',
-        description='平动左右 [-660, 660]'
-    )
-    
-    y_move_arg = DeclareLaunchArgument(
-        'y_move',
-        default_value='0',
-        description='平动前后 [-660, 660]'
-    )
-    
-    yaw_arg = DeclareLaunchArgument(
-        'yaw',
-        default_value='0',
-        description='云台偏航 [-660, 660]'
-    )
-    
-    pitch_arg = DeclareLaunchArgument(
-        'pitch',
-        default_value='0',
-        description='云台俯仰 [-660, 660]'
-    )
-    
-    feed_arg = DeclareLaunchArgument(
-        'feed',
-        default_value='0',
-        description='拨弹轮 [-660, 660]'
-    )
-    
-    left_switch_arg = DeclareLaunchArgument(
-        'left_switch',
-        default_value='3',
-        description='左拨杆 [1, 3]'
-    )
-    
-    right_switch_arg = DeclareLaunchArgument(
-        'right_switch',
-        default_value='3',
-        description='右拨杆 [1, 3]'
-    )
-
-    # 创建节点
-    uart_transmitter_node = Node(
-        package='amadeus_26',
-        executable='uart_transmitter_node',
-        name='uart_transmitter_node',
-        output='screen',
-        parameters=[{
-            'serial_port': LaunchConfiguration('serial_port'),
-            'baudrate': LaunchConfiguration('baudrate'),
-            'send_frequency': LaunchConfiguration('send_frequency'),
-            'x_move': LaunchConfiguration('x_move'),
-            'y_move': LaunchConfiguration('y_move'),
-            'yaw': LaunchConfiguration('yaw'),
-            'pitch': LaunchConfiguration('pitch'),
-            'feed': LaunchConfiguration('feed'),
-            'left_switch': LaunchConfiguration('left_switch'),
-            'right_switch': LaunchConfiguration('right_switch'),
-        }],
-    )
-
-    return LaunchDescription([
-        serial_port_arg,
-        baudrate_arg,
-        send_frequency_arg,
-        x_move_arg,
-        y_move_arg,
-        yaw_arg,
-        pitch_arg,
-        feed_arg,
-        left_switch_arg,
-        right_switch_arg,
-        uart_transmitter_node,
-    ])
--- a/src/imu_receiver_node.cpp
+++ b/src/imu_receiver_node.cpp
@@ -18,6 +18,7 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <std_msgs/msg/bool.hpp>
@@ -104,20 +105,20 @@ public:
    this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
    this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
    this->declare_parameter("verbose", true); // 是否输出详细日志
-    this->declare_parameter("enable_crc", false); // 是否启用CRC校验（默认关闭）
+    this->declare_parameter("enable_crc", true); // 是否启用CRC校验（默认关闭）

    // 获取参数
    serial_port_ = this->get_parameter("serial_port").as_string();
    baudrate_ = this->get_parameter("baudrate").as_int();
    enable_crc_ = this->get_parameter("enable_crc").as_bool();

-    RCLCPP_INFO(this->get_logger(), "=================================");
-    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点启动 (高实时性版本)");
+    RCLCPP_INFO(this->get_logger(), "---------------------------------");
+    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点");
    RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
    RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
    RCLCPP_INFO(this->get_logger(), "CRC校验: %s",
                enable_crc_ ? "启用" : "禁用");
-    RCLCPP_INFO(this->get_logger(), "=================================");
+    RCLCPP_INFO(this->get_logger(), "---------------------------------");

    // 创建发布者
    ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
@@ -251,9 +252,9 @@ private:
    return crc16;
  }

-  // 高实时性接收循环 - 批量读取，无阻塞
+  // 接收循环
  void receiveLoop() {
-    // 使用双缓冲区策略：一个用于接收，一个用于处理
+    // 双缓冲区：一个用于接收，一个用于处理
    std::vector<uint8_t> rx_buffer;
    rx_buffer.reserve(SERIAL_READ_BUF_SIZE * 2);

@@ -369,7 +370,7 @@ private:
        // 读取错误
        RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno));
      }
-      // n == 0 或 EAGAIN: 没有数据可读，继续循环（无sleep，保证实时性）
+      // n == 0 或 EAGAIN: 没有数据可读，继续循环
    }
  }

@@ -481,18 +482,11 @@ private:

    ahrs_pub_->publish(imu_msg);

-    // 根据 verbose 参数输出日志
-    bool verbose = this->get_parameter("verbose").as_bool();
-    if (verbose) {
+    // 输出 AHRS 日志
    RCLCPP_INFO(
        this->get_logger(),
-          "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us",
-          roll, pitch, heading, timestamp_us);
-    } else {
-      RCLCPP_DEBUG(this->get_logger(),
-                   "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll,
-                   pitch, heading);
-    }
+        "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us", roll,
+        pitch, heading, timestamp_us);
  }

  // 解析机体系加速度数据 (0x62)
@@ -500,7 +494,7 @@ private:
    if (len < 16)
      return;

-    // 解析机体系加速度 (m/s²)
+    // 解析机体系加速度 (m/s^2)
    float acc_x = *reinterpret_cast<const float *>(data);
    float acc_y = *reinterpret_cast<const float *>(data + 4);
    float acc_z = *reinterpret_cast<const float *>(data + 8);
@@ -526,17 +520,9 @@ private:
    g_msg.data = g_force;
    g_force_pub_->publish(g_msg);

-    // 根据 verbose 参数输出日志
-    bool verbose = this->get_parameter("verbose").as_bool();
-    if (verbose) {
-      RCLCPP_INFO(this->get_logger(),
-                  "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x,
-                  acc_y, acc_z, g_force);
-    } else {
-      RCLCPP_DEBUG(this->get_logger(),
-                   "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x,
-                   acc_y, acc_z, g_force);
-    }
+    // 输出日志
+    RCLCPP_INFO(this->get_logger(), "ACC - X: %.3f, Y: %.3f, Z: %.3f m/s^2",
+                acc_x, acc_y, acc_z);
  }

  void publishStatus() {
@@ -544,17 +530,15 @@ private:
    msg.data = is_connected_ && (serial_fd_ >= 0);
    connection_status_pub_->publish(msg);

-    if (is_connected_) {
-      RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
-                           "IMU 接收正常 - AHRS: %zu, BodyAcc: %zu",
-                           ahrs_count_.load(), body_acc_count_.load());
+    if (!is_connected_) {
+      RCLCPP_WARN(this->get_logger(), "IMU 接收不正常");
    }
  }

  // 成员变量
  std::string serial_port_;
  int baudrate_;
-  bool enable_crc_ = false; // CRC校验开关
+  bool enable_crc_ = true; // CRC校验开关

  int serial_fd_ = -1;
  bool is_connected_ = false;
--- a/src/uart_transmitter_node.cpp
+++ b/src/uart_transmitter_node.cpp
@@ -38,13 +38,16 @@ public:
    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]
+    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", 100); // 云台俯仰 [-660, 660] 负为向下
+    this->declare_parameter("feed",
+                            660); // 拨弹轮 [-660, 660]， 660 开火，0 无动作
+    this->declare_parameter("left_switch",
+                            2); // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
+    this->declare_parameter("right_switch",
+                            3); // 右拨杆 [1, 3] 2 小陀螺 3 关闭

    // 获取参数
    serial_port_ = this->get_parameter("serial_port").as_string();
@@ -240,9 +243,10 @@ private:
    // CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身)
    // 当前idx=13, 数据长度=11 (从frame[2]到frame[12])
    // uint8_t crc_value = calculateCRC8(frame + 2, 11);
-    frame[idx++] = 0xCC; // crc_value
+    // 帧尾 1，接收没有写 CRC8，因此固定为 CC
+    frame[idx++] = 0xCC;

-    // 帧尾
+    // 帧尾 2
    frame[idx++] = FRAME_TAIL;

    // 调试输出：打印完整帧内容