From 761ede8596cacc60972379150a9869d9fd03ee5d Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 9 Apr 2025 02:50:36 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BA=95=E5=B1=82=E6=8E=A7=E5=88=B6=E5=AE=8C?= =?UTF-8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/robot_bringup.launch.py | 16 ++-- .../unitree_motor_serial_driver.hpp | 9 +- .../launch/leg_ctrl.launch.py | 16 ++-- .../unitree_motor_serial_driver.launch.py | 4 +- .../src/unitree_motor_serial_driver.cpp | 86 +++++++++++-------- 5 files changed, 73 insertions(+), 58 deletions(-) diff --git a/src/robot_bringup/launch/robot_bringup.launch.py b/src/robot_bringup/launch/robot_bringup.launch.py index dc15ae6..6baa4d6 100644 --- a/src/robot_bringup/launch/robot_bringup.launch.py +++ b/src/robot_bringup/launch/robot_bringup.launch.py @@ -17,8 +17,8 @@ def generate_launch_description(): 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') + ('motor_cmds', 'LF/motor_cmds'), + ('data_motors', 'LF/data_motors') ] ) ], @@ -37,8 +37,8 @@ def generate_launch_description(): 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') + ('motor_cmds', 'RF/motor_cmds'), + ('data_motors', 'RF/data_motors') ] ) ], @@ -57,8 +57,8 @@ def generate_launch_description(): 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') + ('motor_cmds', 'LR/motor_cmds'), + ('data_motors', 'LR/data_motors') ] ) ], @@ -77,8 +77,8 @@ def generate_launch_description(): 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') + ('motor_cmds', 'RR/motor_cmds'), + ('data_motors', 'RR/data_motors') ] ) ], diff --git a/src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp b/src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp index 6bb5342..fab08c8 100644 --- a/src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp +++ b/src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp @@ -6,7 +6,8 @@ #include "rclcpp/rclcpp.hpp" #include "serialPort/SerialPort.h" #include "unitreeMotor/unitreeMotor.h" -#include "rc_msgs/msg/data_leg.hpp" +#include "rc_msgs/msg/motor_cmds.hpp" +#include "rc_msgs/msg/data_motors.hpp" namespace unitree_motor_serial_driver { @@ -26,12 +27,12 @@ public: private: void control_motor(); - void data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg); + 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::SharedPtr data_control_sub_; - rclcpp::Publisher::SharedPtr data_motor_pub_; + rclcpp::Subscription::SharedPtr motor_cmd_sub_; + rclcpp::Publisher::SharedPtr data_motor_pub_; int timer_count_ = 0; }; diff --git a/src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py b/src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py index 3d0cdb9..de7382c 100644 --- a/src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py +++ b/src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py @@ -18,8 +18,8 @@ def generate_launch_description(): {'serial_port': '/dev/ttyACM0'} ], remappings=[ - ('data_leg_control', 'LF/data_leg_control'), - ('data_leg_motor', 'LF/data_leg_motor') + ('motor_cmds', 'LF/motor_cmds'), + ('data_motors', 'LF/data_motors') ] ) ], @@ -41,8 +41,8 @@ def generate_launch_description(): {'serial_port': '/dev/ttyACM1'} ], remappings=[ - ('data_leg_control', 'RF/data_leg_control'), - ('data_leg_motor', 'RF/data_leg_motor') + ('motor_cmds', 'RF/motor_cmds'), + ('data_motors', 'RF/data_motors') ] ) ], @@ -64,8 +64,8 @@ def generate_launch_description(): {'serial_port': '/dev/ttyACM2'} ], remappings=[ - ('data_leg_control', 'LR/data_leg_control'), - ('data_leg_motor', 'LR/data_leg_motor') + ('motor_cmds', 'LR/motor_cmds'), + ('data_motors', 'LR/data_motors') ] ) ], @@ -87,8 +87,8 @@ def generate_launch_description(): {'serial_port': '/dev/ttyACM3'} ], remappings=[ - ('data_leg_control', 'RR/data_leg_control'), - ('data_leg_motor', 'RR/data_leg_motor') + ('motor_cmds', 'RR/motor_cmds'), + ('data_motors', 'RR/data_motors') ] ) ], 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 d211ba7..49962b0 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 @@ -19,8 +19,8 @@ def generate_launch_description(): {'serial_retry_delay_ms': 1000} ], remappings=[ - ('data_leg_control', 'data_leg_control'), - ('data_leg_motor', 'data_leg_motor') + ('motor_cmds', 'motor_cmds'), + ('data_motors', 'data_motors') ] ) ], 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 38f978d..82e4074 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 @@ -4,43 +4,57 @@ namespace unitree_motor_serial_driver { - MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options) - : Node("unitree_motor_serial_driver", options), - serial_(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秒 - - 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)); - data_control_sub_ = this->create_subscription( - "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) +MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options) + : Node("unitree_motor_serial_driver", options), + serial_(this->declare_parameter("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值 { - // 将接收到的 DataLeg 消息分别赋值给 motor_t 数组 - for (int i = 0; i < 3; ++i) - { - motor[i].cmd.kp = msg->data_control_0.kp; - motor[i].cmd.kd = msg->data_control_0.kd; - motor[i].cmd.q = msg->data_control_0.pos * queryGearRatio(MotorType::GO_M8010_6); - motor[i].cmd.dq = msg->data_control_0.vw * queryGearRatio(MotorType::GO_M8010_6); - motor[i].cmd.tau = msg->data_control_0.tau; - } + // 声明其他参数 + 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( + "motor_cmds", 10, std::bind(&MotorControlNode::motor_cmd_callback, this, std::placeholders::_1)); + + // 发布 DataMotors 消息 + data_motor_pub_ = this->create_publisher("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::DataLeg motor_msg; + rc_msgs::msg::DataMotors motor_msg; for (int i = 0; i < 3; ++i) { @@ -52,11 +66,11 @@ void MotorControlNode::control_motor() cmd.id = i; cmd.kp = motor[i].cmd.kp; cmd.kd = motor[i].cmd.kd; - cmd.q = motor[i].cmd.q; - cmd.dq = motor[i].cmd.dq; + 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_ = 100) + if (timer_count_ == 100) { Motor_Ctrl_Offline(&motor[i]); } @@ -71,7 +85,7 @@ void MotorControlNode::control_motor() continue; } - // 填充 DataLeg 消息 + // 填充 DataMotors 消息 if (i == 0) { motor_msg.data_motor_0.tau = data.tau; @@ -110,7 +124,7 @@ void MotorControlNode::control_motor() } } - // 发布 DataLeg 消息 + // 发布 DataMotors 消息 data_motor_pub_->publish(motor_msg); }