From 7cd85cbbf3741ae97bc712d90498a8aeeca0786d Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Sat, 17 May 2025 02:18:39 +0800 Subject: [PATCH] =?UTF-8?q?=E5=86=99=E5=AE=8C=E7=8A=B6=E6=80=81=E6=9C=BA?= =?UTF-8?q?=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- .../mdog_simple_controller/CMakeLists.txt | 37 ++++-- .../mdog_simple_controller/FSM/fsm_state.hpp | 15 +++ .../FSM/state_balance.hpp | 13 +++ .../FSM/state_troting.hpp | 13 +++ .../mdog_simple_controller/FSM/state_zero.hpp | 13 +++ .../common/kinematics.hpp | 21 ++++ .../mdog_simple_controller.hpp | 81 +++++++++++++ .../mdog_simple_controller/package.xml | 7 +- .../src/FSM/state_balance.cpp | 18 +++ .../src/FSM/state_troting.cpp | 27 +++++ .../src/FSM/state_zero.cpp | 27 +++++ .../src/common/kinematics.cpp | 46 ++++++++ .../src/mdog_simple_controller.cpp | 108 ++++++++++++++++++ .../unitree_leg_serial_driver/CMakeLists.txt | 1 + .../unitree_leg_serial.hpp | 6 + .../src/unitree_leg_serial.cpp | 32 ++++++ src/rc_msgs/CMakeLists.txt | 8 +- src/rc_msgs/msg/LegCmd.msg | 2 + src/rc_msgs/msg/LegFdb.msg | 2 + src/rc_msgs/package.xml | 3 +- 21 files changed, 467 insertions(+), 16 deletions(-) create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/fsm_state.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_troting.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_zero.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp create mode 100644 src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp create mode 100644 src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp create mode 100644 src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp create mode 100644 src/controllers/mdog_simple_controller/src/common/kinematics.cpp create mode 100644 src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp create mode 100644 src/rc_msgs/msg/LegCmd.msg create mode 100644 src/rc_msgs/msg/LegFdb.msg diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e09a03..c7e459a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,6 @@ { "files.associations": { - "chrono": "cpp" + "chrono": "cpp", + "functional": "cpp" } } \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index deb36c0..a5dafa1 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/src/controllers/mdog_simple_controller/CMakeLists.txt @@ -7,30 +7,45 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rc_msgs REQUIRED) +find_package(control_input_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +include_directories( + include +) + + +add_executable(mdog_simple_controller + src/mdog_simple_controller.cpp + src/FSM/state_zero.cpp + src/FSM/state_balance.cpp + src/FSM/state_troting.cpp + src/common/kinematics.cpp +) +ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs) + +install(TARGETS + mdog_simple_controller + DESTINATION lib/${PROJECT_NAME} +) install( DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) -install( - DIRECTORY include/ +install(DIRECTORY include/ DESTINATION include ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/fsm_state.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/fsm_state.hpp new file mode 100644 index 0000000..5ab0fbb --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/fsm_state.hpp @@ -0,0 +1,15 @@ +#pragma once + +namespace mdog_simple_controller { + +class MdogSimpleController; + +class FSMState { +public: + virtual ~FSMState() = default; + virtual void enter(MdogSimpleController*) {} + virtual void run(MdogSimpleController*) = 0; + virtual void exit(MdogSimpleController*) {} +}; + +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp new file mode 100644 index 0000000..f98d185 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_balance.hpp @@ -0,0 +1,13 @@ +#pragma once +#include "mdog_simple_controller/FSM/fsm_state.hpp" + +namespace mdog_simple_controller { + +class StateBalance : public FSMState { +public: + void enter(MdogSimpleController*) override; + void run(MdogSimpleController*) override; + void exit(MdogSimpleController*) override; +}; + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_troting.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_troting.hpp new file mode 100644 index 0000000..934f97f --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_troting.hpp @@ -0,0 +1,13 @@ +#pragma once +#include "mdog_simple_controller/FSM/fsm_state.hpp" + +namespace mdog_simple_controller { + +class StateTroting : public FSMState { +public: + void enter(MdogSimpleController*) override; + void run(MdogSimpleController*) override; + void exit(MdogSimpleController*) override; +}; + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_zero.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_zero.hpp new file mode 100644 index 0000000..c1cf452 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_zero.hpp @@ -0,0 +1,13 @@ +#pragma once +#include "mdog_simple_controller/FSM/fsm_state.hpp" + +namespace mdog_simple_controller { + +class StateZero : public FSMState { +public: + void enter(MdogSimpleController*) override; + void run(MdogSimpleController*) override; + void exit(MdogSimpleController*) override; +}; + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp new file mode 100644 index 0000000..5ed1ec0 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp @@ -0,0 +1,21 @@ +#pragma once +#include + +namespace mdog_simple_controller { +namespace kinematics { + +// 固定三连杆长度（单位：米，可根据实际机器人参数修改） +constexpr std::array LEG_LINK_LENGTH = {0.05, 0.30, 0.30}; + +// 正运动学：已知关节角，求末端位置 +void forward_kinematics(const std::array& theta, + const std::array& link, + std::array& pos); + +// 逆运动学：已知末端位置，求关节角。返回值为是否可达 +bool inverse_kinematics(const std::array& pos, + const std::array& link, + std::array& theta); + +} // namespace kinematics +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp new file mode 100644 index 0000000..92d6739 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include "rc_msgs/msg/leg_fdb.hpp" +#include "rc_msgs/msg/leg_cmd.hpp" +#include "control_input_msgs/msg/inputs.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "mdog_simple_controller/FSM/state_zero.hpp" +#include "mdog_simple_controller/FSM/state_balance.hpp" +#include "mdog_simple_controller/FSM/state_troting.hpp" + +namespace mdog_simple_controller +{ + +struct LegJointData { + float torque[3]; + float speed[3]; + float pos[3]; +}; + +struct LegJointCmd { + float torque_des[3]; + float speed_des[3]; + float pos_des[3]; + float kp[3]; + float kd[3]; +}; + +struct Voltage{ + float vx; + float vy; + float wz; +}; + +struct AHRS_Eulr_t { + float yaw; + float rol; + float pit; +}; + +struct InputCmd { + Voltage voltage; + AHRS_Eulr_t imu_euler; + AHRS_Eulr_t machine_euler; + float hight; + int8_t status; +}; + +struct MdogControllerData { + LegJointData feedback[4]; + LegJointCmd command[4]; +}; + + +class MdogSimpleController : public rclcpp::Node { +public: + MdogSimpleController(); + + void change_state(FSMState* new_state); + + MdogControllerData data_; + InputCmd input_cmd_; +private: + void fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg); + void input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg); + void ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg); + void control_loop(); + + FSMState* current_state_{nullptr}; + StateZero state_zero_; + StateBalance state_balance_; + StateTroting state_troting_; + + rclcpp::Subscription::SharedPtr fdb_sub_; + rclcpp::Subscription::SharedPtr input_sub_; + rclcpp::Subscription::SharedPtr ahrs_sub_; + rclcpp::Publisher::SharedPtr cmd_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/package.xml b/src/controllers/mdog_simple_controller/package.xml index 2440eb3..41588b5 100644 --- a/src/controllers/mdog_simple_controller/package.xml +++ b/src/controllers/mdog_simple_controller/package.xml @@ -8,6 +8,11 @@ TODO: License declaration ament_cmake + rclcpp + rc_msgs + control_input_msgs + geometry_msgs + std_msgs ament_lint_auto ament_lint_common @@ -15,4 +20,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp new file mode 100644 index 0000000..8436bfb --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp @@ -0,0 +1,18 @@ +#include "mdog_simple_controller/FSM/state_balance.hpp" +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller { + +void StateBalance::enter(MdogSimpleController* ctrl) { + // 进入BALANCE状态时的初始化 +} + +void StateBalance::run(MdogSimpleController* ctrl) { + // BALANCE状态下的控制逻辑 +} + +void StateBalance::exit(MdogSimpleController* ctrl) { + // 离开BALANCE状态时的清理 +} + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp new file mode 100644 index 0000000..6b8e96f --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp @@ -0,0 +1,27 @@ +#include "mdog_simple_controller/FSM/state_troting.hpp" +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller { + +void StateTroting::enter(MdogSimpleController* ctrl) { + // 进入TROTING状态时的初始化 +} + +void StateTroting::run(MdogSimpleController* ctrl) { + //设置所有cmd数据为0 + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + ctrl->data_.command[leg].torque_des[j] = 0; + ctrl->data_.command[leg].speed_des[j] = 0; + ctrl->data_.command[leg].pos_des[j] = 0; + ctrl->data_.command[leg].kp[j] = 0; + ctrl->data_.command[leg].kd[j] = 0.05; + } + } +} + +void StateTroting::exit(MdogSimpleController* ctrl) { + // 离开TROTING状态时的清理 +} + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp new file mode 100644 index 0000000..9d7cde7 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/FSM/state_zero.cpp @@ -0,0 +1,27 @@ +#include "mdog_simple_controller/FSM/state_zero.hpp" +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller { + +void StateZero::enter(MdogSimpleController* ctrl) { + // 清空电机命令 + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + ctrl->data_.command[leg].torque_des[j] = 0; + ctrl->data_.command[leg].speed_des[j] = 0; + ctrl->data_.command[leg].pos_des[j] = 0; + ctrl->data_.command[leg].kp[j] = 0; + ctrl->data_.command[leg].kd[j] = 0; + } + } +} + +void StateZero::run(MdogSimpleController* ctrl) { + // ZERO状态下的控制逻辑 +} + +void StateZero::exit(MdogSimpleController* ctrl) { + // 离开ZERO状态时的清理 +} + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp new file mode 100644 index 0000000..fc9e6fe --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp @@ -0,0 +1,46 @@ +#include "mdog_simple_controller/common/kinematics.hpp" +#include + +namespace mdog_simple_controller { +namespace kinematics { + +void forward_kinematics(const std::array& theta, + const std::array& link, + std::array& pos) { + double t1 = theta[0], t2 = theta[1], t3 = theta[2]; + double l1 = link[0], l2 = link[1], l3 = link[2]; + + double x = (l2 * cos(t2) + l3 * cos(t2 + t3)) * cos(t1); + double y = (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1); + double z = l1 + l2 * sin(t2) + l3 * sin(t2 + t3); + + pos[0] = x; + pos[1] = y; + pos[2] = z; +} + +bool inverse_kinematics(const std::array& pos, + const std::array& link, + std::array& theta) { + double x = pos[0], y = pos[1], z = pos[2]; + double l1 = link[0], l2 = link[1], l3 = link[2]; + + theta[0] = atan2(y, x); + + double d = sqrt(x * x + y * y); + double z1 = z - l1; + + double D = (d * d + z1 * z1 - l2 * l2 - l3 * l3) / (2 * l2 * l3); + if (D < -1.0 || D > 1.0) return false; // 不可达 + + theta[2] = atan2(-sqrt(1 - D * D), D); + + double phi = atan2(z1, d); + double psi = atan2(l3 * sin(theta[2]), l2 + l3 * cos(theta[2])); + theta[1] = phi - psi; + + return true; +} + +} // namespace kinematics +} // namespace mdog_simple_controller \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp new file mode 100644 index 0000000..6a96a64 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp @@ -0,0 +1,108 @@ +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller +{ + +MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") { + fdb_sub_ = this->create_subscription( + "/leg_fdb", 10, + std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1)); + input_sub_ = this->create_subscription( + "/control_input", 10, + std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1)); + ahrs_sub_ = this->create_subscription( + "/euler_angles", 10, + std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1)); + cmd_pub_ = this->create_publisher("/leg_cmd", 10); + current_state_ = &state_zero_; + current_state_->enter(this); + timer_ = this->create_wall_timer( + std::chrono::microseconds(2000), + std::bind(&MdogSimpleController::control_loop, this)); +} + +void MdogSimpleController::change_state(FSMState* new_state) { + if (current_state_) current_state_->exit(this); + current_state_ = new_state; + if (current_state_) current_state_->enter(this); +} + +void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg) { + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + int idx = leg * 3 + j; + data_.feedback[leg].torque[j] = msg->leg_fdb[idx].torque; + data_.feedback[leg].speed[j] = msg->leg_fdb[idx].speed; + data_.feedback[leg].pos[j] = msg->leg_fdb[idx].pos; + } + } +} + +void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg){ + input_cmd_.voltage.vx = msg->lx; + input_cmd_.voltage.vy = msg->ly; + input_cmd_.voltage.wz = msg->rx; + input_cmd_.hight = 0.5; + input_cmd_.status = msg->command; +} + +void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) { + input_cmd_.imu_euler.yaw = msg->z; + input_cmd_.imu_euler.rol = msg->x; + input_cmd_.imu_euler.pit = msg->y; +} + +void MdogSimpleController::control_loop() { + FSMState* target_state = nullptr; + switch (input_cmd_.status) { + case 0: target_state = &state_zero_; break; + case 6: target_state = &state_balance_; break; + case 7: target_state = &state_troting_; break; + default: target_state = &state_zero_; break; + } + if (target_state != current_state_) { + change_state(target_state); + } + if (current_state_) current_state_->run(this); + + rc_msgs::msg::LegCmd msg; + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + msg.leg_cmd[leg * 3 + j].torque_des = data_.command[leg].torque_des[j]; + msg.leg_cmd[leg * 3 + j].speed_des = data_.command[leg].speed_des[j]; + msg.leg_cmd[leg * 3 + j].pos_des = data_.command[leg].pos_des[j]; + msg.leg_cmd[leg * 3 + j].kp = data_.command[leg].kp[j]; + msg.leg_cmd[leg * 3 + j].kd = data_.command[leg].kd[j]; + } + } + msg.header.stamp = this->now(); + msg.header.frame_id = "leg_cmd"; + cmd_pub_->publish(msg); + + if (0){ + RCLCPP_INFO(this->get_logger(), "MdogControllerData:"); + for (int leg = 0; leg < 4; ++leg) { + RCLCPP_INFO(this->get_logger(), "Leg %d:", leg); + for (int j = 0; j < 3; ++j) { + RCLCPP_INFO(this->get_logger(), "Joint %d: Torque: %f, Speed: %f, Pos: %f", + j, data_.feedback[leg].torque[j], data_.feedback[leg].speed[j], data_.feedback[leg].pos[j]); + } + } + RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", + input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); + RCLCPP_INFO(this->get_logger(), "AHRS: Yaw: %f, Roll: %f, Pitch: %f", + input_cmd_.imu_euler.yaw, input_cmd_.imu_euler.rol, input_cmd_.imu_euler.pit); + } + RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", + input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); + +} + +} // namespace mdog_simple_controller + +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 diff --git a/src/hardware/unitree_leg_serial_driver/CMakeLists.txt b/src/hardware/unitree_leg_serial_driver/CMakeLists.txt index 921eb39..d3fa4eb 100644 --- a/src/hardware/unitree_leg_serial_driver/CMakeLists.txt +++ b/src/hardware/unitree_leg_serial_driver/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(serial REQUIRED) find_package(rc_msgs REQUIRED) + # 添加头文件目录 include_directories(include) 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 0fb3d1a..3fa38bf 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 @@ -8,6 +8,8 @@ #include #include "unitree_leg_serial_driver/crc_ccitt.hpp" #include "unitree_leg_serial_driver/gom_protocol.hpp" +#include "rc_msgs/msg/leg_fdb.hpp" +#include "rc_msgs/msg/leg_cmd.hpp" namespace unitree_leg_serial { @@ -19,6 +21,7 @@ public: void write(const std::array& cmds); std::array read(); + private: static constexpr int PORT_NUM = 4; static constexpr int MOTOR_NUM = 3; @@ -30,6 +33,7 @@ private: void motor_cmd_reset(int port_idx, int idx); bool open_serial_port(int port_idx); void close_serial_port(int port_idx); + void leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg); int send_count_[PORT_NUM]{}; int recv_count_[PORT_NUM]{}; @@ -40,6 +44,8 @@ private: rclcpp::TimerBase::SharedPtr timer_; std::array read_thread_; std::atomic running_{false}; + rclcpp::Publisher::SharedPtr leg_fdb_pub_; + rclcpp::Subscription::SharedPtr leg_cmd_sub_; // 状态管理 enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 }; 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 70438d5..bc5d1ae 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,5 +1,7 @@ #include "unitree_leg_serial_driver/unitree_leg_serial.hpp" #include "rclcpp_components/register_node_macro.hpp" +#include "rc_msgs/msg/leg_fdb.hpp" +#include "rc_msgs/msg/leg_cmd.hpp" namespace unitree_leg_serial { @@ -11,6 +13,9 @@ UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options) baud_rate_ = {4000000, 4000000, 4000000, 4000000}; last_freq_time_ = this->now(); + leg_fdb_pub_ = this->create_publisher("leg_fdb", 10); + leg_cmd_sub_ = this->create_subscription( + "leg_cmd", 10, std::bind(&UnitreeLegSerial::leg_cmd_callback, this, std::placeholders::_1)); for (int p = 0; p < PORT_NUM; ++p) { send_count_[p] = 0; @@ -85,6 +90,21 @@ std::array UnitreeLegSerial::read() return result; } +void UnitreeLegSerial::leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg) +{ + std::array cmds; + for (size_t i = 0; i < 12; ++i) { + cmds[i].T = msg->leg_cmd[i].torque_des; + cmds[i].W = msg->leg_cmd[i].speed_des; + cmds[i].Pos = msg->leg_cmd[i].pos_des; + cmds[i].K_P = msg->leg_cmd[i].kp; + cmds[i].K_W = msg->leg_cmd[i].kd; + cmds[i].id = i % 3; + cmds[i].mode = 1; + } + write(cmds); +} + bool UnitreeLegSerial::open_serial_port(int port_idx) { try { @@ -148,6 +168,18 @@ void UnitreeLegSerial::motor_update() } last_freq_time_ = now; } + rc_msgs::msg::LegFdb msg; + + msg.header.stamp = this->now(); + msg.header.frame_id = "leg"; // 可根据实际需要修改 + + auto fbk = read(); + for (size_t i = 0; i < 12; ++i) { + msg.leg_fdb[i].torque = fbk[i].T; + msg.leg_fdb[i].speed = fbk[i].W; + msg.leg_fdb[i].pos = fbk[i].Pos; + } + leg_fdb_pub_->publish(msg); } void UnitreeLegSerial::motor_cmd_reset(int port_idx, int idx) diff --git a/src/rc_msgs/CMakeLists.txt b/src/rc_msgs/CMakeLists.txt index 37837d3..84cd560 100644 --- a/src/rc_msgs/CMakeLists.txt +++ b/src/rc_msgs/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.8) project(rc_msgs) find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + rosidl_generate_interfaces(${PROJECT_NAME} "msg/DataMCU.msg" "msg/DataRef.msg" @@ -11,7 +13,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/DataNav.msg" "msg/GoMotorCmd.msg" "msg/GoMotorFdb.msg" + "msg/LegCmd.msg" + "msg/LegFdb.msg" + DEPENDENCIES std_msgs ) -ament_package() - +ament_package() \ No newline at end of file diff --git a/src/rc_msgs/msg/LegCmd.msg b/src/rc_msgs/msg/LegCmd.msg new file mode 100644 index 0000000..8ebc6dd --- /dev/null +++ b/src/rc_msgs/msg/LegCmd.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +GoMotorCmd[12] leg_cmd \ No newline at end of file diff --git a/src/rc_msgs/msg/LegFdb.msg b/src/rc_msgs/msg/LegFdb.msg new file mode 100644 index 0000000..5284ed2 --- /dev/null +++ b/src/rc_msgs/msg/LegFdb.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +GoMotorFdb[12] leg_fdb \ No newline at end of file diff --git a/src/rc_msgs/package.xml b/src/rc_msgs/package.xml index 91ec4a6..e7dba2b 100644 --- a/src/rc_msgs/package.xml +++ b/src/rc_msgs/package.xml @@ -5,7 +5,8 @@ Describe custom messages biao MIT - + std_msgs + std_msgs rosidl_default_generators rosidl_default_runtime rosidl_interface_packages