可以实现12个电机读取和控制

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=[
+                    ('data_leg_control', 'LF/data_leg_control'),
+                    ('data_leg_motor', 'LF/data_leg_motor')
+                ]
+            )
+        ],
+        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=[
+                    ('data_leg_control', 'RF/data_leg_control'),
+                    ('data_leg_motor', 'RF/data_leg_motor')
+                ]
+            )
+        ],
+        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=[
+                    ('data_leg_control', 'LR/data_leg_control'),
+                    ('data_leg_motor', 'LR/data_leg_motor')
+                ]
+            )
+        ],
+        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=[
+                    ('data_leg_control', 'RR/data_leg_control'),
+                    ('data_leg_motor', 'RR/data_leg_motor')
+                ]
+            )
+        ],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        lf_container,
+        rf_container,
+        lr_container,
+        rr_container
+    ])

src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py

@@ -14,7 +14,9 @@ def generate_launch_description():
                 plugin='unitree_motor_serial_driver::MotorControlNode',
                 name='unitree_motor_serial_driver',
                 parameters=[
-                    {'serial_port': '/dev/ttyACM0'}
+                    {'serial_port': '/dev/ttyACM0'},  # 修改默认串口路径
+                    {'serial_retry_count': 5},
+                    {'serial_retry_delay_ms': 1000}
                 ],
                 remappings=[
                     ('data_leg_control', 'data_leg_control'),

src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp

@@ -6,22 +6,14 @@ namespace unitree_motor_serial_driver
 
     MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
         : Node("unitree_motor_serial_driver", options),
-          serial_("/dev/ttyACM0")  // 在初始化列表中提供必要参数
+          serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
     {
-        // 声明参数
+        // 声明其他参数
-        auto serial_port = this->declare_parameter("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秒
     
-        try {
+        bool connection_success = false;
-            // 用声明的参数重新初始化串口
+        int attempts = 0;
-            if (serial_port != "/dev/ttyACM0") {
-                serial_ = SerialPort(serial_port);
-            }
-            RCLCPP_INFO(this->get_logger(), "Successfully opened serial port: %s", serial_port.c_str());
-        } catch (const std::exception &e) {
-            RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s, error: %s", 
-                        serial_port.c_str(), e.what());
-            throw;
-        }
     
         // 其余代码保持不变
         timer_ = this->create_wall_timer(
@@ -31,6 +23,7 @@ namespace unitree_motor_serial_driver
         data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataLeg>("data_leg_motor", 10);
     }
         
+
 void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg)
 {
     // 将接收到的 DataLeg 消息分别赋值给 motor_t 数组
@@ -73,6 +66,8 @@ void MotorControlNode::control_motor()
             timer_count_++;
         }
 
+        // 添加异常处理以防止程序崩溃
+        try {
             // 优化串口通信，减少阻塞
             if (!serial_.sendRecv(&cmd, &data))
             {
@@ -99,6 +94,24 @@ void MotorControlNode::control_motor()
                 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]);
+        }
     }
 
     // 发布 DataLeg 消息