robofish/CM_DOG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

底层控制完成

Browse Source
This commit is contained in:
Robofish 2025-04-09 02:50:36 +08:00
parent 040a6d8040
commit 761ede8596
5 changed files with 73 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/robot_bringup/launch/robot_bringup.launch.py
View File

@ -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')
]
)
],

9
src/unitree_motor_serial_driver/include/unitree_motor_serial_driver/unitree_motor_serial_driver.hpp
View File

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

16
src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py
View File

@ -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')
]
)
],

4
src/unitree_motor_serial_driver/launch/unitree_motor_serial_driver.launch.py
View File

@ -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')
]
)
],

60
src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
View File

@ -4,10 +4,10 @@
namespace unitree_motor_serial_driver
{
MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
: Node("unitree_motor_serial_driver", options),
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秒
@ -15,32 +15,46 @@ namespace unitree_motor_serial_driver
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);
}
// 订阅 MotorCmds 消息
motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::MotorCmds>(
"motor_cmds", 10, std::bind(&MotorControlNode::motor_cmd_callback, this, std::placeholders::_1));
void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg)
// 发布 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)
{
// 将接收到的 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;
}
// 将接收到的 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);
}