diff --git a/.vscode/settings.json b/.vscode/settings.json index 02e32c6..1507357 100644 --- a/.vscode/settings.json +++ b/.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" } } \ No newline at end of file diff --git a/src/controllers/trot.py b/src/controllers/trot.py new file mode 100644 index 0000000..e69de29 diff --git a/src/hardware/unitree_leg_serial_driver/CMakeLists.txt b/src/hardware/unitree_leg_serial_driver/CMakeLists.txt index 52e408d..921eb39 100644 --- a/src/hardware/unitree_leg_serial_driver/CMakeLists.txt +++ b/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 ) # 注册组件 diff --git a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp index a2b4b0b..260137b 100644 --- a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp +++ b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp @@ -7,7 +7,8 @@ #include #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::SharedPtr motor_fdb_pub_; + rclcpp::Subscription::SharedPtr motor_cmd_sub_; + std::unique_ptr serial_port_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/src/hardware/unitree_leg_serial_driver/package.xml b/src/hardware/unitree_leg_serial_driver/package.xml index 8595382..67d1038 100644 --- a/src/hardware/unitree_leg_serial_driver/package.xml +++ b/src/hardware/unitree_leg_serial_driver/package.xml @@ -9,18 +9,19 @@ ament_cmake - rosidl_default_generators rclcpp rclcpp_components std_msgs serial + rc_msgs rclcpp rclcpp_components std_msgs serial - + rc_msgs + ament_lint_auto ament_lint_common diff --git a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp index a492324..7af8360 100644 --- a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp +++ b/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("motor_fdb", 10); + // 订阅器 + motor_cmd_sub_ = this->create_subscription( + "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); diff --git a/src/rc_msgs/.gitignore b/src/rc_msgs/.gitignore new file mode 100644 index 0000000..35d74bb --- /dev/null +++ b/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 diff --git a/src/rc_msgs/CMakeLists.txt b/src/rc_msgs/CMakeLists.txt new file mode 100644 index 0000000..37837d3 --- /dev/null +++ b/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() + diff --git a/src/rc_msgs/LICENSE b/src/rc_msgs/LICENSE new file mode 100644 index 0000000..cb386d0 --- /dev/null +++ b/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. diff --git a/src/rc_msgs/README.md b/src/rc_msgs/README.md new file mode 100644 index 0000000..006494b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/rc_msgs/msg/DataAI.msg b/src/rc_msgs/msg/DataAI.msg new file mode 100644 index 0000000..69ce453 --- /dev/null +++ b/src/rc_msgs/msg/DataAI.msg @@ -0,0 +1,7 @@ +float32 yaw +float32 pit +float32 rol +float32 vx +float32 vy +float32 wz +uint8 notice diff --git a/src/rc_msgs/msg/DataMCU.msg b/src/rc_msgs/msg/DataMCU.msg new file mode 100644 index 0000000..d3351fd --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/rc_msgs/msg/DataNav.msg b/src/rc_msgs/msg/DataNav.msg new file mode 100644 index 0000000..f7c39af --- /dev/null +++ b/src/rc_msgs/msg/DataNav.msg @@ -0,0 +1,5 @@ +bool reached + +float32 x +float32 y +float32 yaw \ No newline at end of file diff --git a/src/rc_msgs/msg/DataRef.msg b/src/rc_msgs/msg/DataRef.msg new file mode 100644 index 0000000..181dd7f --- /dev/null +++ b/src/rc_msgs/msg/DataRef.msg @@ -0,0 +1,3 @@ +uint16 remain_hp +uint8 game_progress +uint16 stage_remain_time \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/msgs/GoMotorCmd.msg b/src/rc_msgs/msg/GoMotorCmd.msg similarity index 100% rename from src/hardware/unitree_leg_serial_driver/msgs/GoMotorCmd.msg rename to src/rc_msgs/msg/GoMotorCmd.msg diff --git a/src/hardware/unitree_leg_serial_driver/msgs/GoMotorFdb.msg b/src/rc_msgs/msg/GoMotorFdb.msg similarity index 100% rename from src/hardware/unitree_leg_serial_driver/msgs/GoMotorFdb.msg rename to src/rc_msgs/msg/GoMotorFdb.msg diff --git a/src/rc_msgs/msg/GoalPose.msg b/src/rc_msgs/msg/GoalPose.msg new file mode 100644 index 0000000..1ec140b --- /dev/null +++ b/src/rc_msgs/msg/GoalPose.msg @@ -0,0 +1,8 @@ +float32 x +float32 y +float32 angle + +float32 max_speed +float32 tolerance + +bool rotor \ No newline at end of file diff --git a/src/rc_msgs/msg/Ps2Data.msg b/src/rc_msgs/msg/Ps2Data.msg new file mode 100644 index 0000000..2b52c1c --- /dev/null +++ b/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 + diff --git a/src/rc_msgs/package.xml b/src/rc_msgs/package.xml new file mode 100644 index 0000000..91ec4a6 --- /dev/null +++ b/src/rc_msgs/package.xml @@ -0,0 +1,16 @@ + + + rc_msgs + 0.0.1 + Describe custom messages + biao + MIT + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file