添加遥控器复位+控制

.vscode/settings.json

@@ -0,0 +1,8 @@
+{
+    "files.associations": {
+        "array": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "chrono": "cpp"
+    }
+}

1.txt

@@ -0,0 +1,18 @@
+ros2 topic pub --rate 10 /LF/motor_cmds rc_msgs/msg/MotorCmds "motor_cmd_0:
+  tau: 0.0
+  vw: 0.0
+  pos: 0.0
+  kp: 0.0
+  kd: 0.0
+motor_cmd_1:
+  tau: 0.0
+  vw: 0.0
+  pos: 0.0
+  kp: 0.0
+  kd: 0.0
+motor_cmd_2:
+  tau: 0.0
+  vw: 0.0
+  pos: 0.0
+  kp: 0.0
+  kd: 0.0"

README.md

@@ -1,3 +1,19 @@
 # CM_DOG
 
-A simple ros2 program for legged robot . Robocon2025
+A simple ROS2 program for a legged robot. Developed for Robocon2025.
+
+## 硬件配置
+
+- **关节电机**: Unitree GO-8010-6 (12个)
+- **IMU传感器**: 轮趣科技 N100陀螺仪
+- **通信接口**: USB转思路RS485
+
+## 软件依赖
+
+- Ros2 humble
+
+## 使用说明
+
+1. 克隆仓库：
+
+2. 构建项目：

build.sh

@@ -0,0 +1,2 @@
+source install/setup.bash 
+colcon build

src/fdilink_ahrs_ROS2/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.5)
+project(fdilink_ahrs)
+
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(Eigen3 REQUIRED)
+set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(serial REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+include_directories(
+  include
+  src
+  ${catkin_INCLUDE_DIRS}
+  ${Eigen3_INCLUDE_DIRS} 
+)
+
+
+add_executable(ahrs_driver_node src/ahrs_driver.cpp src/crc_table.cpp)
+ament_target_dependencies(ahrs_driver_node rclcpp rclpy  std_msgs  sensor_msgs  serial tf2_ros tf2 nav_msgs geometry_msgs)
+
+
+add_executable(imu_tf_node src/imu_tf.cpp)
+ament_target_dependencies(imu_tf_node rclcpp rclpy std_msgs sensor_msgs serial tf2_ros tf2 tf2_geometry_msgs)
+
+# find_package(Boost 1.55.0 REQUIRED COMPONENTS system filesystem)
+# include_directories(ahrs_driver_node ${Boost_INCLOUDE_DIRS})
+# link_directories(ahrs_driver_node ${Boost_LIBRARY_DIRS})
+# target_link_libraries(ahrs_driver_node ${Boost_LIBRSRIES})
+
+install(TARGETS
+  ahrs_driver_node
+  imu_tf_node
+
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch 
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/fdilink_ahrs_ROS2/include/ahrs_driver.h

@@ -0,0 +1,111 @@
+#ifndef BASE_DRIVER_H_
+#define BASE_DRIVER_H_
+
+
+#include <inttypes.h>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <iostream>
+#include <unistd.h>
+#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
+#include <math.h>
+#include <fstream>
+#include <fdilink_data_struct.h>
+//#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <geometry_msgs/msg/pose2_d.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <string>
+//#include <ros/package.h>
+#include <ament_index_cpp/get_package_prefix.hpp>
+#include <crc_table.h>
+#include <Eigen/Eigen>
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+//#include <boost/thread.h>
+
+using namespace std;
+using namespace Eigen;
+namespace FDILink
+{
+#define FRAME_HEAD 0xfc
+#define FRAME_END 0xfd
+#define TYPE_IMU 0x40
+#define TYPE_AHRS 0x41
+#define TYPE_INSGPS 0x42
+#define TYPE_GEODETIC_POS 0x5c
+#define TYPE_GROUND 0xf0
+
+#define IMU_LEN  0x38   //56
+#define AHRS_LEN 0x30   //48
+#define INSGPS_LEN 0x48 //80
+#define GEODETIC_POS_LEN 0x20 //32
+#define PI 3.141592653589793
+#define DEG_TO_RAD 0.017453292519943295
+
+
+class ahrsBringup : public rclcpp::Node
+{
+public:
+  ahrsBringup();
+  ~ahrsBringup();
+  void processLoop();
+  bool checkCS8(int len);
+  bool checkCS16(int len);
+  void checkSN(int type);
+  void magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz);
+
+private:
+  bool if_debug_;
+  //sum info
+  int sn_lost_ = 0;
+  int crc_error_ = 0;
+  uint8_t read_sn_ = 0;
+  bool frist_sn_;
+  int device_type_ = 1;
+
+  //serial
+  serial::Serial serial_; //声明串口对象
+  std::string serial_port_;
+  int serial_baud_;
+  int serial_timeout_;
+  //data
+  FDILink::imu_frame_read  imu_frame_;
+  FDILink::ahrs_frame_read ahrs_frame_;
+  FDILink::insgps_frame_read insgps_frame_;
+  //FDILink::lanlon_frame_read latlon_frame_;
+  FDILink::Geodetic_Position_frame_read Geodetic_Position_frame_;
+  //frame name
+  //std::string imu_frame_id="gyro_link";
+  std::string imu_frame_id_;
+  std::string insgps_frame_id_;
+  std::string latlon_frame_id_;
+  //topic
+  std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
+  std::string latlon_topic="latlon";
+  std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
+  std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
+
+
+  //Publisher
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr  imu_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr mag_pose_pub_;
+
+  rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
+  
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr  twist_pub_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr NED_odom_pub_;
+
+
+
+}; //ahrsBringup
+} // namespace FDILink
+
+#endif

src/fdilink_ahrs_ROS2/include/crc_table.h

@@ -0,0 +1,10 @@
+#ifndef CRC_TABLE_H
+#define CRC_TABLE_H
+
+#include <stdint.h>
+
+uint8_t CRC8_Table(uint8_t* p, uint8_t counter);
+uint16_t CRC16_Table(uint8_t *p, uint8_t counter);
+uint32_t CRC32_Table(uint8_t *p, uint8_t counter);
+
+#endif // CRC_TABLE_H

src/fdilink_ahrs_ROS2/include/fdilink_data_struct.h

@@ -0,0 +1,189 @@
+#ifndef FDILINK_DATA_STRUCT_H_
+#define FDILINK_DATA_STRUCT_H_
+
+#include <iostream>
+namespace FDILink{
+#pragma pack(1)
+struct fdilink_header
+{
+	uint8_t  header_start;
+	uint8_t  data_type;
+	uint8_t  data_size;
+	uint8_t  serial_num;
+	uint8_t  header_crc8;
+    uint8_t  header_crc16_h;
+	uint8_t  header_crc16_l;
+};
+#pragma pack()
+
+#pragma pack(1)
+struct IMUData_Packet_t
+{
+	float gyroscope_x;          //unit: rad/s
+	float gyroscope_y;          //unit: rad/s
+	float gyroscope_z;          //unit: rad/s
+	float accelerometer_x;      //m/s^2
+	float accelerometer_y;      //m/s^2
+	float accelerometer_z;      //m/s^2
+	float magnetometer_x;       //mG
+	float magnetometer_y;       //mG
+	float magnetometer_z;       //mG
+	float imu_temperature;      //C
+	float Pressure;             //Pa
+	float pressure_temperature; //C
+	int64_t Timestamp;          //us
+};
+#pragma pack()
+
+struct AHRSData_Packet_t
+{
+	float RollSpeed;   //unit: rad/s
+	float PitchSpeed;  //unit: rad/s
+	float HeadingSpeed;//unit: rad/s
+	float Roll;        //unit: rad
+	float Pitch;       //unit: rad
+	float Heading;     //unit: rad
+	float Qw;//w          //Quaternion
+	float Qx;//x
+	float Qy;//y
+	float Qz;//z
+	int64_t Timestamp; //unit: us
+};
+#pragma pack(1)
+struct INSGPSData_Packet_t
+{
+	float BodyVelocity_X;       
+	float BodyVelocity_Y;       
+	float BodyVelocity_Z;       
+	float BodyAcceleration_X;   
+	float BodyAcceleration_Y;   
+	float BodyAcceleration_Z;   
+	float Location_North;      
+	float Location_East;       
+	float Location_Down;
+	float Velocity_North;
+	float Velocity_East;
+	float Velocity_Down;
+	float Acceleration_North;
+	float Acceleration_East;
+	float Acceleration_Down;
+	float Pressure_Altitude;
+	int64_t Timestamp;
+};
+#pragma pack()
+
+#pragma pack(1)
+struct Geodetic_Position_Packet_t
+{
+	double Latitude;       
+	double Longitude;       
+	double Height;
+	float hAcc;
+	float vAcc;      
+};
+#pragma pack()
+
+//for IMU=========================
+#pragma pack(1)
+struct read_imu_struct{
+  fdilink_header     header;    //7                
+  union data
+  {
+	IMUData_Packet_t   data_pack; //56
+	uint8_t            data_buff[56]; //56
+  }data;
+  uint8_t            frame_end; //1                  
+};            
+
+struct read_imu_tmp{
+  uint8_t frame_header[7];
+  uint8_t read_msg[57];
+};                           
+
+union imu_frame_read{
+  struct read_imu_struct frame;
+  read_imu_tmp read_buf;
+  uint8_t read_tmp[64];
+};
+#pragma pack()
+//for IMU------------------------
+
+//for AHRS=========================
+#pragma pack(1)
+struct read_ahrs_struct{
+  fdilink_header     header;    //7                
+  union data
+  {
+	AHRSData_Packet_t  data_pack; //48
+	uint8_t            data_buff[48]; //48
+  }data;
+  uint8_t            frame_end; //1                  
+};       
+
+ 
+struct read_ahrs_tmp{
+  uint8_t frame_header[7];
+  uint8_t read_msg[49];
+};                           
+
+union ahrs_frame_read{
+  struct read_ahrs_struct frame;
+  read_ahrs_tmp read_buf;
+  uint8_t read_tmp[56];
+};
+#pragma pack()
+//for AHRS------------------------
+
+//for INSGPS=========================
+#pragma pack(1)
+struct read_insgps_struct{
+  fdilink_header     header;    //7                
+  union data
+  {
+	INSGPSData_Packet_t  data_pack; //72
+	uint8_t              data_buff[72]; //72
+  }data;
+  uint8_t            frame_end; //1                  
+};   
+
+                           
+struct read_insgps_tmp{
+  uint8_t frame_header[7];
+  uint8_t read_msg[73];
+};                          
+
+union insgps_frame_read{
+  struct read_insgps_struct frame;
+  read_insgps_tmp read_buf;
+  uint8_t read_tmp[80];
+};
+#pragma pack()
+//for INSGPS------------------------
+
+//for Geodetic_Position=========================
+#pragma pack(1)
+struct read_Geodetic_Position_struct{
+  fdilink_header     header;    //7                
+  union data
+  {
+	Geodetic_Position_Packet_t  data_pack; //40
+	uint8_t                     data_buff[32]; //40
+  }data;
+  uint8_t            frame_end; //1                  
+};   
+                           
+struct read_Geodetic_Position_tmp{
+  uint8_t frame_header[7];
+  uint8_t read_msg[33];
+};                          
+
+union Geodetic_Position_frame_read{
+  struct read_Geodetic_Position_struct frame;
+  read_Geodetic_Position_tmp read_buf;
+  uint8_t read_tmp[40];
+};
+ 
+#pragma pack()
+
+}//namespace FDILink
+#endif//FDILINK_DATA_STRUCT_H_

src/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py

@@ -0,0 +1,37 @@
+import os
+from pathlib import Path
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+# bringup_dir = get_package_share_directory('fdilink_ahrs')
+# launch_dir = os.path.join(bringup_dir, 'launch')
+# imu_tf = IncludeLaunchDescription(
+#         PythonLaunchDescriptionSource(os.path.join(launch_dir, 'imu_tf.launch.py')),
+# )
+def generate_launch_description():
+    ahrs_driver=Node(
+        package="fdilink_ahrs",
+        executable="ahrs_driver_node",
+        parameters=[{'if_debug_': False,
+            # 'serial_port_':'/dev/wheeltec_FDI_IMU_GNSS',
+            'serial_port_':'/dev/ttyUSB0',
+            'serial_baud_':921600,
+            'imu_topic':'/imu',
+            'imu_frame_id_':'gyro_link',
+            'mag_pose_2d_topic':'/mag_pose_2d',
+            'Magnetic_topic':'/magnetic',
+            'Euler_angles_topic':'/euler_angles',
+            'gps_topic':'/gps/fix',
+            'twist_topic':'/system_speed',
+            'NED_odom_topic':'/NED_odometry',
+            'device_type_':1}],
+        output="screen"
+    )
+
+    launch_description =LaunchDescription()
+    launch_description.add_action(ahrs_driver)
+#    launch_description.add_action(imu_tf)
+    return launch_description

src/fdilink_ahrs_ROS2/launch/imu_tf.launch.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    imu_tf=Node(
+        package="fdilink_ahrs",
+        #node_executable="imu_tf_node",
+        parameters=[{'imu_topic':'/imu',
+        'world_frame_id':'/world',
+        'imu_frame_id':'/gyro_link',
+        'position_x':1,
+        'position_y':1,
+        'position_z':1,
+        }],
+    )
+
+    launch_description =LaunchDescription([imu_tf])
+    return launch_description

src/fdilink_ahrs_ROS2/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fdilink_ahrs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="noah@todo.todo">noah</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/fdilink_ahrs_ROS2/src/ahrs_driver.cpp

@@ -0,0 +1,675 @@
+#include <ahrs_driver.h>
+//#include <Eigen/Eigen>
+rclcpp::Node::SharedPtr nh_=nullptr;
+
+
+namespace FDILink
+{
+ahrsBringup::ahrsBringup()
+: rclcpp::Node ("ahrs_bringup")
+{
+  //topic_name & frame_id  加载参数服务器
+  this->declare_parameter("if_debug_",false);
+  this->get_parameter("if_debug_", if_debug_);
+
+  this->declare_parameter<std::int8_t>("device_type_",1);
+  this->get_parameter("device_type_",  device_type_);
+
+  this->declare_parameter<std::string>("imu_topic","/imu");
+  this->get_parameter("imu_topic",  imu_topic);
+
+  this->declare_parameter<std::string>("imu_frame_id_","gyro_link");
+  this->get_parameter("imu_frame_id_",   imu_frame_id_);
+
+  this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
+  this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
+
+  this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
+  this->get_parameter("Euler_angles_topic", Euler_angles_topic);
+
+  this->declare_parameter<std::string>("gps_topic","/gps/fix");
+  this->get_parameter("gps_topic", gps_topic);
+
+
+  this->declare_parameter<std::string>("Magnetic_topic","/magnetic");
+  this->get_parameter("Magnetic_topic", Magnetic_topic);
+
+  this->declare_parameter<std::string>("twist_topic","/system_speed");
+  this->get_parameter("twist_topic", twist_topic);
+
+  this->declare_parameter<std::string>("NED_odom_topic","/NED_odometry");
+  this->get_parameter("NED_odom_topic", NED_odom_topic);
+
+  this->declare_parameter<std::string>("serial_port_","/dev/fdilink_ahrs");
+  this->get_parameter("serial_port_", serial_port_);
+
+  this->declare_parameter<std::int64_t>("serial_baud_",921600);
+  this->get_parameter("serial_baud_", serial_baud_);  
+
+  //pravite_nh.param("debug",     if_debug_,  false);
+  //pravite_nh.param("device_type", device_type_, 1); // default: single imu
+  // pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
+  // pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
+  // pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
+  // pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
+  // pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
+ 
+  //serial                                                 
+  //pravite_nh.param("port", serial_port_, std::string("/dev/ttyTHS1")); 
+  //pravite_nh.param("baud", serial_baud_, 115200);
+  
+  //publisher
+  imu_pub_ = create_publisher<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10);
+  gps_pub_ = create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic.c_str(), 10);
+
+  mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
+
+  Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
+  Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
+ 
+  twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
+  NED_odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(NED_odom_topic.c_str(), 10);
+
+
+  //setp up serial  设置串口参数并打开串口
+  try
+  {
+    serial_.setPort(serial_port_);
+    serial_.setBaudrate(serial_baud_);
+    serial_.setFlowcontrol(serial::flowcontrol_none);
+    serial_.setParity(serial::parity_none); //default is parity_none
+    serial_.setStopbits(serial::stopbits_one);
+    serial_.setBytesize(serial::eightbits);
+    serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
+    serial_.setTimeout(time_out);
+    serial_.open();
+  }
+  catch (serial::IOException &e)  // 抓取异常
+  {
+    RCLCPP_ERROR(this->get_logger(),"Unable to open port ");
+    exit(0);
+  }
+  if (serial_.isOpen())
+  {
+    RCLCPP_INFO(this->get_logger(),"Serial Port initialized");
+  }
+  else
+  {
+    RCLCPP_ERROR(this->get_logger(),"Unable to initial Serial port ");
+    exit(0);
+  }
+  processLoop();
+}
+
+ahrsBringup::~ahrsBringup()  // 析构函数关闭串口通道
+{
+  if (serial_.isOpen())
+    serial_.close();
+}
+
+void ahrsBringup::processLoop()  // 数据处理过程
+{
+  RCLCPP_INFO(this->get_logger(),"ahrsBringup::processLoop: start");
+  while (rclcpp::ok())
+  {
+    if (!serial_.isOpen())
+    {
+      RCLCPP_WARN(this->get_logger(),"serial unopen");
+    }
+    //check head start   检查起始 数据帧头
+    uint8_t check_head[1] = {0xff};  
+    size_t head_s = serial_.read(check_head, 1);
+    if (if_debug_){
+      if (head_s != 1)
+      {
+        RCLCPP_ERROR(this->get_logger(),"Read serial port time out! can't read pack head.");
+      }
+      std::cout << std::endl;
+      std::cout << "check_head: " << std::hex << (int)check_head[0] << std::dec << std::endl;
+    }
+    if (check_head[0] != FRAME_HEAD)
+    {
+      continue;
+    }
+    //check head type   检查数据类型
+    uint8_t head_type[1] = {0xff};
+    size_t type_s = serial_.read(head_type, 1);
+    if (if_debug_){
+      std::cout << "head_type:  " << std::hex << (int)head_type[0] << std::dec << std::endl;
+    }
+    if (head_type[0] != TYPE_IMU && head_type[0] != TYPE_AHRS && head_type[0] != TYPE_INSGPS && head_type[0] != TYPE_GEODETIC_POS && head_type[0] != 0x50 && head_type[0] != TYPE_GROUND&& head_type[0] != 0xff)
+    {
+      RCLCPP_WARN(this->get_logger(),"head_type error: %02X",head_type[0]);
+      continue;
+    }
+    //check head length    检查对应数据类型的长度是否符合
+    uint8_t check_len[1] = {0xff};
+    size_t len_s = serial_.read(check_len, 1);
+    if (if_debug_){
+      std::cout << "check_len: "<< std::dec << (int)check_len[0]  << std::endl;
+    }
+    if (head_type[0] == TYPE_IMU && check_len[0] != IMU_LEN)
+    {
+      RCLCPP_WARN(this->get_logger(),"head_len error (imu)");
+      continue;
+    }else if (head_type[0] == TYPE_AHRS && check_len[0] != AHRS_LEN)
+    {
+      RCLCPP_WARN(this->get_logger(),"head_len error (ahrs)");
+      continue;
+    }else if (head_type[0] == TYPE_INSGPS && check_len[0] != INSGPS_LEN)
+    {
+      RCLCPP_WARN(this->get_logger(),"head_len error (insgps)");
+      continue;
+    }else if (head_type[0] == TYPE_GEODETIC_POS && check_len[0] != GEODETIC_POS_LEN)
+    {
+      RCLCPP_WARN(this->get_logger(),"head_len error (GEODETIC_POS)");
+      continue;
+    }
+    else if (head_type[0] == TYPE_GROUND || head_type[0] == 0x50) // 未知数据，防止记录失败
+    {
+      uint8_t ground_sn[1];
+      size_t ground_sn_s = serial_.read(ground_sn, 1);
+      if (++read_sn_ != ground_sn[0])
+      {
+        if ( ground_sn[0] < read_sn_)
+        {
+          if(if_debug_){
+            RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+          }
+          sn_lost_ += 256 - (int)(read_sn_ - ground_sn[0]);
+          read_sn_ = ground_sn[0];
+          // continue;
+        }
+        else
+        {
+          if(if_debug_){
+            RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+          }
+          sn_lost_ += (int)(ground_sn[0] - read_sn_);
+          read_sn_ = ground_sn[0];
+          // continue;
+        }
+      }
+      uint8_t ground_ignore[500];
+      size_t ground_ignore_s = serial_.read(ground_ignore, (check_len[0]+4));
+      continue;
+    }
+    //read head sn 
+    uint8_t check_sn[1] = {0xff};
+    size_t sn_s = serial_.read(check_sn, 1);
+    uint8_t head_crc8[1] = {0xff};
+    size_t crc8_s = serial_.read(head_crc8, 1);
+    uint8_t head_crc16_H[1] = {0xff};
+    uint8_t head_crc16_L[1] = {0xff};
+    size_t crc16_H_s = serial_.read(head_crc16_H, 1);
+    size_t crc16_L_s = serial_.read(head_crc16_L, 1);
+    if (if_debug_){
+      std::cout << "check_sn: "     << std::hex << (int)check_sn[0]     << std::dec << std::endl;
+      std::cout << "head_crc8: "    << std::hex << (int)head_crc8[0]    << std::dec << std::endl;
+      std::cout << "head_crc16_H: " << std::hex << (int)head_crc16_H[0] << std::dec << std::endl;
+      std::cout << "head_crc16_L: " << std::hex << (int)head_crc16_L[0] << std::dec << std::endl;
+    }
+    // put header & check crc8 & count sn lost
+    // check crc8 进行crc8数据校验
+    if (head_type[0] == TYPE_IMU)
+    {
+      imu_frame_.frame.header.header_start   = check_head[0];
+      imu_frame_.frame.header.data_type      = head_type[0];
+      imu_frame_.frame.header.data_size      = check_len[0];
+      imu_frame_.frame.header.serial_num     = check_sn[0];
+      imu_frame_.frame.header.header_crc8    = head_crc8[0];
+      imu_frame_.frame.header.header_crc16_h = head_crc16_H[0];
+      imu_frame_.frame.header.header_crc16_l = head_crc16_L[0];
+      uint8_t CRC8 = CRC8_Table(imu_frame_.read_buf.frame_header, 4);
+      if (CRC8 != imu_frame_.frame.header.header_crc8)
+      {
+        RCLCPP_WARN(this->get_logger(),"header_crc8 error");
+        continue;
+      }
+      if(!frist_sn_){
+        read_sn_  = imu_frame_.frame.header.serial_num - 1;
+        frist_sn_ = true;
+      }
+      //check sn 
+      ahrsBringup::checkSN(TYPE_IMU);
+    }
+    else if (head_type[0] == TYPE_AHRS)
+    {
+      ahrs_frame_.frame.header.header_start   = check_head[0];
+      ahrs_frame_.frame.header.data_type      = head_type[0];
+      ahrs_frame_.frame.header.data_size      = check_len[0];
+      ahrs_frame_.frame.header.serial_num     = check_sn[0];
+      ahrs_frame_.frame.header.header_crc8    = head_crc8[0];
+      ahrs_frame_.frame.header.header_crc16_h = head_crc16_H[0];
+      ahrs_frame_.frame.header.header_crc16_l = head_crc16_L[0];
+      uint8_t CRC8 = CRC8_Table(ahrs_frame_.read_buf.frame_header, 4);
+      if (CRC8 != ahrs_frame_.frame.header.header_crc8)
+      {
+        RCLCPP_WARN(this->get_logger(),"header_crc8 error");
+        continue;
+      }
+      if(!frist_sn_){
+        read_sn_  = ahrs_frame_.frame.header.serial_num - 1;
+        frist_sn_ = true;
+      }
+      //check sn 
+      ahrsBringup::checkSN(TYPE_AHRS);
+    }
+    else if (head_type[0] == TYPE_INSGPS)
+    {
+      insgps_frame_.frame.header.header_start   = check_head[0];
+      insgps_frame_.frame.header.data_type      = head_type[0];
+      insgps_frame_.frame.header.data_size      = check_len[0];
+      insgps_frame_.frame.header.serial_num     = check_sn[0];
+      insgps_frame_.frame.header.header_crc8    = head_crc8[0];
+      insgps_frame_.frame.header.header_crc16_h = head_crc16_H[0];
+      insgps_frame_.frame.header.header_crc16_l = head_crc16_L[0];
+      uint8_t CRC8 = CRC8_Table(insgps_frame_.read_buf.frame_header, 4);
+      if (CRC8 != insgps_frame_.frame.header.header_crc8)
+      {
+        RCLCPP_WARN(this->get_logger(),"header_crc8 error");
+        continue;
+      }
+      else if(if_debug_)
+      {
+        std::cout << "header_crc8 matched." << std::endl;
+      }
+      
+      ahrsBringup::checkSN(TYPE_INSGPS);
+    }
+    else if (head_type[0] == TYPE_GEODETIC_POS)
+    {
+      Geodetic_Position_frame_.frame.header.header_start   = check_head[0];
+      Geodetic_Position_frame_.frame.header.data_type      = head_type[0];
+      Geodetic_Position_frame_.frame.header.data_size      = check_len[0];
+      Geodetic_Position_frame_.frame.header.serial_num     = check_sn[0];
+      Geodetic_Position_frame_.frame.header.header_crc8    = head_crc8[0];
+      Geodetic_Position_frame_.frame.header.header_crc16_h = head_crc16_H[0];
+      Geodetic_Position_frame_.frame.header.header_crc16_l = head_crc16_L[0];
+      uint8_t CRC8 = CRC8_Table(Geodetic_Position_frame_.read_buf.frame_header, 4);
+      if (CRC8 != Geodetic_Position_frame_.frame.header.header_crc8)
+      {
+        RCLCPP_WARN(this->get_logger(),"header_crc8 error");
+        continue;
+      }
+      if(!frist_sn_){
+        read_sn_  = Geodetic_Position_frame_.frame.header.serial_num - 1;
+        frist_sn_ = true;
+      }
+      
+      ahrsBringup::checkSN(TYPE_GEODETIC_POS);
+    }
+    // check crc16 进行crc16数据校验
+    if (head_type[0] == TYPE_IMU)
+    {
+      uint16_t head_crc16_l = imu_frame_.frame.header.header_crc16_l;
+      uint16_t head_crc16_h = imu_frame_.frame.header.header_crc16_h;
+      uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
+      size_t data_s = serial_.read(imu_frame_.read_buf.read_msg, (IMU_LEN + 1)); //48+1
+      // if (if_debug_){
+      //   for (size_t i = 0; i < (IMU_LEN + 1); i++)
+      //   {
+      //     std::cout << std::hex << (int)imu_frame_.read_buf.read_msg[i] << " ";
+      //   }
+      //   std::cout << std::dec << std::endl;
+      // }
+      uint16_t CRC16 = CRC16_Table(imu_frame_.frame.data.data_buff, IMU_LEN);
+      if (if_debug_)
+      {          
+        std::cout << "CRC16:        " << std::hex << (int)CRC16 << std::dec << std::endl;
+        std::cout << "head_crc16:   " << std::hex << (int)head_crc16 << std::dec << std::endl;
+        std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
+        std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
+        bool if_right = ((int)head_crc16 == (int)CRC16);
+        std::cout << "if_right: " << if_right << std::endl;
+      }
+      
+      if (head_crc16 != CRC16)
+      {
+        RCLCPP_WARN(this->get_logger(),"check crc16 faild(imu).");
+        continue;
+      }
+      else if(imu_frame_.frame.frame_end != FRAME_END)
+      {
+        RCLCPP_WARN(this->get_logger(),"check frame end.");
+        continue;
+      }
+      
+    }
+    else if (head_type[0] == TYPE_AHRS)
+    {
+      uint16_t head_crc16_l = ahrs_frame_.frame.header.header_crc16_l;
+      uint16_t head_crc16_h = ahrs_frame_.frame.header.header_crc16_h;
+      uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
+      size_t data_s = serial_.read(ahrs_frame_.read_buf.read_msg, (AHRS_LEN + 1)); //48+1
+      // if (if_debug_){
+      //   for (size_t i = 0; i < (AHRS_LEN + 1); i++)
+      //   {
+      //     std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
+      //   }
+      //   std::cout << std::dec << std::endl;
+      // }
+      uint16_t CRC16 = CRC16_Table(ahrs_frame_.frame.data.data_buff, AHRS_LEN);
+      if (if_debug_){          
+        std::cout << "CRC16:        " << std::hex << (int)CRC16 << std::dec << std::endl;
+        std::cout << "head_crc16:   " << std::hex << (int)head_crc16 << std::dec << std::endl;
+        std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
+        std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
+        bool if_right = ((int)head_crc16 == (int)CRC16);
+        std::cout << "if_right: " << if_right << std::endl;
+      }
+      
+      if (head_crc16 != CRC16)
+      {
+        RCLCPP_WARN(this->get_logger(),"check crc16 faild(ahrs).");
+        continue;
+      }
+      else if(ahrs_frame_.frame.frame_end != FRAME_END)
+      {
+        RCLCPP_WARN(this->get_logger(),"check frame end.");
+        continue;
+      }
+    }
+    else if (head_type[0] == TYPE_INSGPS)
+    {
+      uint16_t head_crc16_l = insgps_frame_.frame.header.header_crc16_l;
+      uint16_t head_crc16_h = insgps_frame_.frame.header.header_crc16_h;
+      uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
+      size_t data_s = serial_.read(insgps_frame_.read_buf.read_msg, (INSGPS_LEN + 1)); //48+1
+      // if (if_debug_){
+      //   for (size_t i = 0; i < (AHRS_LEN + 1); i++)
+      //   {
+      //     std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
+      //   }
+      //   std::cout << std::dec << std::endl;
+      // }
+      uint16_t CRC16 = CRC16_Table(insgps_frame_.frame.data.data_buff, INSGPS_LEN);
+      if (if_debug_){          
+        std::cout << "CRC16:        " << std::hex << (int)CRC16 << std::dec << std::endl;
+        std::cout << "head_crc16:   " << std::hex << (int)head_crc16 << std::dec << std::endl;
+        std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
+        std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
+        bool if_right = ((int)head_crc16 == (int)CRC16);
+        std::cout << "if_right: " << if_right << std::endl;
+      }
+      
+      if (head_crc16 != CRC16)
+      {
+        RCLCPP_WARN(this->get_logger(),"check crc16 faild(ahrs).");
+        continue;
+      }
+      else if(insgps_frame_.frame.frame_end != FRAME_END)
+      {
+        RCLCPP_WARN(this->get_logger(),"check frame end.");
+        continue;
+      } 
+    }
+    else if (head_type[0] == TYPE_GEODETIC_POS)
+    {
+      uint16_t head_crc16_l = Geodetic_Position_frame_.frame.header.header_crc16_l;
+      uint16_t head_crc16_h = Geodetic_Position_frame_.frame.header.header_crc16_h;
+      uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
+      size_t data_s = serial_.read(Geodetic_Position_frame_.read_buf.read_msg, (GEODETIC_POS_LEN + 1)); //24+1
+      // if (if_debug_){
+      //   for (size_t i = 0; i < (AHRS_LEN + 1); i++)
+      //   {
+      //     std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
+      //   }
+      //   std::cout << std::dec << std::endl;
+      // }
+      uint16_t CRC16 = CRC16_Table(Geodetic_Position_frame_.frame.data.data_buff, GEODETIC_POS_LEN);
+      if (if_debug_){          
+        std::cout << "CRC16:        " << std::hex << (int)CRC16 << std::dec << std::endl;
+        std::cout << "head_crc16:   " << std::hex << (int)head_crc16 << std::dec << std::endl;
+        std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
+        std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
+        bool if_right = ((int)head_crc16 == (int)CRC16);
+        std::cout << "if_right: " << if_right << std::endl;
+      }
+      
+      if (head_crc16 != CRC16)
+      {
+        RCLCPP_WARN(this->get_logger(),"check crc16 faild(gps).");
+        continue;
+      }
+      else if(Geodetic_Position_frame_.frame.frame_end != FRAME_END)
+      {
+        RCLCPP_WARN(this->get_logger(),"check frame end.");
+        continue;
+      }
+    }
+    // publish magyaw topic
+    //读取IMU数据进行解析，并发布相关话题
+
+    if (head_type[0] == TYPE_IMU)
+    {
+      // publish imu topic
+      sensor_msgs::msg::Imu imu_data;
+      imu_data.header.stamp = rclcpp::Node::now();
+      imu_data.header.frame_id = imu_frame_id_.c_str();
+      Eigen::Quaterniond q_ahrs(ahrs_frame_.frame.data.data_pack.Qw,
+                                ahrs_frame_.frame.data.data_pack.Qx,
+                                ahrs_frame_.frame.data.data_pack.Qy,
+                                ahrs_frame_.frame.data.data_pack.Qz);
+      Eigen::Quaterniond q_r =                          
+          Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitZ()) * 
+          Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitY()) * 
+          Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX());
+      Eigen::Quaterniond q_rr =                          
+          Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) * 
+          Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) * 
+          Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
+      Eigen::Quaterniond q_xiao_rr =
+          Eigen::AngleAxisd( PI/2, Eigen::Vector3d::UnitZ()) * 
+          Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) * 
+          Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
+      if (device_type_ == 0)         //未经变换的原始数据
+      {
+        imu_data.orientation.w = ahrs_frame_.frame.data.data_pack.Qw;
+        imu_data.orientation.x = ahrs_frame_.frame.data.data_pack.Qx;
+        imu_data.orientation.y = ahrs_frame_.frame.data.data_pack.Qy;
+        imu_data.orientation.z = ahrs_frame_.frame.data.data_pack.Qz;
+        imu_data.angular_velocity.x = imu_frame_.frame.data.data_pack.gyroscope_x;
+        imu_data.angular_velocity.y = imu_frame_.frame.data.data_pack.gyroscope_y;
+        imu_data.angular_velocity.z = imu_frame_.frame.data.data_pack.gyroscope_z;
+        imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
+        imu_data.linear_acceleration.y = imu_frame_.frame.data.data_pack.accelerometer_y;
+        imu_data.linear_acceleration.z = imu_frame_.frame.data.data_pack.accelerometer_z;
+      }
+      else if (device_type_ == 1)    //imu单品rclcpp标准下的坐标变换
+      {
+        
+        Eigen::Quaterniond q_out =  q_r * q_ahrs * q_rr;
+        imu_data.orientation.w = q_out.w();
+        imu_data.orientation.x = q_out.x();
+        imu_data.orientation.y = q_out.y();
+        imu_data.orientation.z = q_out.z();
+        imu_data.angular_velocity.x =  imu_frame_.frame.data.data_pack.gyroscope_x;
+        imu_data.angular_velocity.y = -imu_frame_.frame.data.data_pack.gyroscope_y;
+        imu_data.angular_velocity.z = -imu_frame_.frame.data.data_pack.gyroscope_z;
+        imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
+        imu_data.linear_acceleration.y = -imu_frame_.frame.data.data_pack.accelerometer_y;
+        imu_data.linear_acceleration.z = -imu_frame_.frame.data.data_pack.accelerometer_z;
+      }
+      imu_pub_->publish(imu_data);
+}
+    //读取AHRS数据进行解析，并发布相关话题
+    else if (head_type[0] == TYPE_AHRS)
+    {
+      geometry_msgs::msg::Pose2D pose_2d;
+      pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
+      mag_pose_pub_->publish(pose_2d);
+      //std::cout << "YAW: " << pose_2d.theta << std::endl;
+      geometry_msgs::msg::Vector3 Euler_angles_2d,Magnetic;  
+      Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
+      Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
+      Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
+      Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
+      Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
+      Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
+
+      Euler_angles_pub_->publish(Euler_angles_2d);
+      Magnetic_pub_->publish(Magnetic);
+
+    }
+
+    //读取gps_pos数据进行解析，并发布相关话题
+    else if (head_type[0] == TYPE_GEODETIC_POS)
+    {
+      sensor_msgs::msg::NavSatFix gps_data;
+      gps_data.header.stamp = rclcpp::Node::now();
+      gps_data.header.frame_id = "navsat_link";
+      gps_data.latitude = Geodetic_Position_frame_.frame.data.data_pack.Latitude / DEG_TO_RAD;
+      gps_data.longitude = Geodetic_Position_frame_.frame.data.data_pack.Longitude / DEG_TO_RAD;
+      gps_data.altitude = Geodetic_Position_frame_.frame.data.data_pack.Height;
+
+      //std::cout << "lat: " << Geodetic_Position_frame_.frame.data.data_pack.Latitude << std::endl;
+      //std::cout << "lon: " << Geodetic_Position_frame_.frame.data.data_pack.Longitude << std::endl;
+      //std::cout << "h: " << Geodetic_Position_frame_.frame.data.data_pack.Height << std::endl;
+
+      gps_pub_->publish(gps_data);
+    } 
+    //读取INSGPS数据进行解析，并发布相关话题
+    else if (head_type[0] == TYPE_INSGPS)
+    {
+      nav_msgs::msg::Odometry odom_msg;
+      odom_msg.header.stamp = rclcpp::Node::now(); 
+      // odom_msg.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
+      odom_msg.pose.pose.position.x = insgps_frame_.frame.data.data_pack.Location_North; //Position //位置
+      odom_msg.pose.pose.position.y = insgps_frame_.frame.data.data_pack.Location_East;
+      odom_msg.pose.pose.position.z = insgps_frame_.frame.data.data_pack.Location_Down;
+
+      // odom_msg.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标
+      odom_msg.twist.twist.linear.x =  insgps_frame_.frame.data.data_pack.Velocity_North; //Speed in the X direction //X方向速度
+      odom_msg.twist.twist.linear.y =  insgps_frame_.frame.data.data_pack.Velocity_East; //Speed in the Y direction //Y方向速度
+      odom_msg.twist.twist.linear.z =  insgps_frame_.frame.data.data_pack.Velocity_Down;
+      NED_odom_pub_->publish(odom_msg);
+
+      geometry_msgs::msg::Twist speed_msg;
+      speed_msg.linear.x =  insgps_frame_.frame.data.data_pack.BodyVelocity_X;
+      speed_msg.linear.y =  insgps_frame_.frame.data.data_pack.BodyVelocity_Y;
+      speed_msg.linear.z =  insgps_frame_.frame.data.data_pack.BodyVelocity_Z;   
+      twist_pub_->publish(speed_msg);
+		  
+    //  std::cout << "N: " << insgps_frame_.frame.data.data_pack.Location_North << std::endl;
+    //  std::cout << "E: " << insgps_frame_.frame.data.data_pack.Location_East << std::endl;
+    //  std::cout << "D: " << insgps_frame_.frame.data.data_pack.Location_Down << std::endl;
+
+    }   
+  }
+}
+
+void ahrsBringup::magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz)
+{
+  double temp1 = magy * cos(roll) + magz * sin(roll);
+  double temp2 = magx * cos(pitch) + magy * sin(pitch) * sin(roll) - magz * sin(pitch) * cos(roll);
+  magyaw = atan2(-temp1, temp2);
+  if(magyaw < 0)
+  {
+    magyaw = magyaw + 2 * PI;
+  }
+  // return magyaw;
+}
+
+void ahrsBringup::checkSN(int type)
+{
+  switch (type)
+  {
+  case TYPE_IMU:
+    if (++read_sn_ != imu_frame_.frame.header.serial_num)
+    {
+      if ( imu_frame_.frame.header.serial_num < read_sn_)
+      {
+        sn_lost_ += 256 - (int)(read_sn_ - imu_frame_.frame.header.serial_num);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+      else
+      {
+        sn_lost_ += (int)(imu_frame_.frame.header.serial_num - read_sn_);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+    }
+    read_sn_ = imu_frame_.frame.header.serial_num;
+    break;
+
+  case TYPE_AHRS:
+    if (++read_sn_ != ahrs_frame_.frame.header.serial_num)
+    {
+      if ( ahrs_frame_.frame.header.serial_num < read_sn_)
+      {
+        sn_lost_ += 256 - (int)(read_sn_ - ahrs_frame_.frame.header.serial_num);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+      else
+      {
+        sn_lost_ += (int)(ahrs_frame_.frame.header.serial_num - read_sn_);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+    }
+    read_sn_ = ahrs_frame_.frame.header.serial_num;
+    break;
+
+  case TYPE_INSGPS:
+    if (++read_sn_ != insgps_frame_.frame.header.serial_num)
+    {
+      if ( insgps_frame_.frame.header.serial_num < read_sn_)
+      {
+        sn_lost_ += 256 - (int)(read_sn_ - insgps_frame_.frame.header.serial_num);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+      else
+      {
+        sn_lost_ += (int)(insgps_frame_.frame.header.serial_num - read_sn_);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+    }
+    read_sn_ = insgps_frame_.frame.header.serial_num;
+    break;
+
+  case TYPE_GEODETIC_POS:
+    if (++read_sn_ != Geodetic_Position_frame_.frame.header.serial_num)
+    {
+      if ( Geodetic_Position_frame_.frame.header.serial_num < read_sn_)
+      {
+        sn_lost_ += 256 - (int)(read_sn_ - Geodetic_Position_frame_.frame.header.serial_num);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+      else
+      {
+        sn_lost_ += (int)(Geodetic_Position_frame_.frame.header.serial_num - read_sn_);
+        if(if_debug_){
+          RCLCPP_WARN(this->get_logger(),"detected sn lost.");
+        }
+      }
+    }
+    read_sn_ = Geodetic_Position_frame_.frame.header.serial_num;
+    break;
+
+  default:
+    break;
+  }
+}
+
+} //namespace FDILink
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  FDILink::ahrsBringup bp;
+
+  return 0;
+}

src/fdilink_ahrs_ROS2/src/crc_table.cpp

@@ -0,0 +1,159 @@
+#include <stdint.h>
+#include <crc_table.h>
+
+static const uint8_t CRC8Table[] = {
+    0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
+    157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
+    35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
+    190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
+    70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
+    219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
+    101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
+    248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
+    140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
+    17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
+    175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
+    50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
+    202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
+    87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
+    233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
+    116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53};
+
+static const uint16_t CRC16Table[256] =
+{
+    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+    0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+    0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+    0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+    0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+    0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+    0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+    0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+    0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+    0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+    0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+    0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+    0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+    0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+    0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+    0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+    0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+    0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+    0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+    0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+    0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+    0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+    0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+    0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+    0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+    0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+    0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+    0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+    0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+    0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+    0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+    0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+
+static const uint32_t CRC32Table[] = {
+ 0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL,
+ 0x076dc419L, 0x706af48fL, 0xe963a535L, 0x9e6495a3L,
+ 0x0edb8832L, 0x79dcb8a4L, 0xe0d5e91eL, 0x97d2d988L,
+ 0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L, 0x90bf1d91L,
+ 0x1db71064L, 0x6ab020f2L, 0xf3b97148L, 0x84be41deL,
+ 0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L,
+ 0x136c9856L, 0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL,
+ 0x14015c4fL, 0x63066cd9L, 0xfa0f3d63L, 0x8d080df5L,
+ 0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L, 0xa2677172L,
+ 0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL,
+ 0x35b5a8faL, 0x42b2986cL, 0xdbbbc9d6L, 0xacbcf940L,
+ 0x32d86ce3L, 0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L,
+ 0x26d930acL, 0x51de003aL, 0xc8d75180L, 0xbfd06116L,
+ 0x21b4f4b5L, 0x56b3c423L, 0xcfba9599L, 0xb8bda50fL,
+ 0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
+ 0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL,
+ 0x76dc4190L, 0x01db7106L, 0x98d220bcL, 0xefd5102aL,
+ 0x71b18589L, 0x06b6b51fL, 0x9fbfe4a5L, 0xe8b8d433L,
+ 0x7807c9a2L, 0x0f00f934L, 0x9609a88eL, 0xe10e9818L,
+ 0x7f6a0dbbL, 0x086d3d2dL, 0x91646c97L, 0xe6635c01L,
+ 0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL,
+ 0x6c0695edL, 0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L,
+ 0x65b0d9c6L, 0x12b7e950L, 0x8bbeb8eaL, 0xfcb9887cL,
+ 0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L, 0xfbd44c65L,
+ 0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L,
+ 0x4adfa541L, 0x3dd895d7L, 0xa4d1c46dL, 0xd3d6f4fbL,
+ 0x4369e96aL, 0x346ed9fcL, 0xad678846L, 0xda60b8d0L,
+ 0x44042d73L, 0x33031de5L, 0xaa0a4c5fL, 0xdd0d7cc9L,
+ 0x5005713cL, 0x270241aaL, 0xbe0b1010L, 0xc90c2086L,
+ 0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
+ 0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L,
+ 0x59b33d17L, 0x2eb40d81L, 0xb7bd5c3bL, 0xc0ba6cadL,
+ 0xedb88320L, 0x9abfb3b6L, 0x03b6e20cL, 0x74b1d29aL,
+ 0xead54739L, 0x9dd277afL, 0x04db2615L, 0x73dc1683L,
+ 0xe3630b12L, 0x94643b84L, 0x0d6d6a3eL, 0x7a6a5aa8L,
+ 0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L,
+ 0xf00f9344L, 0x8708a3d2L, 0x1e01f268L, 0x6906c2feL,
+ 0xf762575dL, 0x806567cbL, 0x196c3671L, 0x6e6b06e7L,
+ 0xfed41b76L, 0x89d32be0L, 0x10da7a5aL, 0x67dd4accL,
+ 0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L,
+ 0xd6d6a3e8L, 0xa1d1937eL, 0x38d8c2c4L, 0x4fdff252L,
+ 0xd1bb67f1L, 0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL,
+ 0xd80d2bdaL, 0xaf0a1b4cL, 0x36034af6L, 0x41047a60L,
+ 0xdf60efc3L, 0xa867df55L, 0x316e8eefL, 0x4669be79L,
+ 0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
+ 0xcc0c7795L, 0xbb0b4703L, 0x220216b9L, 0x5505262fL,
+ 0xc5ba3bbeL, 0xb2bd0b28L, 0x2bb45a92L, 0x5cb36a04L,
+ 0xc2d7ffa7L, 0xb5d0cf31L, 0x2cd99e8bL, 0x5bdeae1dL,
+ 0x9b64c2b0L, 0xec63f226L, 0x756aa39cL, 0x026d930aL,
+ 0x9c0906a9L, 0xeb0e363fL, 0x72076785L, 0x05005713L,
+ 0x95bf4a82L, 0xe2b87a14L, 0x7bb12baeL, 0x0cb61b38L,
+ 0x92d28e9bL, 0xe5d5be0dL, 0x7cdcefb7L, 0x0bdbdf21L,
+ 0x86d3d2d4L, 0xf1d4e242L, 0x68ddb3f8L, 0x1fda836eL,
+ 0x81be16cdL, 0xf6b9265bL, 0x6fb077e1L, 0x18b74777L,
+ 0x88085ae6L, 0xff0f6a70L, 0x66063bcaL, 0x11010b5cL,
+ 0x8f659effL, 0xf862ae69L, 0x616bffd3L, 0x166ccf45L,
+ 0xa00ae278L, 0xd70dd2eeL, 0x4e048354L, 0x3903b3c2L,
+ 0xa7672661L, 0xd06016f7L, 0x4969474dL, 0x3e6e77dbL,
+ 0xaed16a4aL, 0xd9d65adcL, 0x40df0b66L, 0x37d83bf0L,
+ 0xa9bcae53L, 0xdebb9ec5L, 0x47b2cf7fL, 0x30b5ffe9L,
+ 0xbdbdf21cL, 0xcabac28aL, 0x53b39330L, 0x24b4a3a6L,
+ 0xbad03605L, 0xcdd70693L, 0x54de5729L, 0x23d967bfL,
+ 0xb3667a2eL, 0xc4614ab8L, 0x5d681b02L, 0x2a6f2b94L,
+ 0xb40bbe37L, 0xc30c8ea1L, 0x5a05df1bL, 0x2d02ef8dL 
+};
+
+
+uint8_t CRC8_Table(uint8_t* p, uint8_t counter)
+{
+    uint8_t crc8 = 0;
+    for (int i = 0; i < counter; i++)
+    {
+        uint8_t value = p[i];
+        uint8_t new_index = crc8 ^ value;
+        crc8 = CRC8Table[new_index];
+    }
+    return (crc8);
+}
+
+
+uint16_t CRC16_Table(uint8_t *p, uint8_t counter)
+{
+    uint16_t crc16 = 0;
+    for (int i = 0; i < counter; i++)
+    {
+        uint8_t value = p[i];
+        crc16 = CRC16Table[((crc16 >> 8) ^ value) & 0xff] ^ (crc16 << 8);
+    }
+    return (crc16);
+}
+
+uint32_t CRC32_Table(uint8_t *p, uint8_t counter)
+{
+    uint16_t crc32 = 0;
+    for (int i = 0; i < counter; i++)
+    {
+        uint8_t value = p[i];
+        crc32 = CRC16Table[((crc32 >> 8) ^ value) & 0xff] ^ (crc32 << 8);
+    }
+    return (crc32);
+}

src/fdilink_ahrs_ROS2/src/imu_tf.cpp

@@ -0,0 +1,107 @@
+#include <memory>
+#include <inttypes.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <string>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <rclcpp/time.hpp>
+using std::placeholders::_1;
+using namespace std;
+
+/* 参考ROS wiki
+ * http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29
+ * */
+
+int position_x ;
+int position_y ;
+int position_z ;
+std::string imu_topic;
+std::string imu_frame_id, world_frame_id;
+
+static  std::shared_ptr<tf2_ros::TransformBroadcaster>  br;
+rclcpp::Node::SharedPtr nh_=nullptr;
+class imu_data_to_tf : public rclcpp::Node
+{
+    public:
+        imu_data_to_tf():Node("imu_data_to_tf")
+        {
+
+            // node.param("/imu_tf/imu_topic", imu_topic, std::string("/imu"));
+            // node.param("/imu_tf/position_x", position_x, 0);
+            // node.param("/imu_tf/position_y", position_y, 0);
+            // node.param("/imu_tf/position_z", position_z, 0);
+            
+            this->declare_parameter<std::string>("world_frame_id","/world");
+            this->get_parameter("world_frame_id",  world_frame_id);
+
+            this->declare_parameter<std::string>("imu_frame_id","/imu");
+            this->get_parameter("imu_frame_id",  imu_frame_id);
+            
+            this->declare_parameter<std::string>("imu_topic","/imu");
+            this->get_parameter("imu_topic",  imu_topic);
+
+            this->declare_parameter<std::int16_t>("position_x",1);
+            this->get_parameter("position_x",   position_x);
+
+            this->declare_parameter<std::int16_t>("position_y",1);
+            this->get_parameter("position_y", position_y);
+
+            this->declare_parameter<std::int16_t>("position_z",1);
+            this->get_parameter("position_z", position_z);
+            br =std::make_shared<tf2_ros::TransformBroadcaster>(this);
+            sub_ = this->create_subscription<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10, std::bind(&imu_data_to_tf::ImuCallback,this,_1));
+
+        }
+    private:
+        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
+
+        //rclcpp::Subscriber sub = node.subscribe(imu_topic.c_str(), 10, &ImuCallback);
+
+
+        void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_data) {
+
+            //static tf2_ros::TransformBroadcaster br;//广播器
+
+            // tf2::Transform transform;
+            // transform.setOrigin(tf2::Vector3(position_x, position_y, position_z));//设置平移部分
+
+            // 从IMU消息包中获取四元数数据
+            tf2::Quaternion q;
+            q.setX(imu_data->orientation.x);
+            q.setY(imu_data->orientation.y);
+            q.setZ(imu_data->orientation.z);
+            q.setW(imu_data->orientation.w);
+            q.normalized();//归一化
+
+            // transform.setRotation(q);//设置旋转部分
+            //广播出去
+            //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "imu"));
+            geometry_msgs::msg::TransformStamped tfs;
+            tfs.header.stamp=rclcpp::Node::now();
+            tfs.header.frame_id ="world";
+            tfs.child_frame_id="imu";
+            tfs.transform.translation.x=position_x;
+            tfs.transform.translation.y=position_y;
+            tfs.transform.translation.z=position_z;
+            tfs.transform.rotation.x=q.getX();
+            tfs.transform.rotation.y=q.getY();
+            tfs.transform.rotation.z=q.getZ();
+            tfs.transform.rotation.w=q.getW();
+            br->sendTransform(tfs);
+            //tf2::(transform, rclcpp::Node::now(), "world", "imu")
+        }
+};
+
+int main (int argc, char ** argv) {
+    rclcpp::init(argc, argv);
+    
+    //ros::NodeHandle node;, "imu_data_to_tf"
+    //auto node =std::make_shared<imu_data_to_tf>();
+    rclcpp::spin(std::make_shared<imu_data_to_tf>());
+    rclcpp::shutdown();
+    return 0;
+}

src/fdilink_ahrs_ROS2/wheeltec_udev.sh

@@ -0,0 +1,14 @@
+#CP2102 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss.rules
+#CH9102，同时系统安装了对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo  'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss2.rules
+#CH9102，同时系统没有安装对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
+echo  'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss3.rules
+#CH340，直接设置别名为wheeltec_FDI_IMU_GNSS
+echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdcontroller_340.rules
+
+service udev reload
+sleep 2
+service udev restart
+
+

src/ps2_controller/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+project(ps2_controller)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rc_msgs REQUIRED)  # 添加对 rc_msgs 的依赖
+
+add_executable(ps2_reader src/ps2_reader.cpp)
+
+ament_target_dependencies(ps2_reader rclcpp rc_msgs)  # 添加 rc_msgs 依赖
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS
+  ps2_reader
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/ps2_controller/launch/ps2_controller.launch.py

@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='ps2_controller',
+            executable='ps2_reader',
+            name='ps2_controller_node',
+            output='screen',
+            parameters=[
+                {'device': '/dev/input/js0'}  # 可根据需要修改设备路径
+            ]
+        )
+    ])

src/ps2_controller/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ps2_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rc_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ps2_controller/src/ps2_reader.cpp

@@ -0,0 +1,123 @@
+#include <rclcpp/rclcpp.hpp>
+#include <linux/joystick.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <iostream>
+#include <string>
+#include "rc_msgs/msg/ps2_data.hpp"  // 引入 Ps2Data 消息类型
+
+class PS2ControllerNode : public rclcpp::Node
+{
+public:
+    PS2ControllerNode() : Node("ps2_controller_node")
+    {
+        this->declare_parameter<std::string>("device", "/dev/input/js0");
+        device_path_ = this->get_parameter("device").as_string();
+
+        joystick_fd_ = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK);
+        if (joystick_fd_ < 0)
+        {
+            RCLCPP_ERROR(this->get_logger(), "Failed to open device: %s", device_path_.c_str());
+            return;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "Connected to device: %s", device_path_.c_str());
+
+        ps2_data_ = std::make_shared<rc_msgs::msg::Ps2Data>();
+        publisher_ = this->create_publisher<rc_msgs::msg::Ps2Data>("ps2_data", 10);
+
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(10),  // 100Hz
+            std::bind(&PS2ControllerNode::publishData, this));
+    }
+
+    ~PS2ControllerNode()
+    {
+        if (joystick_fd_ >= 0)
+        {
+            close(joystick_fd_);
+        }
+    }
+
+private:
+    void publishData()
+    {
+        readJoystick();
+        publisher_->publish(*ps2_data_);
+    }
+
+    void readJoystick()
+    {
+        struct js_event event;
+        while (read(joystick_fd_, &event, sizeof(event)) > 0)
+        {
+            switch (event.type & ~JS_EVENT_INIT)
+            {
+            case JS_EVENT_BUTTON:
+                handleButtonEvent(event);
+                break;
+            case JS_EVENT_AXIS:
+                handleAxisEvent(event);
+                break;
+            default:
+                break;
+            }
+        }
+    }
+
+    void handleButtonEvent(const js_event &event)
+    {
+        switch (event.number)
+        {
+        case 10: ps2_data_->select = event.value; break;
+        case 11: ps2_data_->start = event.value; break;
+        case 6: ps2_data_->l1 = event.value; break;
+        case 8: ps2_data_->l2 = event.value; break;
+        case 7: ps2_data_->r1 = event.value; break;
+        case 9: ps2_data_->r2 = event.value; break;
+        case 3: ps2_data_->x = event.value; break;
+        case 4: ps2_data_->y = event.value; break;
+        case 0: ps2_data_->a = event.value; break;
+        case 1: ps2_data_->b = event.value; break;
+        default:
+            break;
+        }
+    }
+
+    void handleAxisEvent(const js_event &event)
+    {
+        constexpr float MAX_AXIS_VALUE = 32767.0f;  // 最大轴值
+        constexpr float MIN_AXIS_VALUE = -32768.0f; // 最小轴值
+
+        auto normalize = [](int value) -> float {
+            return static_cast<float>(value) / MAX_AXIS_VALUE;
+        };
+
+        switch (event.number)
+        {
+        case 0: ps2_data_->lx = normalize(event.value); break;
+        case 1: ps2_data_->ly = normalize(event.value); break;
+        case 2: ps2_data_->rx = normalize(event.value); break;
+        case 3: ps2_data_->ry = normalize(event.value); break;
+        case 6: ps2_data_->left_right = normalize(event.value); break;
+        case 7: ps2_data_->up_down = normalize(event.value); break;
+        default:
+            break;
+        }
+    }
+
+    std::string device_path_;
+    int joystick_fd_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<rc_msgs::msg::Ps2Data>::SharedPtr publisher_;
+    std::shared_ptr<rc_msgs::msg::Ps2Data> ps2_data_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<PS2ControllerNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/rc_msgs/.gitignore

@@ -0,0 +1,51 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

src/rc_msgs/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(rc_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/Ps2Data.msg"
+        "msg/DataControl.msg"
+        "msg/DataMotor.msg"
+        "msg/DataLeg.msg"
+        "msg/DataMotors.msg"
+        "msg/MotorCmd.msg"
+        "msg/MotorCmds.msg"
+)
+
+ament_package()
+

src/rc_msgs/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 zucheng Lv
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/rc_msgs/README.md

@@ -0,0 +1,6 @@
+# rc_msg
+Some ROS 2 custom messages for Robotaster
+
+Usage
+Modify or add files in the /msg directory as needed
+colcon build

src/rc_msgs/msg/DataControl.msg

@@ -0,0 +1,5 @@
+float32 tau
+float32 vw
+float32 pos
+float32 kp
+float32 kd

src/rc_msgs/msg/DataLeg.msg

@@ -0,0 +1,7 @@
+DataControl data_control_0
+DataControl data_control_1
+DataControl data_control_2
+
+DataMotor data_motor_0
+DataMotor data_motor_1
+DataMotor data_motor_2

src/rc_msgs/msg/DataMotor.msg

@@ -0,0 +1,3 @@
+float32 tau
+float32 vw
+float32 pos

src/rc_msgs/msg/DataMotors.msg

@@ -0,0 +1,3 @@
+DataMotor data_motor_0
+DataMotor data_motor_1
+DataMotor data_motor_2

src/rc_msgs/msg/MotorCmd.msg

@@ -0,0 +1,5 @@
+float32 tau
+float32 vw
+float32 pos
+float32 kp
+float32 kd

src/rc_msgs/msg/MotorCmds.msg

@@ -0,0 +1,3 @@
+MotorCmd motor_cmd_0
+MotorCmd motor_cmd_1
+MotorCmd motor_cmd_2

src/rc_msgs/msg/Ps2Data.msg

@@ -0,0 +1,25 @@
+# control input message
+float32 lx
+float32 ly
+float32 rx
+float32 ry
+
+float32 up_down
+float32 left_right
+
+bool l1
+bool l2
+bool r1
+bool r2
+
+# 四种模式
+uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
+
+bool x
+bool y
+bool a
+bool b
+
+bool select
+bool start
+

src/rc_msgs/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+    <name>rc_msgs</name>
+    <version>0.0.1</version>
+    <description>Describe custom messages</description>
+    <maintainer email="1683502971@qq.com">biao</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/robot_bringup/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(robot_bringup)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/robot_bringup/launch/robot_bringup.launch.py

@@ -0,0 +1,128 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    # 定义 leg_ctrl 的四个容器节点
+    lf_container = ComposableNodeContainer(
+        name='lf_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_lf',
+                parameters=[{'serial_port': '/dev/ttyACM0'}],
+                remappings=[
+                    ('motor_cmds', 'LF/motor_cmds'),
+                    ('data_motors', 'LF/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    rf_container = ComposableNodeContainer(
+        name='rf_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_rf',
+                parameters=[{'serial_port': '/dev/ttyACM1'}],
+                remappings=[
+                    ('motor_cmds', 'RF/motor_cmds'),
+                    ('data_motors', 'RF/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    lr_container = ComposableNodeContainer(
+        name='lr_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_lr',
+                parameters=[{'serial_port': '/dev/ttyACM2'}],
+                remappings=[
+                    ('motor_cmds', 'LR/motor_cmds'),
+                    ('data_motors', 'LR/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    rr_container = ComposableNodeContainer(
+        name='rr_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_rr',
+                parameters=[{'serial_port': '/dev/ttyACM3'}],
+                remappings=[
+                    ('motor_cmds', 'RR/motor_cmds'),
+                    ('data_motors', 'RR/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    # 定义 ahrs_driver 节点
+    ahrs_driver = Node(
+        package="fdilink_ahrs",
+        executable="ahrs_driver_node",
+        parameters=[{
+            'if_debug_': False,
+            'serial_port_': '/dev/ttyUSB0',
+            'serial_baud_': 921600,
+            'imu_topic': '/imu',
+            'imu_frame_id_': 'gyro_link',
+            'mag_pose_2d_topic': '/mag_pose_2d',
+            'Magnetic_topic': '/magnetic',
+            'Euler_angles_topic': '/euler_angles',
+            'gps_topic': '/gps/fix',
+            'twist_topic': '/system_speed',
+            'NED_odom_topic': '/NED_odometry',
+            'device_type_': 1
+        }],
+        output="screen"
+    )
+
+    # 定义 ps2_controller 节点
+    ps2_controller_node = Node(
+        package='ps2_controller',
+        executable='ps2_reader',
+        name='ps2_controller_node',
+        output='screen',
+        parameters=[
+            {'device': '/dev/input/js0'}  # 可根据需要修改设备路径
+        ]
+    )
+
+    # 返回主启动文件的描述
+    return LaunchDescription([
+        lf_container,
+        rf_container,
+        lr_container,
+        rr_container,
+        ahrs_driver,
+        ps2_controller_node  # 添加 ps2_controller 节点
+    ])

src/robot_bringup/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robot_bringup</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>launch</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/simple_quadruped_control/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.8)
+project(simple_quadruped_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rc_msgs REQUIRED)
+
+# 添加头文件目录
+include_directories(include)
+
+add_executable(simple_control src/simple_control.cpp)
+ament_target_dependencies(simple_control rclcpp rc_msgs)
+
+install(TARGETS
+  simple_control
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/simple_quadruped_control/include/simple_quadruped_control/simple_control.hpp

@@ -0,0 +1,100 @@
+#ifndef SIMPLE_QUADRUPED_CONTROL_HPP
+#define SIMPLE_QUADRUPED_CONTROL_HPP
+
+#include "rclcpp/rclcpp.hpp"
+#include "rc_msgs/msg/ps2_data.hpp"
+#include "rc_msgs/msg/motor_cmds.hpp"
+#include "rc_msgs/msg/data_motors.hpp"
+
+struct ps2_t
+{
+    float lx;
+    float ly;
+    float rx;
+    float ry;
+
+    float up_down;
+    float left_right;
+
+    bool l1;
+    bool l2;
+    bool r1;
+    bool r2;
+
+    bool x;
+    bool y;
+    bool a;
+    bool b;
+
+    bool select;
+    bool start;
+};
+
+struct motor_cmd_t
+{
+    float pos;
+    float vw;
+    float tau;
+    float kp;
+    float kd;
+};
+
+struct motor_cmds_t
+{
+    motor_cmd_t motors[3]; // 使用数组替代单独的字段
+};
+
+struct data_motor_t
+{
+    float pos;
+    float vw;
+    float tau;
+};
+
+struct data_motors_t
+{
+    data_motor_t motors[3]; // 使用数组替代单独的字段
+};
+
+struct mech_zero_t
+{
+    float motors[3]; // 使用数组替代单独的字段
+};
+
+struct cmd_t
+{
+    float pos;
+    float vw;
+    float tau;
+    float kp;
+    float kd;
+};
+
+struct cmds_t
+{
+    cmd_t motors[3]; // 使用数组替代单独的字段
+};
+
+ps2_t ps2;
+motor_cmds_t motor_cmds;
+data_motors_t data_motors;
+mech_zero_t mech_zero;
+cmds_t cmds;
+
+class QuadrupedSubscriber : public rclcpp::Node
+{
+public:
+    QuadrupedSubscriber();
+
+private:
+    void ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg);
+    void data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg);
+    float limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit);
+    void publish_motor_cmds();
+
+    rclcpp::Subscription<rc_msgs::msg::Ps2Data>::SharedPtr ps2_data_subscriber_;
+    rclcpp::Publisher<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmds_publisher_;
+    rclcpp::Subscription<rc_msgs::msg::DataMotors>::SharedPtr data_motors_subscriber_;
+};
+
+#endif // SIMPLE_QUADRUPED_CONTROL_HPP

src/simple_quadruped_control/launch/simple_control.launch.py

@@ -0,0 +1,12 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='simple_quadruped_control',
+            executable='simple_control',
+            name='simple_control_node',
+            output='screen'
+        )
+    ])

src/simple_quadruped_control/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>simple_quadruped_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rc_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/simple_quadruped_control/src/simple_control.cpp

@@ -0,0 +1,208 @@
+#include "simple_quadruped_control/simple_control.hpp"
+
+// tau = tff + kp(q - q0) + kd(dq - dq0)
+// tau :输出力矩
+// tff :前馈力矩
+// q：期望角度位置
+// q0：当前角度位置
+// dq 期望角速度
+// dq0 当前角速度
+// kp：位置刚度
+// kd：速度阻尼
+
+QuadrupedSubscriber::QuadrupedSubscriber() : Node("simple_control")
+{
+    // 订阅 Ps2Data 话题
+    ps2_data_subscriber_ = this->create_subscription<rc_msgs::msg::Ps2Data>(
+        "ps2_data", 10,
+        std::bind(&QuadrupedSubscriber::ps2_data_callback, this, std::placeholders::_1));
+
+    // 发布 MotorCmds 话题
+    motor_cmds_publisher_ = this->create_publisher<rc_msgs::msg::MotorCmds>(
+        "/LF/motor_cmds", 10);
+
+    // 订阅 DataMotors 话题
+    data_motors_subscriber_ = this->create_subscription<rc_msgs::msg::DataMotors>(
+        "/LF/data_motors", 10,
+        std::bind(&QuadrupedSubscriber::data_motors_callback, this, std::placeholders::_1));
+
+    // 初始化机械零点
+    mech_zero.motors[0] = 0.7438;
+    mech_zero.motors[1] = 0.5693;
+    mech_zero.motors[2] = 0.6547;
+}
+
+void QuadrupedSubscriber::ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg)
+{
+    ps2.lx = msg->lx;
+    ps2.ly = msg->ly;
+    ps2.rx = msg->rx;
+    ps2.ry = msg->ry;
+
+    ps2.up_down = msg->up_down;
+    ps2.left_right = msg->left_right;
+
+    ps2.l1 = msg->l1;
+    ps2.l2 = msg->l2;
+    ps2.r1 = msg->r1;
+    ps2.r2 = msg->r2;
+
+    ps2.x = msg->x;
+    ps2.y = msg->y;
+    ps2.a = msg->a;
+    ps2.b = msg->b;
+
+    ps2.select = msg->select;
+    ps2.start = msg->start;
+
+    // 回零
+    if (ps2.l1)
+    {
+        for (int i = 0; i < 3; ++i)
+        {   // 重置commonds
+            cmds.motors[i].pos = mech_zero.motors[i];
+            cmds.motors[i].vw = 0.0;
+            cmds.motors[i].tau = 0.0;
+            cmds.motors[i].kp = 1.2;
+            cmds.motors[i].kd = 0.05;
+            // 限制力矩输出
+            motor_cmds.motors[i].pos = cmds.motors[i].pos;
+            motor_cmds.motors[i].vw = cmds.motors[i].vw;
+            motor_cmds.motors[i].tau = cmds.motors[i].tau;
+            motor_cmds.motors[i].kp = cmds.motors[i].kp;
+            motor_cmds.motors[i].kd = cmds.motors[i].kd;
+            motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5);
+        }
+        publish_motor_cmds();
+        RCLCPP_INFO(this->get_logger(), "Zeroing motors");
+        return;
+    }
+    if (ps2.l2)
+    {
+        
+            cmds.motors[0].pos = cmds.motors[0].pos + ps2.lx * 0.01;
+            cmds.motors[0].vw = 0.0;
+            cmds.motors[0].tau = 0.0;
+            cmds.motors[0].kp = 1.2;
+            cmds.motors[0].kd = 0.05;
+
+            cmds.motors[1].pos = cmds.motors[1].pos + ps2.ly * 0.01;
+            cmds.motors[1].vw = 0.0;
+            cmds.motors[1].tau = 0.0;
+            cmds.motors[1].kp = 1.2;
+            cmds.motors[1].kd = 0.05;
+
+            cmds.motors[2].pos = cmds.motors[2].pos + ps2.ly * 0.01;
+            cmds.motors[2].vw = 0.0;
+            cmds.motors[2].tau = 0.0;
+            cmds.motors[2].kp = 1.2;
+            cmds.motors[2].kd = 0.05;
+            // 限制力矩输出
+        for (int i = 0; i < 3; ++i)
+        {
+            motor_cmds.motors[i].pos = cmds.motors[i].pos;
+            motor_cmds.motors[i].vw = cmds.motors[i].vw;
+            motor_cmds.motors[i].tau = cmds.motors[i].tau;
+            motor_cmds.motors[i].kp = cmds.motors[i].kp;
+            motor_cmds.motors[i].kd = cmds.motors[i].kd;
+            motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5);
+        }
+        publish_motor_cmds();
+        // RCLCPP_INFO(this->get_logger(), "Zeroing motors");
+        return;
+    }
+}
+
+void QuadrupedSubscriber::data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg)
+{
+    data_motors.motors[0].pos = msg->data_motor_0.pos;
+    data_motors.motors[0].vw = msg->data_motor_0.vw;
+    data_motors.motors[0].tau = msg->data_motor_0.tau;
+
+    data_motors.motors[1].pos = msg->data_motor_1.pos;
+    data_motors.motors[1].vw = msg->data_motor_1.vw;
+    data_motors.motors[1].tau = msg->data_motor_1.tau;
+
+    data_motors.motors[2].pos = msg->data_motor_2.pos;
+    data_motors.motors[2].vw = msg->data_motor_2.vw;
+    data_motors.motors[2].tau = msg->data_motor_2.tau;
+
+    // RCLCPP_INFO(this->get_logger(), "Updated DataMotors for motors 0, 1, 2");
+}
+
+void QuadrupedSubscriber::publish_motor_cmds()
+{
+    auto motor_cmds_msg = rc_msgs::msg::MotorCmds();
+
+    // // 设置每个电机的命令
+    // for (int i = 0; i < 3; ++i)
+    // {
+    //     motor_cmds_msg.motor_cmds[i].tau = motor_cmds.motors[i].tau;
+    //     motor_cmds_msg.motor_cmds[i].vw = motor_cmds.motors[i].vw;
+    //     motor_cmds_msg.motor_cmds[i].pos = motor_cmds.motors[i].pos;
+    //     motor_cmds_msg.motor_cmds[i].kp = motor_cmds.motors[i].kp;
+    //     motor_cmds_msg.motor_cmds[i].kd = motor_cmds.motors[i].kd;
+    // }
+        // 设置每个电机的命令
+        motor_cmds_msg.motor_cmd_0.tau = motor_cmds.motors[0].tau;
+        motor_cmds_msg.motor_cmd_0.vw = motor_cmds.motors[0].vw;
+        motor_cmds_msg.motor_cmd_0.pos = motor_cmds.motors[0].pos;
+        motor_cmds_msg.motor_cmd_0.kp = motor_cmds.motors[0].kp;
+        motor_cmds_msg.motor_cmd_0.kd = motor_cmds.motors[0].kd;
+    
+        motor_cmds_msg.motor_cmd_1.tau = motor_cmds.motors[1].tau;
+        motor_cmds_msg.motor_cmd_1.vw = motor_cmds.motors[1].vw;
+        motor_cmds_msg.motor_cmd_1.pos = motor_cmds.motors[1].pos;
+        motor_cmds_msg.motor_cmd_1.kp = motor_cmds.motors[1].kp;
+        motor_cmds_msg.motor_cmd_1.kd = motor_cmds.motors[1].kd;
+    
+        motor_cmds_msg.motor_cmd_2.tau = motor_cmds.motors[2].tau;
+        motor_cmds_msg.motor_cmd_2.vw = motor_cmds.motors[2].vw;
+        motor_cmds_msg.motor_cmd_2.pos = motor_cmds.motors[2].pos;
+        motor_cmds_msg.motor_cmd_2.kp = motor_cmds.motors[2].kp;
+        motor_cmds_msg.motor_cmd_2.kd = motor_cmds.motors[2].kd;
+    
+
+    // 发布 MotorCmds 消息
+    motor_cmds_publisher_->publish(motor_cmds_msg);
+}
+
+// 限制输出力矩的函数
+float QuadrupedSubscriber::limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit)
+{
+    // 读取结构体中的参数
+    float tff = motor_cmd.tau;  // 前馈力矩
+    float kp = motor_cmd.kp;    // 位置刚度
+    float kd = motor_cmd.kd;    // 速度阻尼
+    float q = motor_cmd.pos*6.33;    // 期望角度位置
+    float q0 = data_motor.pos*6.33;  // 当前角度位置
+    float dq = motor_cmd.vw*6.33;    // 期望角速度
+    float dq0 = data_motor.vw*6.33;  // 当前角速度
+
+    // 计算输出力矩
+    float tau = tff + kp * (q - q0) + kd * (dq - dq0);
+
+    // 如果力矩超过限制值，调整前馈力矩
+    if (tau > limit)
+    {
+        tff -= (tau - limit) / 2;
+        tau = limit;
+        tff = tau - kp * (q - q0) - kd * (dq - dq0);
+        return tff;
+    }else if (tau < -limit)
+    {
+        tff += (tau + limit) / 2;
+        tau = -limit;
+        tff = tau - kp * (q - q0) - kd * (dq - dq0);
+        return tff;
+    }
+    return tff;
+}
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<QuadrupedSubscriber>());
+    rclcpp::shutdown();
+    return 0;
+}

src/unitree_motor_serial_driver/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.8)
+project(unitree_motor_serial_driver)
+
+# 设置 C++ 标准
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
+
+# 包含 ROS 2 依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rc_msgs REQUIRED)
+
+# 包含目录和库路径
+include_directories(include)
+link_directories(lib)
+
+# 设置全局 RPATH
+set(CMAKE_INSTALL_RPATH "${CMAKE_CURRENT_SOURCE_DIR}/lib")
+set(CMAKE_BUILD_RPATH "${CMAKE_CURRENT_SOURCE_DIR}/lib")
+
+# 根据系统架构选择库
+if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64") 
+    set(EXTRA_LIBS libUnitreeMotorSDK_Arm64.so)
+else()
+    set(EXTRA_LIBS libUnitreeMotorSDK_Linux64.so)
+endif()
+
+# 创建组件库
+add_library(unitree_motor_components SHARED
+  src/unitree_motor_serial_driver.cpp
+)
+ament_target_dependencies(unitree_motor_components
+  rclcpp
+  rclcpp_components
+  rc_msgs
+)
+target_link_libraries(unitree_motor_components ${EXTRA_LIBS})
+rclcpp_components_register_nodes(unitree_motor_components
+  "unitree_motor_serial_driver::MotorControlNode"
+)
+
+# # 创建独立可执行文件
+# add_executable(goM8010_6_motor src/goM8010_6_motor.cpp)
+# ament_target_dependencies(goM8010_6_motor rclcpp rc_msgs)
+# target_link_libraries(goM8010_6_motor ${EXTRA_LIBS})
+
+# 创建节点可执行文件（非组件方式）
+# add_executable(unitree_motor_serial_driver_node src/unitree_motor_serial_driver_node.cpp)
+# target_link_libraries(unitree_motor_serial_driver_node unitree_motor_components)
+# ament_target_dependencies(unitree_motor_serial_driver_node rclcpp)
+
+# 安装目标
+install(TARGETS
+  # goM8010_6_motor
+  # unitree_motor_serial_driver_node
+  unitree_motor_components
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+# 安装头文件
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+# ROS 2 包配置
+ament_package()

src/unitree_motor_serial_driver/include/IOPort/IOPort.h

@@ -0,0 +1,41 @@
+#ifndef __IOPORT_H
+#define __IOPORT_H
+
+#include <stdint.h>
+#include <unistd.h>
+#include <vector>
+#include "unitreeMotor/unitreeMotor.h"
+
+enum class BlockYN{
+    YES,
+    NO
+};
+
+class IOPort{
+public:
+    IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){
+        resetIO(blockYN, recvLength, timeOutUs);
+    }
+    virtual ~IOPort(){}
+    virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
+    virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
+    virtual size_t recv(uint8_t *recvMsg) = 0;
+    virtual bool sendRecv(std::vector<MotorCmd> &sendVec, std::vector<MotorData> &recvVec) = 0;
+    void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
+protected:
+    BlockYN _blockYN = BlockYN::NO;
+    size_t _recvLength;
+    timeval _timeout;
+    timeval _timeoutSaved;
+};
+
+inline void IOPort::resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs){
+    _blockYN = blockYN;
+    _recvLength = recvLength;
+    _timeout.tv_sec  = timeOutUs / 1000000;
+    _timeout.tv_usec = timeOutUs % 1000000;
+    _timeoutSaved = _timeout;
+}
+
+
+#endif  // z1_lib_IOPORT_H

src/unitree_motor_serial_driver/include/crc/crc32.h

@@ -0,0 +1,33 @@
+#ifndef CRC32_H
+#define CRC32_H
+
+#include <stdint.h>
+
+inline uint32_t crc32_core(uint32_t* ptr, uint32_t len){
+    uint32_t xbit = 0;
+    uint32_t data = 0;
+    uint32_t CRC32 = 0xFFFFFFFF;
+    const uint32_t dwPolynomial = 0x04c11db7;
+    for (uint32_t i = 0; i < len; i++)
+    {
+        xbit = 1 << 31;
+        data = ptr[i];
+        for (uint32_t bits = 0; bits < 32; bits++)
+        {
+            if (CRC32 & 0x80000000)
+            {
+                CRC32 <<= 1;
+                CRC32 ^= dwPolynomial;
+            }
+            else
+                CRC32 <<= 1;
+            if (data & xbit)
+                CRC32 ^= dwPolynomial;
+
+            xbit >>= 1;
+        }
+    }
+    return CRC32;
+}
+
+#endif  // CRC32_H

src/unitree_motor_serial_driver/include/crc/crc_ccitt.h

@@ -0,0 +1,67 @@
+#ifndef __CRC_CCITT_H
+#define __CRC_CCITT_H
+
+/*
+ * This mysterious table is just the CRC of each possible byte. It can be
+ * computed using the standard bit-at-a-time methods. The polynomial can
+ * be seen in entry 128, 0x8408. This corresponds to x^0 + x^5 + x^12.
+ * Add the implicit x^16, and you have the standard CRC-CCITT.
+ * https://github.com/torvalds/linux/blob/5bfc75d92efd494db37f5c4c173d3639d4772966/lib/crc-ccitt.c
+ */
+uint16_t const crc_ccitt_table[256] = {
+	0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
+	0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
+	0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+	0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
+	0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
+	0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+	0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
+	0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
+	0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+	0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
+	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
+	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
+	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
+	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
+	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
+	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
+	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
+	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
+	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
+	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
+	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
+	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
+	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
+	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
+	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
+};
+
+
+static uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c)
+{
+	return (crc >> 8) ^ crc_ccitt_table[(crc ^ c) & 0xff];
+}
+
+/**
+ *	crc_ccitt - recompute the CRC (CRC-CCITT variant) for the data
+ *	buffer
+ *	@crc: previous CRC value
+ *	@buffer: data pointer
+ *	@len: number of bytes in the buffer
+ */
+inline uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
+{
+	while (len--)
+		crc = crc_ccitt_byte(crc, *buffer++);
+	return crc;
+}
+
+
+#endif

src/unitree_motor_serial_driver/include/serialPort/SerialPort.h

@@ -0,0 +1,98 @@
+#ifndef __SERIALPORT_H
+#define __SERIALPORT_H
+
+/* 
+High frequency serial communication,
+Not that common, but useful for motor communication.
+*/
+#include <termios.h>
+#include <sys/select.h>
+#include <string>
+#include <string.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/serial.h>    
+#include <unistd.h>
+#include <iostream>
+
+#include "serialPort/include/errorClass.h"
+#include "unitreeMotor/unitreeMotor.h"
+#include "IOPort/IOPort.h"
+
+enum class bytesize_t{
+    fivebits,
+    sixbits,
+    sevenbits,
+    eightbits
+};
+
+enum class parity_t{
+    parity_none,
+    parity_odd,
+    parity_even,
+    parity_mark,
+    parity_space
+};
+
+enum class stopbits_t{
+    stopbits_one,
+    stopbits_two,
+    stopbits_one_point_five
+};
+
+enum class flowcontrol_t{
+    flowcontrol_none,
+    flowcontrol_software,
+    flowcontrol_hardware
+};
+
+class SerialPort : public IOPort{
+public:
+    SerialPort(const std::string &portName,
+            size_t recvLength = 16,
+            uint32_t baudrate = 4000000,
+            size_t timeOutUs = 20000, 
+            BlockYN blockYN = BlockYN::NO,
+            bytesize_t bytesize = bytesize_t::eightbits,
+            parity_t parity = parity_t::parity_none,
+            stopbits_t stopbits = stopbits_t::stopbits_one,
+            flowcontrol_t flowcontrol = flowcontrol_t::flowcontrol_none);
+    virtual ~SerialPort();
+    void resetSerial(size_t recvLength = 16,
+                     uint32_t baudrate = 4000000,
+                     size_t timeOutUs = 20000,
+                     BlockYN blockYN = BlockYN::NO,
+                     bytesize_t bytesize = bytesize_t::eightbits,
+                     parity_t parity = parity_t::parity_none,
+                     stopbits_t stopbits = stopbits_t::stopbits_one,
+                     flowcontrol_t flowcontrol = flowcontrol_t::flowcontrol_none);
+    size_t send(uint8_t *sendMsg, size_t sendLength);
+    size_t recv(uint8_t *recvMsg, size_t recvLength);
+    size_t recv(uint8_t *recvMsg);
+    bool sendRecv(uint8_t *sendMsg, uint8_t *recvMsg, size_t sendLength);
+    bool sendRecv(MotorCmd* sendMsg, MotorData* recvMsg);
+    bool sendRecv(std::vector<MotorCmd> &sendVec, std::vector<MotorData> &recvVec);
+    void test();
+private:
+    void _open();
+    void _set();
+    void _close();
+    size_t _nonBlockRecv(uint8_t *recvMsg, size_t readLen);
+    std::string _portName;
+    uint32_t _baudrate;
+    bytesize_t _bytesize;
+    parity_t _parity;
+    stopbits_t _stopbits;
+    flowcontrol_t _flowcontrol;
+    bool _xonxoff;
+    bool _rtscts;
+    int _fd;
+    fd_set _rSet;
+
+};
+
+
+
+
+#endif  // SERIALPORT_H

src/unitree_motor_serial_driver/include/serialPort/include/errorClass.h

@@ -0,0 +1,52 @@
+#ifndef __ERRORCLASS_H
+#define __ERRORCLASS_H
+
+#include <string>
+#include <exception>
+#include <stdexcept>
+#include <sstream>
+#include <cstring>
+
+#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
+__LINE__, (message) )
+
+class IOException : public std::exception
+{
+  // Disable copy constructors
+  IOException& operator=(const IOException&);
+  std::string file_;
+  int line_;
+  std::string e_what_;
+  int errno_;
+public:
+  explicit IOException (std::string file, int line, int errnum)
+    : file_(file), line_(line), errno_(errnum) {
+      std::stringstream ss;
+#if defined(_WIN32) && !defined(__MINGW32__)
+      char error_str [1024];
+      strerror_s(error_str, 1024, errnum);
+#else
+      char * error_str = strerror(errnum);
+#endif
+      ss << "IO Exception (" << errno_ << "): " << error_str;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  explicit IOException (std::string file, int line, const char * description)
+    : file_(file), line_(line), errno_(0) {
+      std::stringstream ss;
+      ss << "IO Exception: " << description;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  virtual ~IOException() throw() {}
+  IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}
+
+  int getErrorNumber () const { return errno_; }
+
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+#endif  // ERRORCLASS_H

src/unitree_motor_serial_driver/include/unitreeMotor/include/motor_msg_A1B1.h

@@ -0,0 +1,162 @@
+#ifndef MOTOR_A1B1_MSG
+#define MOTOR_A1B1_MSG
+
+#include <stdint.h>
+typedef int16_t q15_t;
+
+#pragma pack(1)
+
+// 发送用单个数据数据结构
+typedef union{
+        int32_t           L;
+        uint8_t       u8[4];
+       uint16_t      u16[2];
+       uint32_t         u32;
+          float           F;
+}COMData32;
+
+typedef struct {
+	// 定义 数据包头
+    unsigned char  start[2];     // 包头
+	unsigned char  motorID;      // 电机ID  0,1,2,3 ...   0xBB 表示向所有电机广播（此时无返回）
+	unsigned char  reserved;
+}COMHead;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct { 
+	
+	   uint8_t  fan_d;       // 关节上的散热风扇转速
+	   uint8_t  Fmusic;      // 电机发声频率   /64*1000   15.625f 频率分度
+	   uint8_t  Hmusic;      // 电机发声强度   推荐值4  声音强度 0.1 分度  
+	   uint8_t  reserved4;
+	
+	   uint8_t  FRGB[4];     // 足端LED 
+	
+}LowHzMotorCmd;
+
+typedef struct {  // 以 4个字节一组排列 ，不然编译器会凑整
+	// 定义 数据
+    uint8_t  mode;        // 关节模式选择
+    uint8_t  ModifyBit;   // 电机控制参数修改位
+    uint8_t  ReadBit;     // 电机控制参数发送位
+    uint8_t  reserved;
+
+    COMData32  Modify;     // 电机参数修改 的数据 
+    //实际给FOC的指令力矩为：
+    //K_P*delta_Pos + K_W*delta_W + T
+    q15_t     T;      // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
+    q15_t     W;      // 期望关节速度 （电机本身的速度） x128,       8 + 7描述	
+    int32_t   Pos;      // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）
+
+    q15_t    K_P;      // 关节刚度系数 x2048  4+11 描述
+    q15_t    K_W;      // 关节速度系数 x1024  5+10 描述
+
+    uint8_t LowHzMotorCmdIndex;     // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
+    uint8_t LowHzMotorCmdByte;      // 电机低频率控制命令的字节
+	
+     COMData32  Res[1];    // 通讯 保留字节  用于实现别的一些通讯内容
+	
+}MasterComdV3;   // 加上数据包的包头 和CRC 34字节
+
+typedef struct {
+	// 定义 电机控制命令数据包	
+    COMHead head;    
+    MasterComdV3 Mdata;
+    COMData32 CRCdata;
+}MasterComdDataV3;//返回数据
+
+// typedef struct {
+// 	// 定义 总得485 数据包
+	
+//   MasterComdData M1;
+// 	MasterComdData M2;
+// 	MasterComdData M3;
+	
+// }DMA485TxDataV3;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct {  // 以 4个字节一组排列 ，不然编译器会凑整
+    // 定义 数据
+    uint8_t  mode;        // 当前关节模式
+    uint8_t  ReadBit;     // 电机控制参数修改     是否成功位
+    int8_t  Temp;        // 电机当前平均温度   
+    uint8_t  MError;      // 电机错误 标识
+ 
+    COMData32  Read;     // 读取的当前 电机 的控制数据 
+    int16_t     T;      // 当前实际电机输出力矩       7 + 8 描述
+
+    int16_t     W;      // 当前实际电机速度（高速）   8 + 7 描述
+    float      LW;      // 当前实际电机速度（低速）   
+
+    int16_t     W2;      // 当前实际关节速度（高速）   8 + 7 描述
+    float      LW2;      // 当前实际关节速度（低速）   
+
+    int16_t    Acc;           // 电机转子加速度       15+0 描述  惯量较小
+    int16_t    OutAcc;        // 输出轴加速度         12+3 描述  惯量较大
+		 
+    int32_t   Pos;      // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
+    int32_t   Pos2;     // 关节编码器位置(输出编码器)
+
+    int16_t     gyro[3];  // 电机驱动板6轴传感器数据
+    int16_t     acc[3];   
+
+    // 力传感器的数据   
+    int16_t     Fgyro[3];  //  
+    int16_t     Facc[3];
+    int16_t     Fmag[3];
+    uint8_t     Ftemp;     // 8位表示的温度  7位（-28~100度）  1位0.5度分辨率
+    
+    int16_t     Force16;   // 力传感器高16位数据
+    int8_t      Force8;    // 力传感器低8位数据
+		
+    uint8_t     FError;    //  足端传感器错误标识
+		
+    int8_t      Res[1];    // 通讯 保留字节
+	
+}ServoComdV3;  // 加上数据包的包头 和CRC 78字节（4+70+4）
+
+typedef struct {
+    // 定义 电机控制命令数据包	
+    COMHead        head;
+    ServoComdV3      Mdata;
+
+    COMData32    CRCdata;
+
+}ServoComdDataV3;	//发送数据
+
+// typedef struct {
+// 	// 定义 总的485 接受数据包
+	
+//   ServoComdDataV3 M[3];
+//  // uint8_t  nullbyte1;
+	
+// }DMA485RxDataV3;
+
+
+#pragma pack()
+
+//  00 00 00 00 00 
+//  00 00 00 00 00 
+//  00 00 00 00 00 
+//  00 00 00
+// 数据包默认初始化 
+// 主机发送的数据包 
+/*
+                 Tx485Data[_FR][i].head.start[0] = 0xFE ;     Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头					 
+				 Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF;    Tx485Data[_FR][i].Mdata.mode = 0;   // 默认不修改数据 和 电机的默认工作模式				
+				 Tx485Data[_FR][i].head.motorID = i;    0                                          // 目标电机标号
+				 Tx485Data[_FR][i].Mdata.T = 0.0f;                           // 默认目标关节输出力矩                      motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f;     // N.M  转化为 N.CM   IQ8描述
+				 Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80;                   // 默认目标关节位置  不启用位置环          14位分辨率 
+				 Tx485Data[_FR][i].Mdata.W = 16000.0f;                       // 默认目标关节速度  不启用速度环          1+8+7描述     motor1.Target_Speed =  motorRxData[1].Mdata.W*0.0078125f;   // 单位 rad/s	       IQ7描述
+				 Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11));        // 默认关节刚度系数   4+11 描述                     motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11);      // 描述刚度的通讯数据格式  4+11
+				 Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10));        // 默认关节速度系数   5+10 描述                    motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10);    // 描述阻尼的通讯数据格式  5+10
+*/ 
+
+
+#endif

src/unitree_motor_serial_driver/include/unitreeMotor/include/motor_msg_GO-M8010-6.h

@@ -0,0 +1,90 @@
+#ifndef __MOTOR_MSG_GO_M8010_6_H
+#define __MOTOR_MSG_GO_M8010_6_H
+
+#include <stdint.h>
+#define CRC_SIZE        2
+#define CTRL_DAT_SIZE   sizeof(ControlData_t) - CRC_SIZE
+#define DATA_DAT_SIZE   sizeof(MotorData_t) - CRC_SIZE
+
+#pragma pack(1)
+
+/**
+ * @brief 电机模式控制信息
+ * 
+ */
+typedef struct
+{
+    uint8_t id     :4;      // 电机ID: 0,1...,14 15表示向所有电机广播数据(此时无返回)
+    uint8_t status :3;      // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
+    uint8_t none   :1;      // 保留位
+} RIS_Mode_t;   // 控制模式 1Byte
+
+/**
+ * @brief 电机状态控制信息
+ * 
+ */
+typedef struct
+{
+    int16_t tor_des;        // 期望关节输出扭矩 unit: N.m     (q8)
+    int16_t spd_des;        // 期望关节输出速度 unit: rad/s   (q7)
+    int32_t pos_des;        // 期望关节输出位置 unit: rad     (q15)
+    uint16_t  k_pos;        // 期望关节刚度系数 unit: 0.0-1.0 (q15)
+    uint16_t  k_spd;        // 期望关节阻尼系数 unit: 0.0-1.0 (q15)
+    
+} RIS_Comd_t;   // 控制参数 12Byte
+
+/**
+ * @brief 电机状态反馈信息
+ * 
+ */
+typedef struct
+{
+    int16_t  torque;        // 实际关节输出扭矩 unit: N.m     (q8)
+    int16_t  speed;         // 实际关节输出速度 unit: rad/s   (q7)
+    int32_t  pos;           // 实际关节输出位置 unit: W       (q15)
+    int8_t   temp;          // 电机温度: -128~127°C 90°C时触发温度保护
+    uint8_t  MError :3;     // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留
+    uint16_t force  :12;    // 足端气压传感器数据 12bit (0-4095)
+    uint8_t  none   :1;     // 保留位
+} RIS_Fbk_t;   // 状态数据 11Byte
+
+
+#pragma pack()
+
+#pragma pack(1)
+
+/**
+ * @brief 控制数据包格式
+ * 
+ */
+typedef struct
+{
+    uint8_t head[2];    // 包头         2Byte
+    RIS_Mode_t mode;    // 电机控制模式  1Byte
+    RIS_Comd_t comd;    // 电机期望数据 12Byte
+    uint16_t   CRC16;   // CRC          2Byte
+
+} ControlData_t;    // 主机控制命令     17Byte
+
+/**
+ * @brief 电机反馈数据包格式
+ * 
+ */
+typedef struct
+{
+    uint8_t head[2];    // 包头         2Byte
+    RIS_Mode_t mode;    // 电机控制模式  1Byte
+    RIS_Fbk_t   fbk;    // 电机反馈数据 11Byte
+    uint16_t  CRC16;    // CRC          2Byte
+
+} MotorData_t;      // 电机返回数据     16Byte
+
+#pragma pack()
+
+#endif
+
+
+
+
+
+

src/unitree_motor_serial_driver/include/unitreeMotor/unitreeMotor.h

@@ -0,0 +1,74 @@
+#ifndef __UNITREEMOTOR_H
+#define __UNITREEMOTOR_H
+
+#include "unitreeMotor/include/motor_msg_GO-M8010-6.h" 
+#include "unitreeMotor/include/motor_msg_A1B1.h"  
+#include <stdint.h>
+#include <iostream>
+
+enum class MotorType{
+    A1, // 4.8M baudrate
+    B1, // 6.0M baudrate
+    GO_M8010_6
+};
+
+enum class MotorMode{
+    BRAKE, 
+    FOC, 
+    CALIBRATE
+};
+
+struct MotorCmd{
+    public:
+        MotorCmd(){}
+        MotorType motorType;
+	    int hex_len;                   
+        unsigned short id;              
+        unsigned short mode;           
+        float tau;                        
+        float dq;                       
+        float q;                     
+        float kp;                     
+        float kd;                     
+
+        void modify_data(MotorCmd* motor_s); 
+        uint8_t* get_motor_send_data();
+
+        COMData32 Res;                
+    private:
+        ControlData_t  GO_M8010_6_motor_send_data;  
+        MasterComdDataV3  A1B1_motor_send_data; 
+};
+
+struct MotorData{
+    public:
+        MotorData(){}
+        MotorType motorType;
+        int hex_len;                   
+        unsigned char motor_id;         
+        unsigned char mode;             
+        int temp;                      
+        int merror;          
+        float tau;                       
+        float dq;                       
+        float q;                     
+       
+        bool correct = false;                  
+        bool extract_data(MotorData* motor_r);
+        uint8_t* get_motor_recv_data();
+       
+        int footForce;
+        float LW;                      
+        int Acc;                      
+
+        float gyro[3];                  
+        float acc[3];
+    private:
+        MotorData_t GO_M8010_6_motor_recv_data;    
+        ServoComdDataV3 A1B1_motor_recv_data;     
+};
+
+// Utility Function
+int queryMotorMode(MotorType motortype,MotorMode motormode);
+float queryGearRatio(MotorType motortype);
+#endif  // UNITREEMOTOR_H

src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp

@@ -0,0 +1,41 @@
+#ifndef UNITREE_MOTOR_SERIAL_DRIVER_HPP
+#define UNITREE_MOTOR_SERIAL_DRIVER_HPP
+
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "serialPort/SerialPort.h"
+#include "unitreeMotor/unitreeMotor.h"
+#include "rc_msgs/msg/motor_cmds.hpp"
+#include "rc_msgs/msg/data_motors.hpp"
+
+namespace unitree_motor_serial_driver
+{
+
+struct Motor_t
+{
+    MotorCmd cmd;
+    MotorData data;
+};
+
+Motor_t motor[3]; // 修改为数组以支持多个电机
+
+class MotorControlNode : public rclcpp::Node
+{
+public:
+    explicit MotorControlNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+    
+private:
+    void control_motor();
+    void motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg);
+    void Motor_Ctrl_Offline(Motor_t *motor_s);
+    SerialPort serial_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Subscription<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmd_sub_;
+    rclcpp::Publisher<rc_msgs::msg::DataMotors>::SharedPtr data_motor_pub_;
+    int timer_count_ = 0;
+};
+
+}
+
+#endif

src/unitree_motor_serial_driver/launch/goM8010_6_motor.launch.py

@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='unitree_motor_serial_driver',
+            executable='goM8010_6_motor',
+            name='goM8010_6_motor',
+            output='screen',
+            parameters=[
+
+            ]
+        )
+    ])

src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py

@@ -0,0 +1,103 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    # 左前腿容器 - LF (Left Front)
+    lf_container = ComposableNodeContainer(
+        name='lf_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_lf',
+                parameters=[
+                    {'serial_port': '/dev/ttyACM0'}
+                ],
+                remappings=[
+                    ('motor_cmds', 'LF/motor_cmds'),
+                    ('data_motors', 'LF/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+    
+    # 右前腿容器 - RF (Right Front)
+    rf_container = ComposableNodeContainer(
+        name='rf_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_rf',
+                parameters=[
+                    {'serial_port': '/dev/ttyACM1'}
+                ],
+                remappings=[
+                    ('motor_cmds', 'RF/motor_cmds'),
+                    ('data_motors', 'RF/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+    
+    # 左后腿容器 - LR (Left Rear)
+    lr_container = ComposableNodeContainer(
+        name='lr_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_lr',
+                parameters=[
+                    {'serial_port': '/dev/ttyACM2'}
+                ],
+                remappings=[
+                    ('motor_cmds', 'LR/motor_cmds'),
+                    ('data_motors', 'LR/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+    
+    # 右后腿容器 - RR (Right Rear)
+    rr_container = ComposableNodeContainer(
+        name='rr_motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver_rr',
+                parameters=[
+                    {'serial_port': '/dev/ttyACM3'}
+                ],
+                remappings=[
+                    ('motor_cmds', 'RR/motor_cmds'),
+                    ('data_motors', 'RR/data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        lf_container,
+        rf_container,
+        lr_container,
+        rr_container
+    ])

src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py

@@ -0,0 +1,30 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    container = ComposableNodeContainer(
+        name='motor_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='unitree_motor_serial_driver',
+                plugin='unitree_motor_serial_driver::MotorControlNode',
+                name='unitree_motor_serial_driver',
+                parameters=[
+                    {'serial_port': '/dev/ttyACM0'},  # 修改默认串口路径
+                    {'serial_retry_count': 5},
+                    {'serial_retry_delay_ms': 1000}
+                ],
+                remappings=[
+                    ('motor_cmds', 'motor_cmds'),
+                    ('data_motors', 'data_motors')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    return LaunchDescription([container])

src/unitree_motor_serial_driver/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>unitree_motor_serial_driver</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>rc_msgs</depend>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/unitree_motor_serial_driver/src/goM8010_6_motor.cpp

@@ -0,0 +1,51 @@
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "serialPort/SerialPort.h"
+#include "unitreeMotor/unitreeMotor.h"
+
+using namespace std::chrono_literals;
+
+class MotorControlNode : public rclcpp::Node
+{
+public:
+    MotorControlNode()
+        : Node("goM8010_6_motor"), serial_("/dev/ttyACM0")
+    {
+        timer_ = this->create_wall_timer(
+            1ms, std::bind(&MotorControlNode::control_motor, this));
+    }
+
+private:
+    void control_motor()
+    {
+        MotorCmd cmd;
+        MotorData data;
+
+        cmd.motorType = MotorType::GO_M8010_6;
+        data.motorType = MotorType::GO_M8010_6;
+        cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
+        cmd.id = 0;
+        cmd.q = 0.0; 
+        cmd.dq = 0.0;
+        cmd.kp = 0.0;
+        cmd.kd = 0.0;
+        cmd.tau = 0.0;
+
+        serial_.sendRecv(&cmd, &data);
+
+        RCLCPP_INFO(this->get_logger(), "\nMotor.q: %f\nMotor.temp: %d\nMotor.W: %f\nMotor.tau: %f\n",
+                    data.q, data.temp, data.dq, data.tau);
+    }
+
+    SerialPort serial_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<MotorControlNode>());
+    rclcpp::shutdown();
+    return 0;
+}

src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp

@@ -0,0 +1,145 @@
+#include "unitree_motor_serial_driver/unitree_motor_serial_driver.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+namespace unitree_motor_serial_driver
+{
+
+MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
+    : Node("unitree_motor_serial_driver", options),
+      serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
+{
+    // 声明其他参数
+    auto retry_count = this->declare_parameter("serial_retry_count", 5);  // 默认重试5次
+    auto retry_delay_ms = this->declare_parameter("serial_retry_delay_ms", 1000);  // 默认延迟1秒
+
+    bool connection_success = false;
+    int attempts = 0;
+    timer_count_ = 0;
+
+    // 定时器
+    timer_ = this->create_wall_timer(
+        std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
+
+    // 订阅 MotorCmds 消息
+    motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::MotorCmds>(
+        "motor_cmds", 10, std::bind(&MotorControlNode::motor_cmd_callback, this, std::placeholders::_1));
+
+    // 发布 DataMotors 消息
+    data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataMotors>("data_motors", 10);
+}
+
+void MotorControlNode::motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg)
+{
+    // 将接收到的 MotorCmds 消息分别赋值给 motor_t 数组
+    motor[0].cmd.tau = msg->motor_cmd_0.tau;
+    motor[0].cmd.dq = msg->motor_cmd_0.vw;
+    motor[0].cmd.q = msg->motor_cmd_0.pos;
+    motor[0].cmd.kp = msg->motor_cmd_0.kp;
+    motor[0].cmd.kd = msg->motor_cmd_0.kd;
+
+    motor[1].cmd.tau = msg->motor_cmd_1.tau;
+    motor[1].cmd.dq = msg->motor_cmd_1.vw;
+    motor[1].cmd.q = msg->motor_cmd_1.pos;
+    motor[1].cmd.kp = msg->motor_cmd_1.kp;
+    motor[1].cmd.kd = msg->motor_cmd_1.kd;
+
+    motor[2].cmd.tau = msg->motor_cmd_2.tau;
+    motor[2].cmd.dq = msg->motor_cmd_2.vw;
+    motor[2].cmd.q = msg->motor_cmd_2.pos;
+    motor[2].cmd.kp = msg->motor_cmd_2.kp;
+    motor[2].cmd.kd = msg->motor_cmd_2.kd;
+
+    timer_count_ = 0;
+}
+
+void MotorControlNode::control_motor()
+{
+    rc_msgs::msg::DataMotors motor_msg;
+
+    timer_count_++;
+
+    for (int i = 0; i < 3; ++i)
+    {
+        MotorCmd cmd;
+        MotorData data;
+        cmd.motorType = MotorType::GO_M8010_6;
+        data.motorType = MotorType::GO_M8010_6;
+        cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
+        cmd.id = i;
+        cmd.kp = motor[i].cmd.kp;
+        cmd.kd = motor[i].cmd.kd;
+        cmd.q = motor[i].cmd.q * queryGearRatio(MotorType::GO_M8010_6);
+        cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6);
+        cmd.tau = motor[i].cmd.tau;
+
+        if (timer_count_ > 50)
+        {
+            Motor_Ctrl_Offline(&motor[i]);
+            timer_count_ = 200;
+        }
+
+        // 添加异常处理以防止程序崩溃
+        try {
+            // 优化串口通信，减少阻塞
+            if (!serial_.sendRecv(&cmd, &data))
+            {
+                RCLCPP_WARN(this->get_logger(), "Failed to communicate with motor %d", i);
+                continue;
+            }
+
+            // 填充 DataMotors 消息
+            if (i == 0)
+            {
+                motor_msg.data_motor_0.tau = data.tau;
+                motor_msg.data_motor_0.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
+                motor_msg.data_motor_0.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
+            }
+            else if (i == 1)
+            {
+                motor_msg.data_motor_1.tau = data.tau;
+                motor_msg.data_motor_1.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
+                motor_msg.data_motor_1.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
+            }
+            else if (i == 2)
+            {
+                motor_msg.data_motor_2.tau = data.tau;
+                motor_msg.data_motor_2.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
+                motor_msg.data_motor_2.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
+            }
+        } catch (const std::exception &e) {
+            RCLCPP_ERROR(this->get_logger(), 
+                "Exception during communication with motor %d: %s", i, e.what());
+            
+            // 尝试重新初始化串口连接
+            try {
+                auto serial_port = this->get_parameter("serial_port").as_string();
+                RCLCPP_INFO(this->get_logger(), "Attempting to reopen serial port: %s", serial_port.c_str());
+                serial_ = SerialPort(serial_port);
+                RCLCPP_INFO(this->get_logger(), "Successfully reopened serial port: %s", serial_port.c_str());
+            } catch (const std::exception &e2) {
+                RCLCPP_ERROR(this->get_logger(), 
+                    "Failed to reopen serial port: %s", e2.what());
+            }
+            
+            // 将电机设置为安全状态
+            Motor_Ctrl_Offline(&motor[i]);
+        }
+    }
+
+    // 发布 DataMotors 消息
+    data_motor_pub_->publish(motor_msg);
+}
+
+void MotorControlNode::Motor_Ctrl_Offline(Motor_t *motor_s)
+{
+    motor_s->cmd.kp = 0.0;
+    motor_s->cmd.kd = 0.01;
+    motor_s->cmd.q = 0.0;
+    motor_s->cmd.dq = 0.0;
+    motor_s->cmd.tau = 0.0;
+}
+
+} // namespace unitree_motor_serial_driver
+
+// 注册组件
+RCLCPP_COMPONENTS_REGISTER_NODE(unitree_motor_serial_driver::MotorControlNode)