From 2f47572350432a08ef8f91a21e4fd4b24b48c9aa Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 8 Apr 2025 23:52:27 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=AF=E4=BB=A5=E5=AE=9E=E7=8E=B012=E4=B8=AA?= =?UTF-8?q?=E7=94=B5=E6=9C=BA=E8=AF=BB=E5=8F=96=E5=92=8C=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/leg_ctrl.launch.py | 103 ++++++++++++++++++ .../unitree_motor_serial_driver.launch.py | 4 +- .../src/unitree_motor_serial_driver.cpp | 93 +++++++++------- 3 files changed, 159 insertions(+), 41 deletions(-) create mode 100644 src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py diff --git a/src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py b/src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py new file mode 100644 index 0000000..3d0cdb9 --- /dev/null +++ b/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 + ]) \ No newline at end of file diff --git a/src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py b/src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py index 7ae1093..d211ba7 100644 --- a/src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py +++ b/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'), diff --git a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp index 979901b..f93a059 100644 --- a/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp +++ b/src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp @@ -6,23 +6,15 @@ namespace unitree_motor_serial_driver MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options) : Node("unitree_motor_serial_driver", options), - serial_("/dev/ttyACM0") // 在初始化列表中提供必要参数 + serial_(this->declare_parameter("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值 { - // 声明参数 - auto serial_port = this->declare_parameter("serial_port", "/dev/ttyACM0"); - - try { - // 用声明的参数重新初始化串口 - 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; - } - + // 声明其他参数 + 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_ = this->create_wall_timer( std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this)); @@ -30,6 +22,7 @@ namespace unitree_motor_serial_driver "data_leg_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1)); data_motor_pub_ = this->create_publisher("data_leg_motor", 10); } + void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg) { @@ -73,31 +66,51 @@ void MotorControlNode::control_motor() timer_count_++; } - // 优化串口通信，减少阻塞 - if (!serial_.sendRecv(&cmd, &data)) - { - RCLCPP_WARN(this->get_logger(), "Failed to communicate with motor %d", i); - continue; - } + // 添加异常处理以防止程序崩溃 + try { + // 优化串口通信，减少阻塞 + if (!serial_.sendRecv(&cmd, &data)) + { + RCLCPP_WARN(this->get_logger(), "Failed to communicate with motor %d", i); + continue; + } - // 填充 DataLeg 消息 - 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); + // 填充 DataLeg 消息 + 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]); } }