From 7d45e841f760ae5756236a4764cd990d765ffb4f Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Tue, 8 Apr 2025 22:41:23 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=88=90=E7=BB=84=E4=BB=B6?=
=?UTF-8?q?=E5=BD=A2=E5=BC=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../CMakeLists.txt | 39 +++++++++++----
.../unitree_motor_serial_driver.hpp | 4 +-
.../unitree_motor_serial_driver.launch.py | 35 +++++++++-----
src/unitree_motor_serial_driver/package.xml | 1 +
.../src/unitree_motor_serial_driver.cpp | 47 ++++++++++++-------
5 files changed, 86 insertions(+), 40 deletions(-)
diff --git a/src/unitree_motor_serial_driver/CMakeLists.txt b/src/unitree_motor_serial_driver/CMakeLists.txt
index 65a20b1..7fed6cf 100644
--- a/src/unitree_motor_serial_driver/CMakeLists.txt
+++ b/src/unitree_motor_serial_driver/CMakeLists.txt
@@ -8,6 +8,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
# 包含 ROS 2 依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rc_msgs REQUIRED)
@@ -26,20 +27,38 @@ else()
set(EXTRA_LIBS libUnitreeMotorSDK_Linux64.so)
endif()
-# 添加可执行文件和链接库
-add_executable(goM8010_6_motor src/goM8010_6_motor.cpp)
-ament_target_dependencies(goM8010_6_motor rclcpp rc_msgs)
-target_link_libraries(goM8010_6_motor ${EXTRA_LIBS})
+# 创建组件库
+add_library(unitree_motor_components SHARED
+ src/unitree_motor_serial_driver.cpp
+)
+ament_target_dependencies(unitree_motor_components
+ rclcpp
+ rclcpp_components
+ rc_msgs
+)
+target_link_libraries(unitree_motor_components ${EXTRA_LIBS})
+rclcpp_components_register_nodes(unitree_motor_components
+ "unitree_motor_serial_driver::MotorControlNode"
+)
-add_executable(unitree_motor_serial_driver src/unitree_motor_serial_driver.cpp)
-ament_target_dependencies(unitree_motor_serial_driver rclcpp rc_msgs)
-target_link_libraries(unitree_motor_serial_driver ${EXTRA_LIBS})
+# # 创建独立可执行文件
+# add_executable(goM8010_6_motor src/goM8010_6_motor.cpp)
+# ament_target_dependencies(goM8010_6_motor rclcpp rc_msgs)
+# target_link_libraries(goM8010_6_motor ${EXTRA_LIBS})
+
+# 创建节点可执行文件（非组件方式）
+# add_executable(unitree_motor_serial_driver_node src/unitree_motor_serial_driver_node.cpp)
+# target_link_libraries(unitree_motor_serial_driver_node unitree_motor_components)
+# ament_target_dependencies(unitree_motor_serial_driver_node rclcpp)
# 安装目标
install(TARGETS
- goM8010_6_motor
- unitree_motor_serial_driver
- DESTINATION lib/${PROJECT_NAME}
+ # goM8010_6_motor
+ # unitree_motor_serial_driver_node
+ unitree_motor_components
+ RUNTIME DESTINATION lib/${PROJECT_NAME}
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
)
install(DIRECTORY launch/
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 8e32d98..6bb5342 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
@@ -22,8 +22,8 @@ Motor_t motor[3]; // 修改为数组以支持多个电机
class MotorControlNode : public rclcpp::Node
{
public:
- MotorControlNode();
-
+ explicit MotorControlNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
private:
void control_motor();
void data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg);
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 ebee65c..7ae1093 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
@@ -1,15 +1,28 @@
from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
def generate_launch_description():
- return LaunchDescription([
- Node(
- package='unitree_motor_serial_driver',
- executable='unitree_motor_serial_driver',
- name='unitree_motor_serial_driver',
- output='screen',
- parameters=[
+ container = ComposableNodeContainer(
+ name='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',
+ parameters=[
+ {'serial_port': '/dev/ttyACM0'}
+ ],
+ remappings=[
+ ('data_leg_control', 'data_leg_control'),
+ ('data_leg_motor', 'data_leg_motor')
+ ]
+ )
+ ],
+ output='screen',
+ )
- ]
- )
- ])
\ No newline at end of file
+ return LaunchDescription([container])
\ No newline at end of file
diff --git a/src/unitree_motor_serial_driver/package.xml b/src/unitree_motor_serial_driver/package.xml
index 0fa91ba..677c1b5 100644
--- a/src/unitree_motor_serial_driver/package.xml
+++ b/src/unitree_motor_serial_driver/package.xml
@@ -10,6 +10,7 @@
ament_cmake
rclcpp
+ rclcpp_components
std_msgs
rc_msgs
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 5be84d6..979901b 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
@@ -1,17 +1,35 @@
#include "unitree_motor_serial_driver/unitree_motor_serial_driver.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
namespace unitree_motor_serial_driver
{
-MotorControlNode::MotorControlNode()
- : Node("unitree_motor_serial_driver"), serial_("/dev/ttyACM0")
-{
- 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);
-}
+ MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
+ : Node("unitree_motor_serial_driver", options),
+ serial_("/dev/ttyACM0") // 在初始化列表中提供必要参数
+ {
+ // 声明参数
+ auto serial_port = this->declare_parameter("serial_port", "/dev/ttyACM0");
+
+ 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;
+ }
+
+ // 其余代码保持不变
+ 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)
{
@@ -45,7 +63,7 @@ void MotorControlNode::control_motor()
cmd.dq = motor[i].cmd.dq;
cmd.tau = motor[i].cmd.tau;
- if (timer_count_ > 300)
+ if (timer_count_ > 100)
{
Motor_Ctrl_Offline(&motor[i]);
timer_count_ = 0;
@@ -98,10 +116,5 @@ void MotorControlNode::Motor_Ctrl_Offline(Motor_t *motor_s)
} // namespace unitree_motor_serial_driver
-int main(int argc, char **argv)
-{
- rclcpp::init(argc, argv);
- rclcpp::spin(std::make_shared());
- rclcpp::shutdown();
- return 0;
-}
\ No newline at end of file
+// 注册组件
+RCLCPP_COMPONENTS_REGISTER_NODE(unitree_motor_serial_driver::MotorControlNode)
\ No newline at end of file