基于话题控制的go，但是话题控制频率最高300

.vscode/settings.json

@@ -1,69 +1,5 @@
 {
     "files.associations": {
-        "cctype": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "array": "cpp",
-        "atomic": "cpp",
-        "bit": "cpp",
-        "bitset": "cpp",
-        "chrono": "cpp",
-        "compare": "cpp",
-        "complex": "cpp",
-        "concepts": "cpp",
-        "condition_variable": "cpp",
-        "cstdint": "cpp",
-        "deque": "cpp",
-        "forward_list": "cpp",
-        "list": "cpp",
-        "map": "cpp",
-        "set": "cpp",
-        "string": "cpp",
-        "unordered_map": "cpp",
-        "unordered_set": "cpp",
-        "vector": "cpp",
-        "exception": "cpp",
-        "algorithm": "cpp",
-        "functional": "cpp",
-        "iterator": "cpp",
-        "memory": "cpp",
-        "memory_resource": "cpp",
-        "numeric": "cpp",
-        "optional": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "string_view": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "fstream": "cpp",
-        "initializer_list": "cpp",
-        "iomanip": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "mutex": "cpp",
-        "new": "cpp",
-        "numbers": "cpp",
-        "ostream": "cpp",
-        "semaphore": "cpp",
-        "sstream": "cpp",
-        "stdexcept": "cpp",
-        "stop_token": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "cinttypes": "cpp",
-        "typeindex": "cpp",
-        "typeinfo": "cpp"
+        "functional": "cpp"
     }
 }

src/hardware/unitree_leg_serial_driver/CMakeLists.txt

@@ -11,23 +11,10 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(serial REQUIRED)
-find_package(rosidl_default_generators REQUIRED) # 添加消息生成依赖
-
+find_package(rc_msgs REQUIRED)
 # 添加头文件目录
 include_directories(include)
 
-# 添加消息文件
-set(msg_files
-  msgs/GoMotorFdb.msg
-  msgs/GoMotorCmd.msg
-)
-
-# 生成消息
-rosidl_generate_interfaces(${PROJECT_NAME}
-  ${msg_files}
-  DEPENDENCIES std_msgs
-)
-
 # 添加源文件并生成共享库
 add_library(${PROJECT_NAME}_lib SHARED  # 修改目标名称，避免冲突
   src/unitree_leg_serial.cpp
@@ -40,6 +27,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib
   rclcpp_components
   std_msgs
   serial
+  rc_msgs
 )
 
 # 注册组件

src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp

@@ -7,7 +7,8 @@
 #include <thread>
 #include "unitree_leg_serial_driver/crc_ccitt.hpp"
 #include "unitree_leg_serial_driver/gom_protocol.hpp"
-
+#include "rc_msgs/msg/go_motor_cmd.hpp"
+#include "rc_msgs/msg/go_motor_fdb.hpp"
 namespace unitree_leg_serial
 {
 
@@ -25,6 +26,15 @@ private:
     void motor_cmd_reset();
     bool open_serial_port();
     void close_serial_port();
+    void motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg);
+    
+    int send_count_;
+    int recv_count_;
+    rclcpp::Time last_freq_time_;
+
+    rclcpp::Publisher<rc_msgs::msg::GoMotorFdb>::SharedPtr motor_fdb_pub_;
+    rclcpp::Subscription<rc_msgs::msg::GoMotorCmd>::SharedPtr motor_cmd_sub_;
+    
 
     std::unique_ptr<serial::Serial> serial_port_;
     rclcpp::TimerBase::SharedPtr timer_;

src/hardware/unitree_leg_serial_driver/package.xml

@@ -9,18 +9,19 @@
 
   <!-- Build dependencies -->
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_components</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>serial</build_depend>
+  <build_depend>rc_msgs</build_depend>
 
   <!-- Execution dependencies -->
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>serial</exec_depend>
-
+  <exec_depend>rc_msgs</exec_depend>
+  
   <!-- Test dependencies -->
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp

@@ -1,15 +1,27 @@
 #include "unitree_leg_serial_driver/unitree_leg_serial.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
-
+#include "rc_msgs/msg/go_motor_cmd.hpp"
+#include "rc_msgs/msg/go_motor_fdb.hpp"
 namespace unitree_leg_serial
 {
 
 UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options)
     : Node("unitree_leg_serial", options)
 {
-    serial_port_name_ = "/dev/ttyACM3";
+    serial_port_name_ = "/dev/ttyACM0";
     baud_rate_ = 4000000;
 
+    send_count_ = 0;
+    recv_count_ = 0;
+    last_freq_time_ = this->now();
+
+    // 发布器
+    motor_fdb_pub_ = this->create_publisher<rc_msgs::msg::GoMotorFdb>("motor_fdb", 10);
+    // 订阅器
+    motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::GoMotorCmd>(
+        "motor_cmd", 10,
+        std::bind(&UnitreeLegSerial::motor_cmd_callback, this, std::placeholders::_1));
+
     if (!open_serial_port()) {
         RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_.c_str());
         return;
@@ -34,6 +46,18 @@ UnitreeLegSerial::~UnitreeLegSerial()
     close_serial_port();
 }
 
+void UnitreeLegSerial::motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg)
+{
+    // 填充motor_cmd_结构体
+    motor_cmd_.T = msg->torque_des;
+    motor_cmd_.W = msg->speed_des;
+    motor_cmd_.Pos = msg->pos_des;
+    motor_cmd_.K_P = msg->kp;
+    motor_cmd_.K_W = msg->kd;
+    status_flag_ = CONTROLED;
+    tick_ = 0;
+}
+
 bool UnitreeLegSerial::open_serial_port()
 {
     try {
@@ -64,6 +88,16 @@ void UnitreeLegSerial::motor_update()
         motor_cmd_reset();
     }
     send_motor_data(motor_cmd_);
+    send_count_++;
+
+    // 每秒打印一次频率
+    auto now = this->now();
+    if ((now - last_freq_time_).seconds() >= 1.0) {
+        RCLCPP_INFO(this->get_logger(), "发送频率: %d Hz, 接收频率: %d Hz", send_count_, recv_count_);
+        send_count_ = 0;
+        recv_count_ = 0;
+        last_freq_time_ = now;
+    }
 }
 
 void UnitreeLegSerial::motor_cmd_reset()
@@ -162,6 +196,13 @@ void UnitreeLegSerial::receive_data()
                 }
             }
             if (motor_fbk_.correct) {
+                // 发布反馈消息
+                rc_msgs::msg::GoMotorFdb msg;
+                msg.torque = motor_fbk_.T;
+                msg.speed = motor_fbk_.W;
+                msg.pos = motor_fbk_.Pos;
+                motor_fdb_pub_->publish(msg);
+                recv_count_++;
                 RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Motor ID: %d, Position: %f", motor_fbk_.motor_id, motor_fbk_.Pos);
             }
             std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size);

src/rc_msgs/.gitignore

@@ -0,0 +1,51 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

src/rc_msgs/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.8)
+project(rc_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/DataMCU.msg"
+        "msg/DataRef.msg"
+        "msg/DataAI.msg"
+        "msg/Ps2Data.msg"
+        "msg/GoalPose.msg"
+        "msg/DataNav.msg"
+        "msg/GoMotorCmd.msg"
+        "msg/GoMotorFdb.msg"
+)
+
+ament_package()
+

src/rc_msgs/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 zucheng Lv
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/rc_msgs/README.md

@@ -0,0 +1,6 @@
+# rc_msg
+Some ROS 2 custom messages for Robocon
+
+Usage
+Modify or add files in the /msg directory as needed
+colcon build

src/rc_msgs/msg/DataAI.msg

@@ -0,0 +1,7 @@
+float32 yaw
+float32 pit
+float32 rol
+float32 vx
+float32 vy
+float32 wz
+uint8 notice

src/rc_msgs/msg/DataMCU.msg

@@ -0,0 +1,8 @@
+float32 q0
+float32 q1
+float32 q2
+float32 q3
+float32 yaw
+float32 pit
+float32 rol
+uint8 notice

src/rc_msgs/msg/DataNav.msg

@@ -0,0 +1,5 @@
+bool reached
+
+float32 x
+float32 y
+float32 yaw

src/rc_msgs/msg/DataRef.msg

@@ -0,0 +1,3 @@
+uint16 remain_hp
+uint8 game_progress
+uint16 stage_remain_time

src/rc_msgs/msg/GoalPose.msg

@@ -0,0 +1,8 @@
+float32 x
+float32 y
+float32 angle
+
+float32 max_speed
+float32 tolerance
+
+bool rotor

src/rc_msgs/msg/Ps2Data.msg

@@ -0,0 +1,20 @@
+# control input message
+float32 lx
+float32 ly
+float32 rx
+float32 ry
+
+float32 up_down
+float32 left_right
+
+bool l1
+bool l2
+bool r1
+bool r2
+
+# 四种模式
+uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
+
+bool select
+bool start
+

src/rc_msgs/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+    <name>rc_msgs</name>
+    <version>0.0.1</version>
+    <description>Describe custom messages</description>
+    <maintainer email="1683502971@qq.com">biao</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>