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

可以实现12个电机读取和控制

Browse Source
This commit is contained in:
Robofish 2025-04-08 23:52:27 +08:00
parent 7d45e841f7
commit 2f47572350
3 changed files with 159 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
src/unitree_motor_serial_driver/launch/leg_ctrl.launch.py Normal file
View File

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

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

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

89
src/unitree_motor_serial_driver/src/unitree_motor_serial_driver.cpp
View File

@ -6,22 +6,14 @@ namespace unitree_motor_serial_driver
MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
: Node("unitree_motor_serial_driver", options),
serial_("/dev/ttyACM0") // 在初始化列表中提供必要参数
serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
{
// 声明参数
auto serial_port = 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秒
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;
}
bool connection_success = false;
int attempts = 0;
// 其余代码保持不变
timer_ = this->create_wall_timer(
@ -31,6 +23,7 @@ namespace unitree_motor_serial_driver
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 数组
@ -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]);
}
}