From 90c944d2309cacc30841285476c45b3fc8b9482b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 8 Apr 2025 20:42:18 +0800 Subject: [PATCH] =?UTF-8?q?3=E5=85=B3=E8=8A=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rc_msgs/CMakeLists.txt | 1 + src/rc_msgs/msg/DataLeg.msg | 7 ++ .../unitree_motor_serial_driver.hpp | 11 +- .../src/unitree_motor_serial_driver.cpp | 112 +++++++++++------- 4 files changed, 84 insertions(+), 47 deletions(-) create mode 100644 src/rc_msgs/msg/DataLeg.msg diff --git a/src/rc_msgs/CMakeLists.txt b/src/rc_msgs/CMakeLists.txt index e2b1db6..68923fc 100644 --- a/src/rc_msgs/CMakeLists.txt +++ b/src/rc_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Ps2Data.msg" "msg/DataControl.msg" "msg/DataMotor.msg" + "msg/DataLeg.msg" ) ament_package() diff --git a/src/rc_msgs/msg/DataLeg.msg b/src/rc_msgs/msg/DataLeg.msg new file mode 100644 index 0000000..191aa4c --- /dev/null +++ b/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 \ No newline at end of file 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 49deaff..8e32d98 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,8 +6,7 @@ #include "rclcpp/rclcpp.hpp" #include "serialPort/SerialPort.h" #include "unitreeMotor/unitreeMotor.h" -#include "rc_msgs/msg/data_control.hpp" // 添加 DataControl 消息 -#include "rc_msgs/msg/data_motor.hpp" // 添加 DataMotor 消息 +#include "rc_msgs/msg/data_leg.hpp" namespace unitree_motor_serial_driver { @@ -18,7 +17,7 @@ struct Motor_t MotorData data; }; -Motor_t motor; +Motor_t motor[3]; // 修改为数组以支持多个电机 class MotorControlNode : public rclcpp::Node { @@ -27,12 +26,12 @@ public: private: void control_motor(); - void data_control_callback(const rc_msgs::msg::DataControl::SharedPtr msg); + void data_control_callback(const rc_msgs::msg::DataLeg::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 data_control_sub_; + rclcpp::Publisher::SharedPtr data_motor_pub_; int timer_count_ = 0; }; 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 c13b32e..5be84d6 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 @@ -7,67 +7,97 @@ MotorControlNode::MotorControlNode() : Node("unitree_motor_serial_driver"), serial_("/dev/ttyACM0") { timer_ = this->create_wall_timer( - std::chrono::milliseconds(1), std::bind(&MotorControlNode::control_motor, this)); - data_control_sub_ = this->create_subscription( - "data_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1)); - data_motor_pub_ = this->create_publisher("data_motor", 10); + 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::DataControl::SharedPtr msg) +void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg) { - // 将接收到的 DataControl 消息赋值给 motor_t - motor.cmd.kp = msg->kp; - motor.cmd.kd = msg->kd; - motor.cmd.q = msg->pos * queryGearRatio(MotorType::GO_M8010_6); - motor.cmd.dq = msg->vw * queryGearRatio(MotorType::GO_M8010_6); - motor.cmd.tau = msg->tau; - timer_count_=0; + // 将接收到的 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; + } + timer_count_ = 0; } void MotorControlNode::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.kp = motor.cmd.kp; - cmd.kd = motor.cmd.kd; - cmd.q = motor.cmd.q; - cmd.dq = motor.cmd.dq; - cmd.tau = motor.cmd.tau; + rc_msgs::msg::DataLeg motor_msg; - if (timer_count_ > 300) + for (int i = 0; i < 3; ++i) { - Motor_Ctrl_Offline(&motor); - timer_count_ = 0; - } - else - { - timer_count_++; - } - serial_.sendRecv(&cmd, &data); + 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; + cmd.dq = motor[i].cmd.dq; + cmd.tau = motor[i].cmd.tau; - // 发布 DataMotor 消息 - auto motor_msg = rc_msgs::msg::DataMotor(); - motor_msg.tau = data.tau; - motor_msg.vw = data.dq; - motor_msg.pos = data.q; + if (timer_count_ > 300) + { + Motor_Ctrl_Offline(&motor[i]); + timer_count_ = 0; + } + else + { + timer_count_++; + } + + // 优化串口通信，减少阻塞 + 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 消息 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.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 -}// namespace unitree_motor_serial_driver int main(int argc, char **argv) { rclcpp::init(argc, argv);