底层控制完成

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'),
+                    ('motor_cmds', 'LF/motor_cmds'),
-                    ('data_leg_motor', 'LF/data_leg_motor')
+                    ('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'),
+                    ('motor_cmds', 'RF/motor_cmds'),
-                    ('data_leg_motor', 'RF/data_leg_motor')
+                    ('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'),
+                    ('motor_cmds', 'LR/motor_cmds'),
-                    ('data_leg_motor', 'LR/data_leg_motor')
+                    ('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'),
+                    ('motor_cmds', 'RR/motor_cmds'),
-                    ('data_leg_motor', 'RR/data_leg_motor')
+                    ('data_motors', 'RR/data_motors')
                 ]
             )
         ],

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<rc_msgs::msg::DataLeg>::SharedPtr data_control_sub_;
+    rclcpp::Subscription<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmd_sub_;
-    rclcpp::Publisher<rc_msgs::msg::DataLeg>::SharedPtr data_motor_pub_;
+    rclcpp::Publisher<rc_msgs::msg::DataMotors>::SharedPtr data_motor_pub_;
     int timer_count_ = 0;
 };
 

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'),
+                    ('motor_cmds', 'LF/motor_cmds'),
-                    ('data_leg_motor', 'LF/data_leg_motor')
+                    ('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'),
+                    ('motor_cmds', 'RF/motor_cmds'),
-                    ('data_leg_motor', 'RF/data_leg_motor')
+                    ('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'),
+                    ('motor_cmds', 'LR/motor_cmds'),
-                    ('data_leg_motor', 'LR/data_leg_motor')
+                    ('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'),
+                    ('motor_cmds', 'RR/motor_cmds'),
-                    ('data_leg_motor', 'RR/data_leg_motor')
+                    ('data_motors', 'RR/data_motors')
                 ]
             )
         ],

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'),
+                    ('motor_cmds', 'motor_cmds'),
-                    ('data_leg_motor', 'data_leg_motor')
+                    ('data_motors', 'data_motors')
                 ]
             )
         ],

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)
+MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
-        : Node("unitree_motor_serial_driver", options),
+    : Node("unitree_motor_serial_driver", options),
-          serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
+      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));
-        data_control_sub_ = this->create_subscription<rc_msgs::msg::DataLeg>(
-            "data_leg_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1));
-        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 数组
+    // 声明其他参数
-    for (int i = 0; i < 3; ++i)
+    auto retry_count = this->declare_parameter("serial_retry_count", 5);  // 默认重试5次
-    {
+    auto retry_delay_ms = this->declare_parameter("serial_retry_delay_ms", 1000);  // 默认延迟1秒
-        motor[i].cmd.kp = msg->data_control_0.kp;
+
-        motor[i].cmd.kd = msg->data_control_0.kd;
+    bool connection_success = false;
-        motor[i].cmd.q = msg->data_control_0.pos * queryGearRatio(MotorType::GO_M8010_6);
+    int attempts = 0;
-        motor[i].cmd.dq = msg->data_control_0.vw * queryGearRatio(MotorType::GO_M8010_6);
+    timer_count_ = 0;
-        motor[i].cmd.tau = msg->data_control_0.tau;
+
-    }
+    // 定时器
+    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::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.q = motor[i].cmd.q * queryGearRatio(MotorType::GO_M8010_6);
-        cmd.dq = motor[i].cmd.dq;
+        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);
 }