抽象力控

.vscode/settings.json

@@ -1,8 +1,9 @@
 {
     "files.associations": {
-        "array": "cpp",
+        "deque": "cpp",
         "string": "cpp",
-        "string_view": "cpp",
+        "vector": "cpp",
-        "chrono": "cpp"
+        "chrono": "cpp",
+        "array": "cpp"
     }
 }

1.txt

@@ -1,18 +0,0 @@
-ros2 topic pub --rate 10 /LF/motor_cmds rc_msgs/msg/MotorCmds "motor_cmd_0:
-  tau: 0.0
-  vw: 0.0
-  pos: 0.0
-  kp: 0.0
-  kd: 0.0
-motor_cmd_1:
-  tau: 0.0
-  vw: 0.0
-  pos: 0.0
-  kp: 0.0
-  kd: 0.0
-motor_cmd_2:
-  tau: 0.0
-  vw: 0.0
-  pos: 0.0
-  kp: 0.0
-  kd: 0.0"

README.md

@@ -1,19 +1,5 @@
 # CM_DOG
 
-A simple ROS2 program for a legged robot. Developed for Robocon2025.
+A simple ros2 program for legged robot . Robocon2025
 
-## 硬件配置
+有点不太会用ros2 controller 所以直接做的控制。
-
-- **关节电机**: Unitree GO-8010-6 (12个)
-- **IMU传感器**: 轮趣科技 N100陀螺仪
-- **通信接口**: USB转思路RS485
-
-## 软件依赖
-
-- Ros2 humble
-
-## 使用说明
-
-1. 克隆仓库：
-
-2. 构建项目：

src/commands/README.md

@@ -0,0 +1,3 @@
+# Control Command Inputs
+
+This folder contains the ros2 node for control input, like keyboard or gamepad.

src/commands/control_input_msgs/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.8)
+project(control_input_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/Inputs.msg"
+)
+
+ament_package()

src/commands/control_input_msgs/msg/Inputs.msg

@@ -0,0 +1,8 @@
+# control input message
+
+int32 command
+
+float32 lx
+float32 ly
+float32 rx
+float32 ry

src/commands/control_input_msgs/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>control_input_msgs</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+    <license>Apache-2.0</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>

src/commands/joystick_input/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.8)
+project(joystick_input)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(control_input_msgs REQUIRED)
+
+add_executable(joystick_input src/JoystickInput.cpp)
+target_include_directories(joystick_input
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ament_target_dependencies(
+        joystick_input PUBLIC
+        rclcpp
+        sensor_msgs
+        control_input_msgs
+)
+
+install(TARGETS
+        joystick_input
+        DESTINATION lib/${PROJECT_NAME})
+
+install(
+        DIRECTORY launch
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+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()

src/commands/joystick_input/README.md

@@ -0,0 +1,34 @@
+# Joystick Input
+
+This node will listen to the joystick topic and publish a control_input_msgs/Input message.
+
+Tested environment:
+* Ubuntu 24.04
+  * ROS2 Jazzy
+  * Logitech F310 Gamepad
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to joystick_input
+```
+
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch joystick_input joystick.launch.py
+```
+
+## 1. Use Instructions for Unitree Guide
+
+### 1.1 Control Mode
+
+* Passive Mode: LB + B
+* Fixed Stand: LB + A
+    * Free Stand: LB + X
+    * Trot: LB + Y
+    * SwingTest: LT + B
+    * Balance: LT + A
+
+### 1.2 Control Input
+
+* WASD IJKL: Move robot
+* Space: Reset Speed Input

src/commands/joystick_input/include/joystick_input/JoystickInput.h

@@ -0,0 +1,27 @@
+//
+// Created by tlab-uav on 24-9-13.
+//
+
+#ifndef JOYSTICKINPUT_H
+#define JOYSTICKINPUT_H
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+
+
+class JoystickInput final : public rclcpp::Node {
+public:
+    JoystickInput();
+
+    ~JoystickInput() override = default;
+
+private:
+    void joy_callback(sensor_msgs::msg::Joy::SharedPtr msg);
+
+    control_input_msgs::msg::Inputs inputs_;
+    rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription_;
+};
+
+
+#endif //JOYSTICKINPUT_H

src/commands/joystick_input/launch/joystick.launch.py

@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='joy',
+            executable='joy_node',
+            name='joynode',
+            parameters=[{
+                'dev': '/dev/input/js0'
+            }]
+        ),
+        Node(
+            package='joystick_input',
+            executable='joystick_input',
+            name='joystick_input_node'
+        )
+    ])

src/commands/joystick_input/package.xml

@@ -1,18 +1,17 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>ps2_controller</name>
+  <name>joystick_input</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>A package to convert joystick signal to control input</description>
-  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>rc_msgs</depend>
+  <depend>sensor_msgs</depend>
-  <test_depend>ament_lint_auto</test_depend>
+  <depend>control_input_msgs</depend>
-  <test_depend>ament_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/commands/joystick_input/src/JoystickInput.cpp

@@ -0,0 +1,48 @@
+//
+// Created by tlab-uav on 24-9-13.
+//
+
+#include "joystick_input/JoystickInput.h"
+
+using std::placeholders::_1;
+
+JoystickInput::JoystickInput() : Node("joysick_input_node") {
+    publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
+    subscription_ = create_subscription<
+        sensor_msgs::msg::Joy>("joy", 10, std::bind(&JoystickInput::joy_callback, this, _1));
+}
+
+void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
+    if (msg->buttons[0]) {
+        inputs_.command = 1; // BACK
+    } else if (msg->buttons[1]) {
+        inputs_.command = 2; // RB
+    } else if (msg->buttons[3]) {
+        inputs_.command = 3; // LB
+    } else if (msg->buttons[4]) {
+        inputs_.command = 4; // Y
+    } else if (msg->buttons[6]) {
+        inputs_.command = 5; // X
+    } else if (msg->buttons[7]) {
+        inputs_.command = 6; // B
+    } else if (msg->buttons[8]) {
+        inputs_.command = 7; // A
+    } else if (msg->buttons[9]) {
+         inputs_.command = 8;
+    } else {
+        inputs_.command = 0;
+        inputs_.lx = -msg->axes[0];
+        inputs_.ly = msg->axes[1];
+        inputs_.rx = -msg->axes[2];
+        inputs_.ry = msg->axes[3];
+    }
+    publisher_->publish(inputs_);
+}
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<JoystickInput>();
+    spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/commands/keyboard_input/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(keyboard_input)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(control_input_msgs REQUIRED)
+
+add_executable(keyboard_input src/KeyboardInput.cpp)
+target_include_directories(keyboard_input
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ament_target_dependencies(
+        keyboard_input PUBLIC
+        rclcpp
+        control_input_msgs
+)
+
+install(TARGETS
+        keyboard_input
+        DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/commands/keyboard_input/README.md

@@ -0,0 +1,33 @@
+# Keyboard Input
+
+This node will read the keyboard input and publish a control_input_msgs/Input message.
+
+Tested environment:
+* Ubuntu 24.04
+  * ROS2 Jazzy
+* Ubuntu 22.04
+  * ROS2 Humble
+
+### Build Command
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to keyboard_input
+```
+
+### Launch Command
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 run keyboard_input keyboard_input
+```
+
+## 1. Use Instructions for Unitree Guide
+### 1.1 Control Mode
+* Passive Mode: Keyboard 1
+* Fixed Stand: Keyboard 2
+    * Free Stand: Keyboard 3
+    * Trot: Keyboard 4
+    * SwingTest: Keyboard 5
+    * Balance: Keyboard 6
+### 1.2 Control Input
+* WASD IJKL: Move robot
+* Space: Reset Speed Input

src/commands/keyboard_input/include/keyboard_input/KeyboardInput.h

@@ -0,0 +1,53 @@
+//
+// Created by biao on 24-9-11.
+//
+
+
+#ifndef KEYBOARDINPUT_H
+#define KEYBOARDINPUT_H
+#include <rclcpp/rclcpp.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+#include <termios.h>
+
+
+template <typename T1, typename T2>
+T1 max(const T1 a, const T2 b) {
+    return (a > b ? a : b);
+}
+
+template <typename T1, typename T2>
+T1 min(const T1 a, const T2 b) {
+    return (a < b ? a : b);
+}
+
+
+class KeyboardInput final : public rclcpp::Node {
+public:
+    KeyboardInput();
+
+    ~KeyboardInput() override {
+        tcsetattr(STDIN_FILENO, TCSANOW, &old_tio_);
+    }
+
+private:
+    void timer_callback();
+
+    void check_command(char key);
+    void check_value(char key);
+
+    static bool kbhit();
+
+    control_input_msgs::msg::Inputs inputs_;
+    rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    bool just_published_ = false;
+    int reset_count_ = 0;
+
+    float sensitivity_left_ = 0.05;
+    float sensitivity_right_ = 0.05;
+    termios old_tio_{}, new_tio_{};
+};
+
+
+#endif //KEYBOARDINPUT_H

src/commands/keyboard_input/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>keyboard_input</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>rclcpp</depend>
+    <depend>control_input_msgs</depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/commands/keyboard_input/src/KeyboardInput.cpp

@@ -0,0 +1,150 @@
+//
+// Created by biao on 24-9-11.
+//
+
+#include <keyboard_input/KeyboardInput.h>
+
+KeyboardInput::KeyboardInput() : Node("keyboard_input_node") {
+    publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
+    timer_ = create_wall_timer(std::chrono::microseconds(100), std::bind(&KeyboardInput::timer_callback, this));
+    inputs_ = control_input_msgs::msg::Inputs();
+
+    tcgetattr(STDIN_FILENO, &old_tio_);
+    new_tio_ = old_tio_;
+    new_tio_.c_lflag &= (~ICANON & ~ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &new_tio_);
+    RCLCPP_INFO(get_logger(), "Keyboard input node started.");
+    RCLCPP_INFO(get_logger(), "Press 1-0 to switch between different modes");
+    RCLCPP_INFO(get_logger(), "Use W/S/A/D and I/K/J/L to move the robot.");
+    RCLCPP_INFO(get_logger(), "Please input keys, press Ctrl+C to quit.");
+}
+
+void KeyboardInput::timer_callback() {
+    if (kbhit()) {
+        char key = getchar();
+        check_command(key);
+        if (inputs_.command == 0) check_value(key);
+        else {
+            inputs_.lx = 0;
+            inputs_.ly = 0;
+            inputs_.rx = 0;
+            inputs_.ry = 0;
+            reset_count_ = 100;
+        }
+        publisher_->publish(inputs_);
+        just_published_ = true;
+    } else {
+        if (just_published_) {
+            reset_count_ -= 1;
+            if (reset_count_ == 0) {
+                just_published_ = false;
+                if (inputs_.command != 0) {
+                    inputs_.command = 0;
+                    publisher_->publish(inputs_);
+                }
+            }
+        }
+    }
+}
+
+void KeyboardInput::check_command(const char key) {
+    switch (key) {
+        case '1':
+            inputs_.command = 1; // L2_B
+            break;
+        case '2':
+            inputs_.command = 2; // L2_A
+            break;
+        case '3':
+            inputs_.command = 3; // L2_X
+            break;
+        case '4':
+            inputs_.command = 4; // L2_Y
+            break;
+        case '5':
+            inputs_.command = 5; // L1_A
+            break;
+        case '6':
+            inputs_.command = 6; // L1_B
+            break;
+        case '7':
+            inputs_.command = 7; // L1_X
+            break;
+        case '8':
+            inputs_.command = 8; // L1_Y
+            break;
+        case '9':
+            inputs_.command = 9;
+            break;
+        case '0':
+            inputs_.command = 10;
+        break;
+        case ' ':
+            inputs_.lx = 0;
+            inputs_.ly = 0;
+            inputs_.rx = 0;
+            inputs_.ry = 0;
+            inputs_.command = 0;
+            break;
+        default:
+            inputs_.command = 0;
+            break;
+    }
+}
+
+void KeyboardInput::check_value(char key) {
+    switch (key) {
+        case 'w':
+        case 'W':
+            inputs_.ly = min<float>(inputs_.ly + sensitivity_left_, 1.0);
+            break;
+        case 's':
+        case 'S':
+            inputs_.ly = max<float>(inputs_.ly - sensitivity_left_, -1.0);
+            break;
+        case 'd':
+        case 'D':
+            inputs_.lx = min<float>(inputs_.lx + sensitivity_left_, 1.0);
+            break;
+        case 'a':
+        case 'A':
+            inputs_.lx = max<float>(inputs_.lx - sensitivity_left_, -1.0);
+            break;
+
+        case 'i':
+        case 'I':
+            inputs_.ry = min<float>(inputs_.ry + sensitivity_right_, 1.0);
+            break;
+        case 'k':
+        case 'K':
+            inputs_.ry = max<float>(inputs_.ry - sensitivity_right_, -1.0);
+            break;
+        case 'l':
+        case 'L':
+            inputs_.rx = min<float>(inputs_.rx + sensitivity_right_, 1.0);
+            break;
+        case 'j':
+        case 'J':
+            inputs_.rx = max<float>(inputs_.rx - sensitivity_right_, -1.0);
+            break;
+        default:
+            break;
+    }
+}
+
+bool KeyboardInput::kbhit() {
+    timeval tv = {0L, 0L};
+    fd_set fds;
+    FD_ZERO(&fds);
+    FD_SET(STDIN_FILENO, &fds);
+    return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
+}
+
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<KeyboardInput>();
+    spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.8)
+project(mdog_simple_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rc_msgs REQUIRED)
+find_package(control_input_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+
+add_executable(mdog_simple_controller
+  src/mdog_simple_controller.cpp
+  src/FSM/state_safe.cpp
+  src/FSM/state_stand.cpp
+  src/FSM/state_zero.cpp
+  src/FSM/state_walk.cpp
+  src/FSM/state_balance.cpp
+  src/FSM/state_troting.cpp
+  src/common/kinematics.cpp
+  src/common/user_math.cpp
+  src/common/bezier_curve.cpp
+  src/common/pid.cpp
+)
+ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs )
+
+# add_executable(mdog_sim_controller
+#   src/mdog_sim_controller.cpp
+#   src/FSM/state_safe.cpp
+#   src/FSM/state_zero.cpp
+#   src/FSM/state_balance.cpp
+#   src/FSM/state_troting.cpp
+#   src/common/kinematics.cpp
+#   src/common/user_math.cpp
+#   src/common/bezier_curve.cpp
+# )
+# ament_target_dependencies(mdog_sim_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs)
+
+install(TARGETS
+  mdog_simple_controller
+  # mdog_sim_controller
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

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

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;
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_safe.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateSafe : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp

@@ -0,0 +1,40 @@
+#pragma once
+#include <array>
+#include <cstdint>
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateStand : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+
+private:
+    bool inited = false;
+    bool if_stand = false;
+    uint16_t joint_reached = 0;
+    float max_speed = 0.001;
+    float pos_threshold = 0.01;
+    float speed_threshold = 0.01;
+    // 默认PD参数
+    std::array<double, 3> kp = {4.5, 4.5, 4.5};
+    std::array<double, 3> kd = {0.05, 0.05, 0.05};
+
+    std::array<std::array<double, 3>, 4> stand_foot_pos = {
+        {
+            {0.0, 0.05, -0.25}, // FR
+            {0.0, 0.05, -0.25}, // FL
+            {0.0, 0.05, -0.25}, // RR
+            {0.0, 0.05, -0.25}  // RL
+        }
+    };
+
+    std::array<std::array<double, 3>, 4> target_joint_pos;
+    std::array<std::array<double, 3>, 4> start_joint_pos;
+    std::array<std::array<double, 3>, 4> last_joint_pos;
+
+};
+
+}

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;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_walk.hpp

@@ -0,0 +1,13 @@
+#pragma once
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateWalk : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+};
+
+}

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;
+};
+
+}

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/bezier_curve.hpp

@@ -0,0 +1,18 @@
+#pragma once
+#include <vector>
+#include <array>
+
+namespace mdog_simple_controller {
+namespace bezier {
+
+// 一维贝塞尔曲线
+double bezier_curve_1d(const std::vector<double>& control_points, double t);
+
+// 二维贝塞尔曲线
+std::array<double, 2> bezier_curve_2d(const std::vector<std::array<double, 2>>& control_points, double t);
+
+// 三维贝塞尔曲线
+std::array<double, 3> bezier_curve_3d(const std::vector<std::array<double, 3>>& control_points, double t);
+
+} // namespace bezier
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp

@@ -0,0 +1,69 @@
+#pragma once
+#include <array>
+
+namespace mdog_simple_controller {
+namespace kinematics {
+
+// 固定三连杆长度（单位：米，可根据实际机器人参数修改）
+constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.08, 0.25, 0.25};
+
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length();
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs();
+
+// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL）
+const std::array<std::array<double, 3>, 4>& get_hip_offsets();
+
+
+// 正运动学：已知关节角，求末端位置
+void forward_kinematics(const std::array<double, 3>& theta,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& pos);
+
+// 逆运动学：已知末端位置，求关节角。返回值为是否可达
+bool inverse_kinematics(const std::array<double, 3>& pos,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& theta);
+
+// 躯干->单腿坐标变换
+void foot_body_to_leg(const std::array<double, 3>& foot_body,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_leg);
+
+// 单腿->躯干坐标变换
+void foot_leg_to_body(const std::array<double, 3>& foot_leg,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_body);
+
+// 躯干姿态变换（欧拉角+高度） body->world
+void body_to_world(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_body,
+                   std::array<double, 3>& foot_world);
+
+// world->body
+void world_to_body(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_world,
+                   std::array<double, 3>& foot_body);
+
+// 综合正运动学（关节角->世界坐标）
+void leg_forward_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& theta,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& foot_world);
+
+// 综合逆运动学（世界坐标->关节角）
+bool leg_inverse_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& foot_world,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& theta);
+                                  
+} // namespace kinematics
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+namespace mdog_simple_controller {
+
+struct PIDParam {
+    double kp{0.0};
+    double ki{0.0};
+    double kd{0.0};
+    double dt{0.01};
+    double max_out{1e6};
+    double min_out{-1e6};
+};
+
+class PID {
+public:
+    PID(const PIDParam& param);
+
+    void set_param(const PIDParam& param);
+    void reset();
+    double compute(double setpoint, double measurement);
+
+    const PIDParam& get_param() const { return param_; }
+
+private:
+    PIDParam param_;
+    double prev_error_{0.0};
+    double integral_{0.0};
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+// 关节0点位置offset和减速比定义
+constexpr float JOINT_OFFSET[4][3] = {
+    {-5.149f, -5.8456f, 21.230f}, // 可根据实际填写
+    {-3.156f, 0.0f, 0.0f},
+    {0.0f, 0.0f, 0.0f},
+    // {0.2f, 0.393f, 0.5507f}
+    {5.63f, 2.9f, 0.0f}
+};
+
+constexpr float JOINT_RATIO[4][3] = {
+    {6.33f, 6.33f, 6.33f},
+    {6.33f, 6.33f, 6.33f},
+    {6.33f, 6.33f, 6.33f},
+    {6.33f, 6.33f, 6.33f}
+};
+
+// feedback → real
+void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst);
+
+// real → feedback
+void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst);
+
+// real cmd → 输出 cmd
+void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst);
+
+void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd);
+
+double limit_speed(double current, double target, double max_speed);
+int8_t get_direction(double target, double current);
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include "rc_msgs/msg/leg_fdb.hpp"
+#include "rc_msgs/msg/leg_cmd.hpp"
+#include "control_input_msgs/msg/inputs.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "geometry_msgs/msg/vector3.hpp"
+#include "mdog_simple_controller/FSM/state_safe.hpp"
+#include "mdog_simple_controller/FSM/state_stand.hpp"
+#include "mdog_simple_controller/FSM/state_zero.hpp"
+#include "mdog_simple_controller/FSM/state_walk.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 eulr;
+    float hight;
+    int8_t status;
+};
+
+struct MdogControllerData {
+    LegJointData feedback[4];
+    LegJointData feedback_real[4];
+    LegJointCmd command[4];
+    LegJointCmd command_real[4];
+    AHRS_Eulr_t imu_eulr;
+    AHRS_Eulr_t cmd_eulr;
+};
+
+
+class MdogSimpleController : public rclcpp::Node {
+public:
+    MdogSimpleController();
+
+    void change_state(FSMState* new_state);
+
+    MdogControllerData data_;
+    InputCmd input_cmd_;
+
+    bool if_no_hardware_{false};
+    bool if_debug_{false};
+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};
+    StateSafe state_safe_;
+    StateStand state_stand_;
+    StateZero state_zero_;
+    StateWalk state_walk_;
+    StateBalance state_balance_;
+    StateTroting state_troting_;
+
+
+
+    rclcpp::Subscription<rc_msgs::msg::LegFdb>::SharedPtr fdb_sub_;
+    rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr input_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr ahrs_sub_;
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
+
+    rclcpp::Publisher<rc_msgs::msg::LegCmd>::SharedPtr cmd_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mdog_simple_controller',
+            executable='mdog_simple_controller',
+            name='mdog_simple_controller',
+            output='screen',
+            parameters=[{'if_no_hardware': False}]
+        )
+    ])

src/controllers/mdog_simple_controller/launch/no_hardware.launch.py

@@ -4,12 +4,10 @@ from launch_ros.actions import Node
 def generate_launch_description():
     return LaunchDescription([
         Node(
-            package='unitree_motor_serial_driver',
+            package='mdog_simple_controller',
-            executable='goM8010_6_motor',
+            executable='mdog_simple_controller',
-            name='goM8010_6_motor',
+            name='mdog_simple_controller',
             output='screen',
-            parameters=[
+            parameters=[{'if_no_hardware': True}]
-
-            ]
         )
     ])

src/controllers/mdog_simple_controller/package.xml

@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>unitree_motor_serial_driver</name>
+  <name>mdog_simple_controller</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="1683502971@qq.com">robofish</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-
   <depend>rclcpp</depend>
-  <depend>rclcpp_components</depend>
-  <depend>std_msgs</depend>
   <depend>rc_msgs</depend>
+  <depend>control_input_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp

@@ -0,0 +1,68 @@
+#include "mdog_simple_controller/FSM/state_balance.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+
+void StateBalance::enter(MdogSimpleController* ctrl) {
+    // 进入BALANCE状态时的初始化
+}
+
+void StateBalance::run(MdogSimpleController* ctrl) {
+    using namespace kinematics;
+    const auto& link = get_leg_link_length();
+    const auto& hip_signs = get_hip_signs();
+
+    // 获取当前躯干高度和欧拉角
+    double height = ctrl->input_cmd_.hight > 0.1 ? ctrl->input_cmd_.hight : 0.35;
+    std::array<double, 3> body_pos = {0.0, 0.0, height};
+    std::array<double, 3> eulr = {
+        ctrl->data_.imu_eulr.rol,
+        ctrl->data_.imu_eulr.pit,
+        ctrl->data_.imu_eulr.yaw
+    };
+
+    // 原地平衡：足端目标点固定在body系下
+    std::array<std::array<double, 3>, 4> foot_body_targets;
+    for (int leg = 0; leg < 4; ++leg) {
+        // 可加扰动补偿：如roll/pitch影响下的z修正
+        double dz = 0.0;
+        // dz = -body_pos[2] * std::sin(eulr[0]); // roll补偿示例
+        foot_body_targets[leg] = {0.0, 0.0, -height + dz};
+    }
+
+    // 计算足端世界坐标
+    std::array<std::array<double, 3>, 4> foot_world_targets;
+    for (int leg = 0; leg < 4; ++leg) {
+        std::array<double, 3> foot_leg;
+        foot_body_to_leg(foot_body_targets[leg], get_hip_offsets()[leg], foot_leg);
+        body_to_world(body_pos, eulr, foot_leg, foot_world_targets[leg]);
+    }
+
+    // 逆运动学求解每条腿的关节角
+    for (int leg = 0; leg < 4; ++leg) {
+        std::array<double, 3> theta;
+        bool ok = leg_inverse_kinematics_world(
+            leg, foot_world_targets[leg], link, body_pos, eulr, theta);
+        if (ok) {
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 1.0;
+                ctrl->data_.command_real[leg].kd[j] = 0.02;
+            }
+        }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateBalance::exit(MdogSimpleController* ctrl) {
+    // 离开BALANCE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_safe.cpp

@@ -0,0 +1,27 @@
+#include "mdog_simple_controller/FSM/state_safe.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+void StateSafe::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.05;
+        }
+    }
+}
+
+void StateSafe::run(MdogSimpleController* ctrl) {
+    // SAFE状态下的控制逻辑
+}
+
+void StateSafe::exit(MdogSimpleController* ctrl) {
+    // 离开SAFE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp

@@ -0,0 +1,69 @@
+#include "mdog_simple_controller/FSM/state_stand.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+namespace mdog_simple_controller {
+
+void StateStand::enter(MdogSimpleController* ctrl) {
+    inited = false;
+    if_stand = false;
+    max_speed = 0.04;
+    joint_reached = 0;
+    pos_threshold = 0.01;
+    speed_threshold = 0.01;
+
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    for (int leg = 0; leg < 4; ++leg) {
+        kinematics::inverse_kinematics(stand_foot_pos[leg], kinematics::get_leg_link_length(), target_joint_pos[leg]);
+        for (int j = 0; j < 3; ++j) {
+            start_joint_pos[leg][j] = ctrl->data_.feedback_real[leg].pos[j];
+            last_joint_pos[leg][j] = start_joint_pos[leg][j];
+            ctrl->data_.command_real[leg].pos_des[j] = 0;
+            ctrl->data_.command_real[leg].speed_des[j] = 0;
+            ctrl->data_.command_real[leg].torque_des[j] = 0;
+            ctrl->data_.command_real[leg].kp[j] = kp[j];
+            ctrl->data_.command_real[leg].kd[j] = kd[j];
+
+
+        }
+        target_joint_pos[leg][0] *= hip_signs[leg];
+    }
+
+}
+
+void StateStand::run(MdogSimpleController* ctrl) {
+
+    if (!if_stand) {
+        for (int leg = 0; leg < 4; ++leg) {
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = limit_speed(ctrl->data_.feedback_real[leg].pos[j], target_joint_pos[leg][j], max_speed);
+                if (!ctrl->if_no_hardware_){
+                    if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) > pos_threshold) {
+                        if (std::abs(ctrl->data_.feedback_real[leg].speed[j]) < speed_threshold) {
+                            ctrl->data_.command_real[leg].torque_des[j] += 0.01*get_direction(target_joint_pos[leg][j], ctrl->data_.feedback_real[leg].pos[j]);
+                            RCLCPP_INFO(ctrl->get_logger(), "leg %d joint %d torque_des: %f", leg, j, ctrl->data_.command_real[leg].torque_des[j]);
+
+                    } else {
+                        ctrl->data_.command_real[leg].speed_des[j] = 0;
+                    }
+                }
+                if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) < 0.01) {
+                    //将对应标志位置为1
+                    joint_reached |= (1 << (leg * 3 + j));
+                }
+            }
+        }
+        if (joint_reached == 0b0000111111111111) {
+            if_stand = true;
+        }
+    }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateStand::exit(MdogSimpleController* ctrl) {
+    // 离开SAFE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp

@@ -0,0 +1,88 @@
+#include "mdog_simple_controller/FSM/state_troting.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 1.2;
+}
+
+void StateTroting::enter(MdogSimpleController* ctrl) {
+    // 进入TROTING状态时的初始化
+}
+
+void StateTroting::run(MdogSimpleController* ctrl) {
+    //设置所有cmd数据为0
+    std::vector<std::array<double, 3>> control_points = {
+        {0.25, 0.0, -0.35},      // 起点
+        {0.1, 0.0, -0.25},     // 抬腿中点
+        {-0.05, 0.0, -0.35},     // 落点
+        // {0.40, 0.0, 0.05},      // 起点
+        // {0.40, 0.0, 0.05},     // 抬腿中点
+
+        // {0.40, 0.0, 0.05},     // 抬腿中点
+        // {0.40, 0.0, 0.05},     // 抬腿中点
+        // {0.40, 0.0, 0.05},     // 落点
+    };
+    // 起点和终点
+    const auto& p0 = control_points.front();
+    const auto& p1 = control_points.back();
+
+    // 时间推进
+    static rclcpp::Time last_time = ctrl->now();
+    rclcpp::Time now = ctrl->now();
+    double dt = (now - last_time).seconds();
+    last_time = now;
+    bezier_time += dt;
+    if (bezier_time > bezier_period) bezier_time -= bezier_period;
+    double t = bezier_time / bezier_period; // [0,1]
+
+    const auto& link = kinematics::get_leg_link_length();
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    // 对角腿同步，交错运动，相位差0.5
+    for (int leg = 0; leg < 4; ++leg) {
+        // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
+        double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5;
+        double phase = std::fmod(t + phase_offset, 1.0);
+
+        std::array<double, 3> foot_pos;
+        if (phase < 0.5) {
+            // 摆动相：贝塞尔曲线
+            double swing_t = phase / 0.5; // 归一化到[0,1]
+            foot_pos = bezier::bezier_curve_3d(control_points, swing_t);
+        } else {
+            // 支撑相：直线连接首尾
+            double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1]
+            for (int i = 0; i < 3; ++i) {
+                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t;
+            }
+        }
+
+        std::array<double, 3> theta;
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            // 使用参数化的hip方向
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 1.2;
+                ctrl->data_.command_real[leg].kd[j] = 0.02;
+            }
+        }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateTroting::exit(MdogSimpleController* ctrl) {
+    // 离开TROTING状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp

@@ -0,0 +1,127 @@
+#include "mdog_simple_controller/FSM/state_walk.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+// ...existing code...
+namespace mdog_simple_controller 
+{
+
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 2;
+constexpr double swing_ratio = 0.25; // 摆动相占整个周期的比例
+const double phase_offset[4] = {0.0, 0.25, 0.5, 0.75}; // 四条腿交错
+
+// 新增：回位阶段相关变量
+bool homing_done = false;
+double homing_time = 0.0;
+constexpr double homing_duration = 3.0; // 回位持续3秒
+}
+
+void StateWalk::enter(MdogSimpleController* ctrl) {
+    homing_done = false;
+    homing_time = 0.0;
+    bezier_time = 0.0;
+    // 记录进入回位时每条腿的当前位置
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 保存初始关节角度
+            ctrl->data_.command_real[leg].pos_des[j] = ctrl->data_.feedback_real[leg].pos[j];
+        }
+    }
+}
+
+void StateWalk::run(MdogSimpleController* ctrl) {
+    static rclcpp::Time last_time = ctrl->now();
+    rclcpp::Time now = ctrl->now();
+    double dt = (now - last_time).seconds();
+    last_time = now;
+
+    if (!homing_done) {
+        homing_time += dt;
+        double alpha = std::min(homing_time / homing_duration, 1.0); // 插值系数
+
+        std::array<double, 3> home_pos = {0.0, 0.0, -0.35};
+        const auto& link = kinematics::get_leg_link_length();
+        const auto& hip_signs = kinematics::get_hip_signs();
+
+        std::array<double, 3> target_theta;
+        kinematics::inverse_kinematics(home_pos, link, target_theta);
+
+        for (int leg = 0; leg < 4; ++leg) {
+            std::array<double, 3> theta;
+            for (int j = 0; j < 3; ++j) {
+                // 每次都用当前位置做起点
+                double start = ctrl->data_.feedback_real[leg].pos[j];
+                double tgt = (j == 0) ? (target_theta[j] * hip_signs[leg]) : target_theta[j];
+                theta[j] = (1 - alpha) * start + alpha * tgt;
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 0.8;
+                ctrl->data_.command_real[leg].kd[j] = 0.01;
+            }
+        }
+        realcmd_to_cmd(ctrl->data_, ctrl->data_);
+
+        if (homing_time >= homing_duration) {
+            homing_done = true;
+        }
+        return;
+    }
+
+    // 以下内容必须在 run 函数体内
+    std::vector<std::array<double, 3>> control_points = {
+        {0.25, 0.0, -0.35},      // 起点
+        {0.1, 0.0, -0.25},     // 抬腿中点
+        {-0.05, 0.0, -0.35},     // 落点
+    };
+    const auto& p0 = control_points.front();
+    const auto& p1 = control_points.back();
+
+    bezier_time += dt;
+    if (bezier_time > bezier_period) bezier_time -= bezier_period;
+    double t = bezier_time / bezier_period; // [0,1]
+
+    const auto& link = kinematics::get_leg_link_length();
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    for (int leg = 0; leg < 4; ++leg) {
+        double phase = std::fmod(t + phase_offset[leg], 1.0);
+        std::array<double, 3> foot_pos;
+        if (phase < swing_ratio) {
+            // 摆动相：贝塞尔曲线
+            double swing_phase = phase / swing_ratio; // 归一化到[0,1]
+            foot_pos = bezier::bezier_curve_3d(control_points, swing_phase);
+        } else {
+            // 支撑相：直线滑动
+            double stance_phase = (phase - swing_ratio) / (1.0 - swing_ratio); // 归一化到[0,1]
+            for (int i = 0; i < 3; ++i) {
+                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_phase;
+            }
+        }
+
+        std::array<double, 3> theta;
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 0.8;
+                ctrl->data_.command_real[leg].kd[j] = 0.01;
+            }
+        }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+}
+
+void StateWalk::exit(MdogSimpleController* ctrl) {
+    // 可选：状态退出时清理
+}
+
+}

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.05;
+        }
+    }
+}
+
+void StateZero::run(MdogSimpleController* ctrl) {
+    // ZERO状态下的控制逻辑
+}
+
+void StateZero::exit(MdogSimpleController* ctrl) {
+    // 离开ZERO状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/common/bezier_curve.cpp

@@ -0,0 +1,46 @@
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+namespace bezier {
+
+// 组合数
+static double binomial_coefficient(int n, int k) {
+    double res = 1.0;
+    for (int i = 1; i <= k; ++i) {
+        res *= (n - i + 1) / double(i);
+    }
+    return res;
+}
+
+double bezier_curve_1d(const std::vector<double>& control_points, double t) {
+    int n = control_points.size() - 1;
+    double result = 0.0;
+    for (int i = 0; i <= n; ++i) {
+        double binom = binomial_coefficient(n, i);
+        result += binom * std::pow(1 - t, n - i) * std::pow(t, i) * control_points[i];
+    }
+    return result;
+}
+
+std::array<double, 2> bezier_curve_2d(const std::vector<std::array<double, 2>>& control_points, double t) {
+    std::vector<double> x, y;
+    for (const auto& pt : control_points) {
+        x.push_back(pt[0]);
+        y.push_back(pt[1]);
+    }
+    return { bezier_curve_1d(x, t), bezier_curve_1d(y, t) };
+}
+
+std::array<double, 3> bezier_curve_3d(const std::vector<std::array<double, 3>>& control_points, double t) {
+    std::vector<double> x, y, z;
+    for (const auto& pt : control_points) {
+        x.push_back(pt[0]);
+        y.push_back(pt[1]);
+        z.push_back(pt[2]);
+    }
+    return { bezier_curve_1d(x, t), bezier_curve_1d(y, t), bezier_curve_1d(z, t) };
+}
+
+} // namespace bezier
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/kinematics.cpp

@@ -0,0 +1,164 @@
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include <cmath>
+
+namespace mdog_simple_controller {
+namespace kinematics {
+
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length() {
+    return LEG_LINK_LENGTH;
+}
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs() {
+    static constexpr std::array<int, 4> HIP_SIGNS = {1, -1, 1, -1};
+    return HIP_SIGNS;
+}
+
+// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL）
+const std::array<std::array<double, 3>, 4>& get_hip_offsets() {
+    // 以body中心为原点，x前后，y左右，z向上
+    static constexpr std::array<std::array<double, 3>, 4> HIP_OFFSETS = {
+        std::array<double, 3>{ 0.20, -0.10, 0.0 }, // FR
+        std::array<double, 3>{ 0.20,  0.10, 0.0 }, // FL
+        std::array<double, 3>{-0.20, -0.10, 0.0 }, // RR
+        std::array<double, 3>{-0.20,  0.10, 0.0 }  // RL
+    };
+    return HIP_OFFSETS;
+}
+
+
+// 正运动学：已知关节角，求末端位置
+void forward_kinematics(const std::array<double, 3>& theta,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& pos) {
+    // theta: [hip_yaw, hip_pitch, knee_pitch]
+    // link:  [hip_offset(y方向), thigh_length(x方向), shank_length(x方向)]
+    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 = l1 + (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1);
+    double z = l2 * sin(t2) + l3 * sin(t2 + t3);
+
+    // 逆运动学中 x = -pos[2], y = pos[1], z = pos[0]
+    // 所以正运动学输出应为 pos[0]=z, pos[1]=y, pos[2]=-x
+    pos[0] = z;
+    pos[1] = y;
+    pos[2] = -x;
+}
+
+// 逆运动学：已知末端位置，求关节角
+bool inverse_kinematics(const std::array<double, 3>& pos,
+                        const std::array<double, 3>& link,
+                        std::array<double, 3>& theta) {
+    double x = -pos[2], y = pos[1], z = pos[0];
+    double l1 = link[0], l2 = link[1], l3 = link[2];
+
+    double y1 = y - l1;
+    double t1 = atan2(y1, x);
+
+    double d = sqrt(x * x + y1 * y1);
+    double z1 = z;
+
+    double D = (d * d + z1 * z1 - l2 * l2 - l3 * l3) / (2 * l2 * l3);
+    if (D < -1.0 || D > 1.0) return false;
+
+    double t3 = atan2(-sqrt(1 - D * D), D);
+    double k1 = l2 + l3 * cos(t3);
+    double k2 = l3 * sin(t3);
+    double t2 = atan2(z1, d) - atan2(k2, k1);
+
+    theta[0] = t1;
+    theta[1] = t2;
+    theta[2] = t3;
+    return true;
+}
+
+// 躯干->单腿坐标变换
+void foot_body_to_leg(const std::array<double, 3>& foot_body,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_leg) {
+    for (int i = 0; i < 3; ++i)
+        foot_leg[i] = foot_body[i] - hip_offset[i];
+}
+
+// 单腿->躯干坐标变换
+void foot_leg_to_body(const std::array<double, 3>& foot_leg,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_body) {
+    for (int i = 0; i < 3; ++i)
+        foot_body[i] = foot_leg[i] + hip_offset[i];
+}
+
+// 躯干->世界坐标变换
+void body_to_world(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_body,
+                   std::array<double, 3>& foot_world) {
+    double cr = cos(eulr[0]), sr = sin(eulr[0]);
+    double cp = cos(eulr[1]), sp = sin(eulr[1]);
+    double cy = cos(eulr[2]), sy = sin(eulr[2]);
+    double R[3][3] = {
+        {cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr},
+        {sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr},
+        {-sp,   cp*sr,            cp*cr}
+    };
+    for (int i = 0; i < 3; ++i) {
+        foot_world[i] = body_pos[i];
+        for (int j = 0; j < 3; ++j)
+            foot_world[i] += R[i][j] * foot_body[j];
+    }
+}
+
+// 世界->躯干坐标变换
+void world_to_body(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_world,
+                   std::array<double, 3>& foot_body) {
+    std::array<double, 3> delta;
+    for (int i = 0; i < 3; ++i)
+        delta[i] = foot_world[i] - body_pos[i];
+    double cr = cos(eulr[0]), sr = sin(eulr[0]);
+    double cp = cos(eulr[1]), sp = sin(eulr[1]);
+    double cy = cos(eulr[2]), sy = sin(eulr[2]);
+    double R[3][3] = {
+        {cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr},
+        {sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr},
+        {-sp,   cp*sr,            cp*cr}
+    };
+    for (int i = 0; i < 3; ++i) {
+        foot_body[i] = 0;
+        for (int j = 0; j < 3; ++j)
+            foot_body[i] += R[j][i] * delta[j];
+    }
+}
+
+// 综合正运动学（关节角->世界坐标）
+void leg_forward_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& theta,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& foot_world) {
+    std::array<double, 3> foot_leg, foot_body;
+    forward_kinematics(theta, link, foot_leg);
+    foot_leg_to_body(foot_leg, get_hip_offsets()[leg_index], foot_body);
+    body_to_world(body_pos, eulr, foot_body, foot_world);
+}
+
+bool leg_inverse_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& foot_world,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& theta) {
+    std::array<double, 3> foot_body, foot_leg;
+    world_to_body(body_pos, eulr, foot_world, foot_body);
+    foot_body_to_leg(foot_body, get_hip_offsets()[leg_index], foot_leg);
+    return inverse_kinematics(foot_leg, link, theta);
+}
+
+} // namespace kinematics
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/pid.cpp

@@ -0,0 +1,29 @@
+#include "mdog_simple_controller/common/pid.hpp"
+
+namespace mdog_simple_controller {
+
+PID::PID(const PIDParam& param)
+    : param_(param), prev_error_(0.0), integral_(0.0) {}
+
+void PID::set_param(const PIDParam& param) {
+    param_ = param;
+    reset();
+}
+
+void PID::reset() {
+    prev_error_ = 0.0;
+    integral_ = 0.0;
+}
+
+double PID::compute(double setpoint, double measurement) {
+    double error = setpoint - measurement;
+    integral_ += error * param_.dt;
+    double derivative = (error - prev_error_) / param_.dt;
+    double output = param_.kp * error + param_.ki * integral_ + param_.kd * derivative;
+    if (output > param_.max_out) output = param_.max_out;
+    if (output < param_.min_out) output = param_.min_out;
+    prev_error_ = error;
+    return output;
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/user_math.cpp

@@ -0,0 +1,86 @@
+#include "mdog_simple_controller/common/user_math.hpp"
+
+namespace mdog_simple_controller {
+
+// feedback → real
+void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：去掉offset，除以减速比
+            dst.feedback_real[leg].pos[j] = (src.feedback[leg].pos[j] - JOINT_OFFSET[leg][j]) / JOINT_RATIO[leg][j];
+            // 速度：除以减速比
+            dst.feedback_real[leg].speed[j] = src.feedback[leg].speed[j] / JOINT_RATIO[leg][j];
+            // 力矩：乘以减速比
+            dst.feedback_real[leg].torque[j] = src.feedback[leg].torque[j] * JOINT_RATIO[leg][j];
+        }
+    }
+}
+
+// real -> feedback
+void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：乘以减速比，加offset
+            dst.feedback[leg].pos[j] = src.feedback_real[leg].pos[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j];
+            // 速度：乘以减速比
+            dst.feedback[leg].speed[j] = src.feedback_real[leg].speed[j] * JOINT_RATIO[leg][j];
+            // 力矩：除以减速比
+            dst.feedback[leg].torque[j] = src.feedback_real[leg].torque[j] / JOINT_RATIO[leg][j];
+        }
+    }
+}
+
+// real cmd → 输出 cmd
+void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：乘以减速比，加offset
+            dst.command[leg].pos_des[j] = src.command_real[leg].pos_des[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j];
+            // 速度：乘以减速比
+            dst.command[leg].speed_des[j] = src.command_real[leg].speed_des[j] * JOINT_RATIO[leg][j];
+            // 力矩：除以减速比
+            dst.command[leg].torque_des[j] = src.command_real[leg].torque_des[j] / JOINT_RATIO[leg][j];
+            // kp/kd直接赋值
+            dst.command[leg].kp[j] = src.command_real[leg].kp[j];
+            dst.command[leg].kd[j] = src.command_real[leg].kd[j];
+        }
+    }
+}
+
+void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            data.command[leg].kp[j] = kp[j];
+            data.command[leg].kd[j] = kd[j];
+        }
+    }
+}
+
+//pd计算公式：
+// u =du + kp * (target - current) + kd * (target_speed - current_speed)
+
+// double simple_torque_ctrl(double detal_pos, )
+
+//计算方向
+int8_t get_direction(double target, double current) {
+    if (target > current) {
+        return 1;
+    } else if (target < current) {
+        return -1;
+    }
+    return 0;
+}
+
+// 移动速度最大限制器
+double limit_speed(double current, double target, double max_speed) {
+    double diff = target - current;
+    if (diff > max_speed) {
+        return current + max_speed;
+    } else if (diff < -max_speed) {
+        return current - max_speed;
+    }
+    return target;
+
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -0,0 +1,138 @@
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+
+namespace mdog_simple_controller
+{
+
+MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
+    fdb_sub_ = this->create_subscription<rc_msgs::msg::LegFdb>(
+        "/leg_fdb", 10,
+        std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1));
+    input_sub_ = this->create_subscription<control_input_msgs::msg::Inputs>(
+        "/control_input", 10,
+        std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1));
+    ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
+        "/eulr_angles", 10,
+        std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1));
+    cmd_pub_ = this->create_publisher<rc_msgs::msg::LegCmd>("/leg_cmd", 10);
+    joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
+
+    if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
+    if_debug_ = this->declare_parameter<bool>("if_debug", false);
+
+    memset(&data_, 0, sizeof(data_));
+    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;
+        }
+    }
+    feedback_to_real(data_, data_);
+}
+
+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.35;
+    input_cmd_.status = msg->command;
+}
+
+void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
+    data_.imu_eulr.yaw = msg->z;
+    data_.imu_eulr.rol = msg->x;
+    data_.imu_eulr.pit = msg->y;
+}
+
+void MdogSimpleController::control_loop() {
+    FSMState* target_state = nullptr;
+    switch (input_cmd_.status) {
+        case 0: target_state = &state_safe_; break;
+        // case 5: target_state = &state_balance_; break;
+        // case 6: target_state = &state_troting_; break;
+        // case 7: target_state = &state_walk_; break;
+        case 8: target_state = &state_stand_; break;
+        default: target_state = &state_safe_; break;
+    }
+    if (target_state != current_state_) {
+        change_state(target_state);
+    }
+    if (current_state_) current_state_->run(this);
+
+    if (if_no_hardware_) {
+        for (int leg = 0; leg < 4; ++leg) {
+            for (int j = 0; j < 3; ++j) {
+                data_.feedback_real[leg].torque[j] = data_.command_real[leg].torque_des[j];
+                data_.feedback_real[leg].speed[j]  = data_.command_real[leg].speed_des[j];
+                data_.feedback_real[leg].pos[j]    = data_.command_real[leg].pos_des[j];
+                data_.feedback[leg].torque[j] = data_.command_real[leg].torque_des[j];
+                data_.feedback[leg].speed[j]  = data_.command_real[leg].speed_des[j];
+            }
+        }
+        real_to_feedback(data_, data_);
+    }
+
+    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);
+    //打印调试信息
+
+        // RCLCPP_INFO(this->get_logger(), "cmd: %f %f %f %f %f %f %f %f %f %f %f %f",
+        //     data_.command[0].pos_des[0], data_.command[0].pos_des[1], data_.command[0].pos_des[2],
+        //     data_.command[1].pos_des[0], data_.command[1].pos_des[1], data_.command[1].pos_des[2],
+        //     data_.command[2].pos_des[0], data_.command[2].pos_des[1], data_.command[2].pos_des[2],
+        //     data_.command[3].pos_des[0], data_.command[3].pos_des[1], data_.command[3].pos_des[2]);
+
+    sensor_msgs::msg::JointState js_msg;
+    js_msg.header.stamp = this->now();
+    js_msg.name = {
+        "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
+        "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
+        "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint",
+        "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint"
+    };
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            js_msg.position.push_back(data_.feedback_real[leg].pos[j]);
+            js_msg.velocity.push_back(data_.feedback_real[leg].speed[j]);
+            js_msg.effort.push_back(data_.feedback_real[leg].torque[j]);
+        }
+    }
+    joint_state_pub_->publish(js_msg);
+}
+
+
+} // namespace mdog_simple_controller
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<mdog_simple_controller::MdogSimpleController>());
+    rclcpp::shutdown();
+    return 0;
+}

src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h

@@ -87,7 +87,7 @@ private:
   //topic
   std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
   std::string latlon_topic="latlon";
-  std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
+  std::string Eulr_angles_topic="/Eulr_angles", Magnetic_topic="/Magnetic";
   std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
 
 
@@ -97,7 +97,7 @@ private:
 
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
   
-  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Eulr_angles_pub_;
   rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
 
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr  twist_pub_;

src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
             'imu_frame_id_':'gyro_link',
             'mag_pose_2d_topic':'/mag_pose_2d',
             'Magnetic_topic':'/magnetic',
-            'Euler_angles_topic':'/euler_angles',
+            'Eulr_angles_topic':'/eulr_angles',
             'gps_topic':'/gps/fix',
             'twist_topic':'/system_speed',
             'NED_odom_topic':'/NED_odometry',

src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp

@@ -24,8 +24,8 @@ ahrsBringup::ahrsBringup()
   this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
   this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
 
-  this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
+  this->declare_parameter<std::string>("Eulr_angles_topic","/eulr_angles");
-  this->get_parameter("Euler_angles_topic", Euler_angles_topic);
+  this->get_parameter("Eulr_angles_topic", Eulr_angles_topic);
 
   this->declare_parameter<std::string>("gps_topic","/gps/fix");
   this->get_parameter("gps_topic", gps_topic);
@@ -51,7 +51,7 @@ ahrsBringup::ahrsBringup()
   // pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
   // pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
   // pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
-  // pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
+  // pravite_nh.param("Eulr_angles_pub_", Eulr_angles_topic_, std::string("/eulr_angles"));
   // pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
  
   //serial                                                 
@@ -64,7 +64,7 @@ ahrsBringup::ahrsBringup()
 
   mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
 
-  Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
+  Eulr_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Eulr_angles_topic.c_str(), 10);
   Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
  
   twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
@@ -500,15 +500,15 @@ void ahrsBringup::processLoop()  // 数据处理过程
       pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
       mag_pose_pub_->publish(pose_2d);
       //std::cout << "YAW: " << pose_2d.theta << std::endl;
-      geometry_msgs::msg::Vector3 Euler_angles_2d,Magnetic;  
+      geometry_msgs::msg::Vector3 Eulr_angles_2d,Magnetic;  
-      Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
+      Eulr_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
-      Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
+      Eulr_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
-      Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
+      Eulr_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
       Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
       Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
       Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
 
-      Euler_angles_pub_->publish(Euler_angles_2d);
+      Eulr_angles_pub_->publish(Eulr_angles_2d);
       Magnetic_pub_->publish(Magnetic);
 
     }

src/hardware/unitree_leg_serial_driver/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.8)
+project(unitree_leg_serial_driver)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(serial REQUIRED)
+find_package(rc_msgs REQUIRED)
+
+# 添加头文件目录
+include_directories(include)
+
+# 添加源文件并生成共享库
+add_library(${PROJECT_NAME}_lib SHARED  # 修改目标名称，避免冲突
+  src/unitree_leg_serial.cpp
+  src/crc_ccitt.cpp
+)
+
+# 链接依赖库
+ament_target_dependencies(${PROJECT_NAME}_lib
+  rclcpp
+  rclcpp_components
+  std_msgs
+  serial
+  rc_msgs
+)
+
+# 注册组件
+rclcpp_components_register_nodes(${PROJECT_NAME}_lib "unitree_leg_serial::UnitreeLegSerial")
+
+# 安装目标文件和头文件
+install(TARGETS ${PROJECT_NAME}_lib
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+ament_package()

src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/crc_ccitt.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdlib.h>
+
+
+namespace crc_ccitt {
+
+uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c);
+
+/**
+ *	crc_ccitt - recompute the CRC (CRC-CCITT variant) for the data
+ *	buffer
+ *	@crc: previous CRC value
+ *	@buffer: data pointer
+ *	@len: number of bytes in the buffer
+ */
+uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len);
+
+} // namespace crc_ccitt

src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/gom_protocol.hpp

@@ -0,0 +1,109 @@
+#pragma once
+
+#include "crc_ccitt.hpp"
+
+#pragma pack(1)
+/**
+ * @brief 电机模式控制信息
+ *
+ */
+typedef struct
+{
+    uint8_t id : 4;      // 电机ID: 0,1...,13,14 15表示向所有电机广播数据(此时无返回)
+    uint8_t status : 3;  // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
+    uint8_t reserve : 1; // 保留位
+} RIS_Mode_t;            // 控制模式 1Byte
+
+/**
+ * @brief 电机状态控制信息
+ *
+ */
+typedef struct
+{
+    int16_t tor_des; // 期望关节输出扭矩 unit: N.m      (q8)
+    int16_t spd_des; // 期望关节输出速度 unit: rad/s    (q8)
+    int32_t pos_des; // 期望关节输出位置 unit: rad      (q15)
+    int16_t k_pos;   // 期望关节刚度系数 unit: -1.0-1.0 (q15)
+    int16_t k_spd;   // 期望关节阻尼系数 unit: -1.0-1.0 (q15)
+
+} RIS_Comd_t; // 控制参数 12Byte
+
+/**
+ * @brief 电机状态反馈信息
+ *
+ */
+typedef struct
+{
+    int16_t torque;      // 实际关节输出扭矩 unit: N.m     (q8)
+    int16_t speed;       // 实际关节输出速度 unit: rad/s   (q8)
+    int32_t pos;         // 实际关节输出位置 unit: rad     (q15)
+    int8_t temp;         // 电机温度: -128~127°C
+    uint8_t MError : 3;  // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留
+    uint16_t force : 12; // 足端气压传感器数据 12bit (0-4095)
+    uint8_t none : 1;    // 保留位
+} RIS_Fbk_t;             // 状态数据 11Byte
+
+/**
+ * @brief 控制数据包格式
+ *
+ */
+typedef struct
+{
+    uint8_t head[2]; // 包头         2Byte
+    RIS_Mode_t mode; // 电机控制模式  1Byte
+    RIS_Comd_t comd; // 电机期望数据 12Byte
+    uint16_t CRC16;  // CRC          2Byte
+
+} RIS_ControlData_t; // 主机控制命令     17Byte
+
+/**
+ * @brief 电机反馈数据包格式
+ *
+ */
+typedef struct
+{
+    uint8_t head[2]; // 包头         2Byte
+    RIS_Mode_t mode; // 电机控制模式  1Byte
+    RIS_Fbk_t fbk;   // 电机反馈数据 11Byte
+    uint16_t CRC16;  // CRC          2Byte
+
+} RIS_MotorData_t; // 电机返回数据     16Byte
+
+#pragma pack()
+
+/// @brief 电机指令结构体
+typedef struct
+{
+    unsigned short id;   // 电机ID，15代表广播数据包
+    unsigned short mode; // 0:空闲 1:FOC控制 2:电机标定
+    float T;             // 期望关节的输出力矩(电机本身的力矩)(Nm)
+    float W;             // 期望关节速度(电机本身的速度)(rad/s)
+    float Pos;           // 期望关节位置(rad)
+    float K_P;           // 关节刚度系数(0-25.599)
+    float K_W;           // 关节速度系数(0-25.599)
+
+    RIS_ControlData_t motor_send_data;
+
+} MotorCmd_t;
+
+/// @brief 电机反馈结构体
+typedef struct
+{
+    unsigned char motor_id; // 电机ID
+    unsigned char mode;     // 0:空闲 1:FOC控制 2:电机标定
+    int Temp;               // 温度
+    int MError;             // 错误码
+    float T;                // 当前实际电机输出力矩(电机本身的力矩)(Nm)
+    float W;                // 当前实际电机速度(电机本身的速度)(rad/s)
+    float Pos;              // 当前电机位置(rad)
+    int correct;            // 接收数据是否完整(1完整，0不完整)
+    int footForce;          // 足端力传感器原始数值
+
+    uint16_t calc_crc;
+    uint32_t timeout;       // 通讯超时 数量
+    uint32_t bad_msg;       // CRC校验错误 数量
+    RIS_MotorData_t motor_recv_data; // 电机接收数据结构体
+
+} MotorData_t;
+
+#pragma pack()

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

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <serial/serial.h>
+#include <atomic>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <thread>
+#include <array>
+#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
+{
+
+class UnitreeLegSerial : public rclcpp::Node
+{
+public:
+    explicit UnitreeLegSerial(const rclcpp::NodeOptions &options);
+    ~UnitreeLegSerial() override;
+
+    void write(const std::array<MotorCmd_t, 12>& cmds);
+    std::array<MotorData_t, 12> read();
+
+private:
+    static constexpr int PORT_NUM = 4;
+    static constexpr int MOTOR_NUM = 3;
+
+    void receive_data(int port_idx);
+    void reopen_port(int port_idx);
+    void send_motor_data(int port_idx, const MotorCmd_t &cmd);
+    void motor_update();
+    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]{};
+    bool if_debug_{true};
+    rclcpp::Time last_freq_time_;
+
+    std::array<std::unique_ptr<serial::Serial>, PORT_NUM> serial_port_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    std::array<std::thread, PORT_NUM> read_thread_;
+    std::atomic<bool> running_{false};
+    rclcpp::Publisher<rc_msgs::msg::LegFdb>::SharedPtr leg_fdb_pub_;
+    rclcpp::Subscription<rc_msgs::msg::LegCmd>::SharedPtr leg_cmd_sub_;
+
+    // 状态管理
+    enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 };
+    std::atomic<StatusFlag> status_flag_{OFFLINE};
+    std::array<std::atomic<int8_t>, PORT_NUM> tick_;
+
+    // 串口参数
+    std::array<std::string, PORT_NUM> serial_port_name_;
+    std::array<int, PORT_NUM> baud_rate_;
+
+    // 电机数据
+    std::array<std::array<MotorCmd_t, MOTOR_NUM>, PORT_NUM> motor_cmd_;
+    std::array<std::array<MotorData_t, MOTOR_NUM>, PORT_NUM> motor_fbk_;
+    std::array<int, PORT_NUM> current_motor_idx_{};
+    std::array<std::array<int, MOTOR_NUM>, PORT_NUM> send_id_count_{};
+};
+
+} // namespace unitree_leg_serial

src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py

@@ -0,0 +1,29 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "if_debug",
+            default_value="true",
+            description="If true, use ROS topics for communication"
+        ),
+        ComposableNodeContainer(
+            name="component_container",
+            namespace="",
+            package="rclcpp_components",
+            executable="component_container",
+            composable_node_descriptions=[
+                ComposableNode(
+                    package="unitree_leg_serial_driver",
+                    plugin="unitree_leg_serial::UnitreeLegSerial",
+                    name="unitree_leg_serial",
+                    parameters=[{"if_debug": LaunchConfiguration("if_debug")}]
+                )
+            ],
+            output="screen"
+        )
+    ])

src/hardware/unitree_leg_serial_driver/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>unitree_leg_serial_driver</name>
+  <version>0.0.0</version>
+  <description>Unitree Leg Serial Driver for ROS 2</description>
+  <maintainer email="1683502971@qq.com">robofish</maintainer>
+  <license>Apache-2.0</license>
+
+  <!-- Build dependencies -->
+  <buildtool_depend>ament_cmake</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>
+
+  <!-- Required for packages with interfaces -->
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/hardware/unitree_leg_serial_driver/src/crc_ccitt.cpp

@@ -1,14 +1,10 @@
-#ifndef __CRC_CCITT_H
+#include "unitree_leg_serial_driver/crc_ccitt.hpp"
-#define __CRC_CCITT_H
 
-/*
+namespace crc_ccitt
- * This mysterious table is just the CRC of each possible byte. It can be
+{
- * computed using the standard bit-at-a-time methods. The polynomial can
+
- * be seen in entry 128, 0x8408. This corresponds to x^0 + x^5 + x^12.
+// CRC-CCITT 预计算表
- * Add the implicit x^16, and you have the standard CRC-CCITT.
+const uint16_t crc_ccitt_table[256] = {
- * https://github.com/torvalds/linux/blob/5bfc75d92efd494db37f5c4c173d3639d4772966/lib/crc-ccitt.c
- */
-uint16_t const crc_ccitt_table[256] = {
 	0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
 	0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
 	0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
@@ -43,20 +39,25 @@ uint16_t const crc_ccitt_table[256] = {
 	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
 };
 
-
+/**
-static uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c)
+ * @brief 计算单字节的 CRC-CCITT 值
+ * @param crc 当前的 CRC 值
+ * @param c 输入字节
+ * @return 更新后的 CRC 值
+ */
+ uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c)
  {
 	 return (crc >> 8) ^ crc_ccitt_table[(crc ^ c) & 0xff];
  }
 
 /**
- *	crc_ccitt - recompute the CRC (CRC-CCITT variant) for the data
+ * @brief 计算数据缓冲区的 CRC-CCITT 值
- *	buffer
+ * @param crc 初始 CRC 值
- *	@crc: previous CRC value
+ * @param buffer 数据缓冲区指针
- *	@buffer: data pointer
+ * @param len 缓冲区长度
- *	@len: number of bytes in the buffer
+ * @return 计算后的 CRC 值
  */
-inline uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
+ uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
  {
 	 while (len--)
 		 crc = crc_ccitt_byte(crc, *buffer++);
@@ -64,4 +65,4 @@ inline uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
  }
  
 
-#endif
+} // namespace crc_ccitt

src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp

@@ -0,0 +1,312 @@
+#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
+{
+
+UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options)
+    : Node("unitree_leg_serial", options)
+{
+    // 串口名和波特率初始化
+    serial_port_name_ = {"/dev/ttyACM0", "/dev/ttyACM1", "/dev/ttyACM2", "/dev/ttyACM3"};
+    baud_rate_ = {4000000, 4000000, 4000000, 4000000};
+
+    last_freq_time_ = this->now();
+    leg_fdb_pub_ = this->create_publisher<rc_msgs::msg::LegFdb>("leg_fdb", 10);
+    leg_cmd_sub_ = this->create_subscription<rc_msgs::msg::LegCmd>(
+        "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;
+        recv_count_[p] = 0;
+        tick_[p] = 0;
+        current_motor_idx_[p] = 0;
+        send_id_count_[p].fill(0);
+
+        // 初始化每个串口的3个电机命令
+        for (int i = 0; i < MOTOR_NUM; ++i) {
+            motor_cmd_[p][i] = MotorCmd_t{};
+            motor_cmd_[p][i].id = i;
+            motor_cmd_[p][i].mode = 1;
+        }
+
+        if (!open_serial_port(p)) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_[p].c_str());
+            continue;
+        }
+    }
+    status_flag_ = OFFLINE;
+
+    running_ = true;
+    // 启动每个串口的接收线程
+    for (int p = 0; p < PORT_NUM; ++p) {
+        if (serial_port_[p] && serial_port_[p]->isOpen()) {
+            read_thread_[p] = std::thread(&UnitreeLegSerial::receive_data, this, p);
+        }
+    }
+
+    // 定时器，轮询所有串口
+    timer_ = this->create_wall_timer(
+        std::chrono::microseconds(333),
+        [this]() { motor_update(); });
+}
+
+UnitreeLegSerial::~UnitreeLegSerial()
+{
+    running_ = false;
+    for (int p = 0; p < PORT_NUM; ++p) {
+        if (read_thread_[p].joinable()) {
+            read_thread_[p].join();
+        }
+        close_serial_port(p);
+    }
+}
+
+void UnitreeLegSerial::write(const std::array<MotorCmd_t, 12>& cmds)
+{
+    for (int p = 0; p < PORT_NUM; ++p) {
+        for (int m = 0; m < MOTOR_NUM; ++m) {
+            int idx = p * MOTOR_NUM + m;
+            if (idx < 12) {
+                motor_cmd_[p][m] = cmds[idx];
+            }
+        }
+    }
+    status_flag_ = CONTROLED;
+}
+
+std::array<MotorData_t, 12> UnitreeLegSerial::read()
+{
+    std::array<MotorData_t, 12> result;
+    for (int p = 0; p < PORT_NUM; ++p) {
+        for (int m = 0; m < MOTOR_NUM; ++m) {
+            int idx = p * MOTOR_NUM + m;
+            if (idx < 12) {
+                result[idx] = motor_fbk_[p][m];
+            }
+        }
+    }
+    return result;
+}
+
+void UnitreeLegSerial::leg_cmd_callback(const rc_msgs::msg::LegCmd::SharedPtr msg)
+{
+    std::array<MotorCmd_t, 12> 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 {
+        serial_port_[port_idx] = std::make_unique<serial::Serial>(
+            serial_port_name_[port_idx], baud_rate_[port_idx], serial::Timeout::simpleTimeout(1000));
+        return serial_port_[port_idx]->isOpen();
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(this->get_logger(), "Serial open exception [%d]: %s", port_idx, e.what());
+        return false;
+    }
+}
+
+void UnitreeLegSerial::close_serial_port(int port_idx)
+{
+    if (serial_port_[port_idx] && serial_port_[port_idx]->isOpen()) {
+        serial_port_[port_idx]->close();
+    }
+}
+
+void UnitreeLegSerial::motor_update()
+{
+    for (int p = 0; p < PORT_NUM; ++p) {
+        if (tick_[p] < 500) {
+            ++tick_[p];
+        } else {
+            status_flag_ = OFFLINE;
+        }
+
+        if (status_flag_ != CONTROLED) {
+            for (int i = 0; i < MOTOR_NUM; ++i) {
+                motor_cmd_reset(p, i);
+            }
+        }
+        // 每次只发送一个电机命令，轮流发送
+        send_motor_data(p, motor_cmd_[p][current_motor_idx_[p]]);
+        send_count_[p]++;
+        send_id_count_[p][current_motor_idx_[p]]++;
+        current_motor_idx_[p] = (current_motor_idx_[p] + 1) % MOTOR_NUM;
+    }
+
+    // 每秒打印一次频率和所有电机状态
+    auto now = this->now();
+    if ((now - last_freq_time_).seconds() >= 1.0) {
+        for (int p = 0; p < PORT_NUM; ++p) {
+            RCLCPP_INFO(this->get_logger(), "[Port%d] 发送频率: %d Hz, 接收频率: %d Hz", p, send_count_[p], recv_count_[p]);
+            for (int i = 0; i < MOTOR_NUM; ++i) {
+                RCLCPP_INFO(this->get_logger(),
+                    "[Port%d] 电机%d: send_count=%d, Pos=%.3f, W=%.3f, T=%.3f, Temp=%d, Mode=%d, Correct=%d",
+                    p, i, send_id_count_[p][i],
+                    motor_fbk_[p][i].Pos,
+                    motor_fbk_[p][i].W,
+                    motor_fbk_[p][i].T,
+                    motor_fbk_[p][i].Temp,
+                    motor_fbk_[p][i].mode,
+                    motor_fbk_[p][i].correct
+                );
+                send_id_count_[p][i] = 0; // 重置
+            }
+            send_count_[p] = 0;
+            recv_count_[p] = 0;
+        }
+        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)
+{
+    motor_cmd_[port_idx][idx] = MotorCmd_t{};
+    motor_cmd_[port_idx][idx].id = idx;
+    motor_cmd_[port_idx][idx].mode = 1;
+}
+
+void UnitreeLegSerial::send_motor_data(int port_idx, const MotorCmd_t &cmd)
+{
+    if (!serial_port_[port_idx] || !serial_port_[port_idx]->isOpen()) {
+        RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[Port%d] Serial port is not open.", port_idx);
+        return;
+    }
+
+    RIS_ControlData_t out;
+    out.head[0] = 0xFE;
+    out.head[1] = 0xEE;
+
+    auto saturate = [](auto &val, auto min, auto max) {
+        if (val < min) val = min;
+        else if (val > max) val = max;
+    };
+
+    MotorCmd_t cmd_copy = cmd;
+    saturate(cmd_copy.id, 0, 15);
+    saturate(cmd_copy.mode, 0, 7);
+    saturate(cmd_copy.K_P, 0.0f, 25.599f);
+    saturate(cmd_copy.K_W, 0.0f, 25.599f);
+    saturate(cmd_copy.T, -127.99f, 127.99f);
+    saturate(cmd_copy.W, -804.00f, 804.00f);
+    saturate(cmd_copy.Pos, -411774.0f, 411774.0f);
+
+    out.mode.id = cmd_copy.id;
+    out.mode.status = cmd_copy.mode;
+    out.comd.k_pos = cmd_copy.K_P / 25.6f * 32768.0f;
+    out.comd.k_spd = cmd_copy.K_W / 25.6f * 32768.0f;
+    out.comd.pos_des = cmd_copy.Pos / 6.28318f * 32768.0f;
+    out.comd.spd_des = cmd_copy.W / 6.28318f * 256.0f;
+    out.comd.tor_des = cmd_copy.T * 256.0f;
+    out.CRC16 = crc_ccitt::crc_ccitt(
+        0, (uint8_t *)&out, sizeof(RIS_ControlData_t) - sizeof(out.CRC16));
+
+    serial_port_[port_idx]->write(reinterpret_cast<const uint8_t *>(&out), sizeof(RIS_ControlData_t));
+}
+
+void UnitreeLegSerial::receive_data(int port_idx)
+{
+    size_t packet_size = sizeof(RIS_MotorData_t);
+    std::vector<uint8_t> buffer(packet_size * 2);
+    size_t buffer_offset = 0;
+
+    while (running_) {
+        try {
+            size_t bytes_read = serial_port_[port_idx]->read(buffer.data() + buffer_offset, buffer.size() - buffer_offset);
+            if (bytes_read == 0) continue;
+            buffer_offset += bytes_read;
+
+            size_t header_index = 0;
+            while (header_index < buffer_offset - 1) {
+                if (buffer[header_index] == 0xFD && buffer[header_index + 1] == 0xEE) break;
+                ++header_index;
+            }
+            if (header_index >= buffer_offset - 1) {
+                buffer_offset = 0;
+                continue;
+            }
+            if (header_index > 0) {
+                std::memmove(buffer.data(), buffer.data() + header_index, buffer_offset - header_index);
+                buffer_offset -= header_index;
+            }
+            if (buffer_offset < packet_size) continue;
+
+            RIS_MotorData_t recv_data;
+            std::memcpy(&recv_data, buffer.data(), packet_size);
+
+            int id = recv_data.mode.id;
+            if (id < 0 || id >= MOTOR_NUM) {
+                buffer_offset -= packet_size;
+                std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset);
+                continue;
+            }
+
+            if (recv_data.head[0] != 0xFD || recv_data.head[1] != 0xEE) {
+                motor_fbk_[port_idx][id].correct = 0;
+            } else {
+                motor_fbk_[port_idx][id].calc_crc = crc_ccitt::crc_ccitt(
+                    0, (uint8_t *)&recv_data, sizeof(RIS_MotorData_t) - sizeof(recv_data.CRC16));
+                if (recv_data.CRC16 != motor_fbk_[port_idx][id].calc_crc) {
+                    memset(&motor_fbk_[port_idx][id].motor_recv_data, 0, sizeof(RIS_MotorData_t));
+                    motor_fbk_[port_idx][id].correct = 0;
+                    motor_fbk_[port_idx][id].bad_msg++;
+                } else {
+                    std::memcpy(&motor_fbk_[port_idx][id].motor_recv_data, &recv_data, packet_size);
+                    motor_fbk_[port_idx][id].motor_id = recv_data.mode.id;
+                    motor_fbk_[port_idx][id].mode = recv_data.mode.status;
+                    motor_fbk_[port_idx][id].Temp = recv_data.fbk.temp;
+                    motor_fbk_[port_idx][id].MError = recv_data.fbk.MError;
+                    motor_fbk_[port_idx][id].W = ((float)recv_data.fbk.speed / 256.0f) * 6.28318f;
+                    motor_fbk_[port_idx][id].T = ((float)recv_data.fbk.torque) / 256.0f;
+                    motor_fbk_[port_idx][id].Pos = 6.28318f * ((float)recv_data.fbk.pos) / 32768.0f;
+                    motor_fbk_[port_idx][id].footForce = recv_data.fbk.force;
+                    motor_fbk_[port_idx][id].correct = 1;
+                }
+            }
+            if (motor_fbk_[port_idx][id].correct) {
+                recv_count_[port_idx]++;
+                RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+                    "[Port%d] Motor ID: %d, Position: %f", port_idx, motor_fbk_[port_idx][id].motor_id, motor_fbk_[port_idx][id].Pos);
+            }
+            std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size);
+            buffer_offset -= packet_size;
+        } catch (const std::exception &e) {
+            RCLCPP_ERROR(this->get_logger(), "Serial read exception [%d]: %s", port_idx, e.what());
+            reopen_port(port_idx);
+        }
+    }
+}
+
+void UnitreeLegSerial::reopen_port(int port_idx)
+{
+    close_serial_port(port_idx);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    open_serial_port(port_idx);
+}
+
+} // namespace unitree_leg_serial
+
+RCLCPP_COMPONENTS_REGISTER_NODE(unitree_leg_serial::UnitreeLegSerial)

src/ps2_controller/CMakeLists.txt

@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(ps2_controller)
-
-# Default to C++17
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-# Find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rc_msgs REQUIRED)  # 添加对 rc_msgs 的依赖
-
-add_executable(ps2_reader src/ps2_reader.cpp)
-
-ament_target_dependencies(ps2_reader rclcpp rc_msgs)  # 添加 rc_msgs 依赖
-
-install(DIRECTORY launch
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-install(TARGETS
-  ps2_reader
-  DESTINATION lib/${PROJECT_NAME})
-
-ament_package()

src/ps2_controller/launch/ps2_controller.launch.py

@@ -1,15 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='ps2_controller',
-            executable='ps2_reader',
-            name='ps2_controller_node',
-            output='screen',
-            parameters=[
-                {'device': '/dev/input/js0'}  # 可根据需要修改设备路径
-            ]
-        )
-    ])

src/ps2_controller/src/ps2_reader.cpp

@@ -1,123 +0,0 @@
-#include <rclcpp/rclcpp.hpp>
-#include <linux/joystick.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <iostream>
-#include <string>
-#include "rc_msgs/msg/ps2_data.hpp"  // 引入 Ps2Data 消息类型
-
-class PS2ControllerNode : public rclcpp::Node
-{
-public:
-    PS2ControllerNode() : Node("ps2_controller_node")
-    {
-        this->declare_parameter<std::string>("device", "/dev/input/js0");
-        device_path_ = this->get_parameter("device").as_string();
-
-        joystick_fd_ = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK);
-        if (joystick_fd_ < 0)
-        {
-            RCLCPP_ERROR(this->get_logger(), "Failed to open device: %s", device_path_.c_str());
-            return;
-        }
-
-        RCLCPP_INFO(this->get_logger(), "Connected to device: %s", device_path_.c_str());
-
-        ps2_data_ = std::make_shared<rc_msgs::msg::Ps2Data>();
-        publisher_ = this->create_publisher<rc_msgs::msg::Ps2Data>("ps2_data", 10);
-
-        timer_ = this->create_wall_timer(
-            std::chrono::milliseconds(10),  // 100Hz
-            std::bind(&PS2ControllerNode::publishData, this));
-    }
-
-    ~PS2ControllerNode()
-    {
-        if (joystick_fd_ >= 0)
-        {
-            close(joystick_fd_);
-        }
-    }
-
-private:
-    void publishData()
-    {
-        readJoystick();
-        publisher_->publish(*ps2_data_);
-    }
-
-    void readJoystick()
-    {
-        struct js_event event;
-        while (read(joystick_fd_, &event, sizeof(event)) > 0)
-        {
-            switch (event.type & ~JS_EVENT_INIT)
-            {
-            case JS_EVENT_BUTTON:
-                handleButtonEvent(event);
-                break;
-            case JS_EVENT_AXIS:
-                handleAxisEvent(event);
-                break;
-            default:
-                break;
-            }
-        }
-    }
-
-    void handleButtonEvent(const js_event &event)
-    {
-        switch (event.number)
-        {
-        case 10: ps2_data_->select = event.value; break;
-        case 11: ps2_data_->start = event.value; break;
-        case 6: ps2_data_->l1 = event.value; break;
-        case 8: ps2_data_->l2 = event.value; break;
-        case 7: ps2_data_->r1 = event.value; break;
-        case 9: ps2_data_->r2 = event.value; break;
-        case 3: ps2_data_->x = event.value; break;
-        case 4: ps2_data_->y = event.value; break;
-        case 0: ps2_data_->a = event.value; break;
-        case 1: ps2_data_->b = event.value; break;
-        default:
-            break;
-        }
-    }
-
-    void handleAxisEvent(const js_event &event)
-    {
-        constexpr float MAX_AXIS_VALUE = 32767.0f;  // 最大轴值
-        constexpr float MIN_AXIS_VALUE = -32768.0f; // 最小轴值
-
-        auto normalize = [](int value) -> float {
-            return static_cast<float>(value) / MAX_AXIS_VALUE;
-        };
-
-        switch (event.number)
-        {
-        case 0: ps2_data_->lx = normalize(event.value); break;
-        case 1: ps2_data_->ly = normalize(event.value); break;
-        case 2: ps2_data_->rx = normalize(event.value); break;
-        case 3: ps2_data_->ry = normalize(event.value); break;
-        case 6: ps2_data_->left_right = normalize(event.value); break;
-        case 7: ps2_data_->up_down = normalize(event.value); break;
-        default:
-            break;
-        }
-    }
-
-    std::string device_path_;
-    int joystick_fd_;
-    rclcpp::TimerBase::SharedPtr timer_;
-    rclcpp::Publisher<rc_msgs::msg::Ps2Data>::SharedPtr publisher_;
-    std::shared_ptr<rc_msgs::msg::Ps2Data> ps2_data_;
-};
-
-int main(int argc, char *argv[])
-{
-    rclcpp::init(argc, argv);
-    auto node = std::make_shared<PS2ControllerNode>();
-    rclcpp::spin(node);
-    rclcpp::shutdown();
-    return 0;
-}

src/rc_msgs/CMakeLists.txt

@@ -2,15 +2,20 @@ 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"
+        "msg/DataAI.msg"
         "msg/Ps2Data.msg"
-        "msg/DataControl.msg"
+        "msg/GoalPose.msg"
-        "msg/DataMotor.msg"
+        "msg/DataNav.msg"
-        "msg/DataLeg.msg"
+        "msg/GoMotorCmd.msg"
-        "msg/DataMotors.msg"
+        "msg/GoMotorFdb.msg"
-        "msg/MotorCmd.msg"
+        "msg/LegCmd.msg"
-        "msg/MotorCmds.msg"
+        "msg/LegFdb.msg"
+    DEPENDENCIES std_msgs
 )
 
 ament_package()
-

src/rc_msgs/README.md

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

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/DataControl.msg

@@ -1,5 +0,0 @@
-float32 tau
-float32 vw
-float32 pos
-float32 kp
-float32 kd

src/rc_msgs/msg/DataLeg.msg

@@ -1,7 +0,0 @@
-DataControl data_control_0
-DataControl data_control_1
-DataControl data_control_2
-
-DataMotor data_motor_0
-DataMotor data_motor_1
-DataMotor data_motor_2

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/DataMotor.msg

@@ -1,3 +0,0 @@
-float32 tau
-float32 vw
-float32 pos

src/rc_msgs/msg/DataMotors.msg

@@ -1,3 +0,0 @@
-DataMotor data_motor_0
-DataMotor data_motor_1
-DataMotor data_motor_2

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/GoMotorCmd.msg

@@ -0,0 +1,5 @@
+float32 torque_des
+float32 speed_des
+float32 pos_des
+float32 kp
+float32 kd

src/rc_msgs/msg/GoMotorFdb.msg

@@ -0,0 +1,3 @@
+float32 torque
+float32 speed
+float32 pos

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/LegCmd.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+GoMotorCmd[12] leg_cmd

src/rc_msgs/msg/LegFdb.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+GoMotorFdb[12] leg_fdb

src/rc_msgs/msg/MotorCmd.msg

@@ -1,5 +0,0 @@
-float32 tau
-float32 vw
-float32 pos
-float32 kp
-float32 kd

src/rc_msgs/msg/MotorCmds.msg

@@ -1,3 +0,0 @@
-MotorCmd motor_cmd_0
-MotorCmd motor_cmd_1
-MotorCmd motor_cmd_2

src/rc_msgs/msg/Ps2Data.msg

@@ -15,11 +15,6 @@ bool r2
 # 四种模式
 uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
 
-bool x
-bool y
-bool a
-bool b
-
 bool select
 bool start
 

src/rc_msgs/package.xml

@@ -5,7 +5,8 @@
     <description>Describe custom messages</description>
     <maintainer email="1683502971@qq.com">biao</maintainer>
     <license>MIT</license>
-
+    <build_depend>std_msgs</build_depend>
+    <exec_depend>std_msgs</exec_depend>
     <buildtool_depend>rosidl_default_generators</buildtool_depend>
     <exec_depend>rosidl_default_runtime</exec_depend>
     <member_of_group>rosidl_interface_packages</member_of_group>

src/robot_bringup/CMakeLists.txt

@@ -1,27 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(robot_bringup)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-
-install(DIRECTORY launch
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-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()

src/robot_bringup/launch/robot_bringup.launch.py

@@ -1,128 +0,0 @@
-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():
-    # 定义 leg_ctrl 的四个容器节点
-    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=[
-                    ('motor_cmds', 'LF/motor_cmds'),
-                    ('data_motors', 'LF/data_motors')
-                ]
-            )
-        ],
-        output='screen',
-    )
-
-    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=[
-                    ('motor_cmds', 'RF/motor_cmds'),
-                    ('data_motors', 'RF/data_motors')
-                ]
-            )
-        ],
-        output='screen',
-    )
-
-    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=[
-                    ('motor_cmds', 'LR/motor_cmds'),
-                    ('data_motors', 'LR/data_motors')
-                ]
-            )
-        ],
-        output='screen',
-    )
-
-    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=[
-                    ('motor_cmds', 'RR/motor_cmds'),
-                    ('data_motors', 'RR/data_motors')
-                ]
-            )
-        ],
-        output='screen',
-    )
-
-    # 定义 ahrs_driver 节点
-    ahrs_driver = Node(
-        package="fdilink_ahrs",
-        executable="ahrs_driver_node",
-        parameters=[{
-            'if_debug_': False,
-            'serial_port_': '/dev/ttyUSB0',
-            'serial_baud_': 921600,
-            'imu_topic': '/imu',
-            'imu_frame_id_': 'gyro_link',
-            'mag_pose_2d_topic': '/mag_pose_2d',
-            'Magnetic_topic': '/magnetic',
-            'Euler_angles_topic': '/euler_angles',
-            'gps_topic': '/gps/fix',
-            'twist_topic': '/system_speed',
-            'NED_odom_topic': '/NED_odometry',
-            'device_type_': 1
-        }],
-        output="screen"
-    )
-
-    # 定义 ps2_controller 节点
-    ps2_controller_node = Node(
-        package='ps2_controller',
-        executable='ps2_reader',
-        name='ps2_controller_node',
-        output='screen',
-        parameters=[
-            {'device': '/dev/input/js0'}  # 可根据需要修改设备路径
-        ]
-    )
-
-    # 返回主启动文件的描述
-    return LaunchDescription([
-        lf_container,
-        rf_container,
-        lr_container,
-        rr_container,
-        ahrs_driver,
-        ps2_controller_node  # 添加 ps2_controller 节点
-    ])

src/robot_descriptions/README.md

@@ -0,0 +1,68 @@
+# Robot Descriptions
+
+This folder contains the URDF and SRDF files for the quadruped robot.
+
+* Unitree
+    * [Go1](unitree/go1_description/)
+    * [Go2](unitree/go2_description/)
+    * [A1](unitree/a1_description/)
+    * [Aliengo](unitree/aliengo_description/)
+    * [B2](unitree/b2_description/)
+* Xiaomi
+    * [Cyberdog](xiaomi/cyberdog_description/)
+* Deep Robotics
+    * [Lite 3](deep_robotics/lite3_description/)
+    * [X30](deep_robotics/x30_description/)
+* Anybotics
+    * [Anymal C](anybotics/anymal_c_description/)
+
+## 1. Steps to transfer urdf to Mujoco model
+
+* Install [Mujoco](https://github.com/google-deepmind/mujoco)
+* Transfer the mesh files to mujoco supported format, like stl.
+* Adjust the urdf tile to match the mesh file. Transfer the mesh file from .dae to .stl may change the scale size of the
+  mesh file.
+* use `xacro` to generate the urdf file.
+  ```
+  xacro robot.xacro > ../urdf/robot.urdf
+  ```
+* use mujoco to convert the urdf file to mujoco model.
+  ```
+  compile robot.urdf robot.xml
+  ```
+
+## 2. Dependencies for Gazebo Simulation
+
+Gazebo Simulation only tested on ROS2 Jazzy. It add support for ROS2 Humble because the package name is different.
+
+* Gazebo Harmonic
+  ```bash
+  sudo apt-get install ros-jazzy-ros-gz
+  ```
+* Ros2-Control for Gazebo
+  ```bash
+  sudo apt-get install ros-jazzy-gz-ros2-control
+  ``` 
+* Legged PD Controller
+    ```bash
+    cd ~/ros2_ws
+    colcon build --packages-up-to leg_pd_controller
+    ```
+
+## 2. Dependencies for Gazebo Classic Simulation
+
+Gazebo Classic (Gazebo11) Simulation only tested on ROS2 Humble.
+
+* Gazebo Classic
+  ```bash
+  sudo apt-get install ros-humble-gazebo-ros
+  ```
+* Ros2-Control for Gazebo
+  ```bash
+  sudo apt-get install ros-humble-gazebo-ros2-control
+  ``` 
+* Legged PD Controller
+    ```bash
+    cd ~/ros2_ws
+    colcon build --packages-up-to leg_pd_controller
+    ```

src/robot_descriptions/anybotics/anymal_c_description/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(anymal_c_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+        DIRECTORY meshes xacro launch config urdf
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/robot_descriptions/anybotics/anymal_c_description/LICENSE

@@ -0,0 +1,29 @@
+Copyright 2020, ANYbotics AG.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in
+   the documentation and/or other materials provided with the
+   distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived
+   from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/robot_descriptions/anybotics/anymal_c_description/README.md

@@ -0,0 +1,39 @@
+# Anybotics Anymal_C Description
+This repository contains the urdf model of lite3.
+
+![anymal_c](../../../.images/anymal_c.png)
+
+Tested environment:
+* Ubuntu 24.04
+    * ROS2 Jazzy
+
+## Build
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to anymal_c_description --symlink-install
+```
+
+## Visualize the robot
+To visualize and check the configuration of the robot in rviz, simply launch:
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch anymal_c_description visualize.launch.py
+```
+
+## Launch ROS2 Control
+### Mujoco Simulator
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=anymal_c_description
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description
+  ```
+* Legged Gym Controller
+    ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description
+  ```

src/robot_descriptions/anybotics/anymal_c_description/config/ocs2/gait.info

@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.35
+     [2]     0.70
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}

src/robot_descriptions/anybotics/anymal_c_description/config/ocs2/reference.info

@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.5;
+targetRotationVelocity              1.57;
+
+comHeight                           0.6;
+
+defaultJointState
+{
+   (0,0)  -0.25   ; LF_HAA
+   (1,0)   0.60   ; LF_HFE
+   (2,0)  -0.85   ; LF_KFE
+   (3,0)  -0.25   ; LH_HAA
+   (4,0)  -0.60   ; LH_HFE
+   (5,0)   0.85   ; LH_KFE
+   (6,0)   0.25   ; RF_HAA
+   (7,0)   0.60   ; RF_HFE
+   (8,0)  -0.85   ; RF_KFE
+   (9,0)   0.25   ; RH_HAA
+   (10,0) -0.60   ; RH_HFE
+   (11,0)  0.85   ; RH_KFE
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}

src/robot_descriptions/anybotics/anymal_c_description/config/ocs2/task.info

@@ -0,0 +1,318 @@
+centroidalModelType             0      // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
+  
+legged_robot_interface
+{
+  verbose                               false  // show the loaded parameters
+}
+
+model_settings
+{
+  positionErrorGain             0.0 ; 20.0
+  phaseTransitionStanceTime     0.4
+
+  verboseCppAd                  true
+  recompileLibrariesCppAd       false
+  modelFolderCppAd              /tmp/ocs2_quadruped_controller/anymal_c
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.2
+  touchDownVelocity            -0.4
+  swingHeight                   0.1
+  touchdownAfterHorizon         0.2
+  swingTimeScale                0.15
+}
+
+; Multiple_Shooting SQP settings
+sqp
+{
+  nThreads                              3
+  dt                                    0.015
+  sqpIteration                          1
+  deltaTol                              1e-4
+  g_max                                 1e-2
+  g_min                                 1e-6
+  inequalityConstraintMu                0.1
+  inequalityConstraintDelta             5.0
+  projectStateInputEqualityConstraints  true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     true
+  integratorType                        RK2
+  threadPriority                        50
+}
+
+; Multiple_Shooting IPM settings
+ipm
+{
+  nThreads                              3
+  dt                                    0.015
+  ipmIteration                          1
+  deltaTol                              1e-4
+  g_max                                 10.0
+  g_min                                 1e-6
+  computeLagrangeMultipliers            true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     true
+  integratorType                        RK2
+  threadPriority                        50
+
+  initialBarrierParameter               1e-4
+  targetBarrierParameter                1e-4
+  barrierLinearDecreaseFactor           0.2
+  barrierSuperlinearDecreasePower       1.5
+  barrierReductionCostTol               1e-3
+  barrierReductionConstraintTol         1e-3
+
+  fractionToBoundaryMargin              0.995
+  usePrimalStepSizeForDual              false
+
+  initialSlackLowerBound                1e-4
+  initialDualLowerBound                 1e-4
+  initialSlackMarginRate                1e-2
+  initialDualMarginRate                 1e-2
+}
+
+; DDP settings
+ddp
+{
+  algorithm                       SLQ
+
+  nThreads                        3
+  threadPriority                  50
+
+  maxNumIterations                1
+  minRelCost                      1e-1
+  constraintTolerance             5e-3
+
+  displayInfo                     false
+  displayShortSummary             false
+  checkNumericalStability         false
+  debugPrintRollout               false
+
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  maxNumStepsPerSecond            10000
+  timeStep                        0.015
+  backwardPassIntegratorType      ODE45
+
+  constraintPenaltyInitialValue   20.0
+  constraintPenaltyIncreaseRate   2.0
+
+  preComputeRiccatiTerms          true
+
+  useFeedbackPolicy               false
+
+  strategy                        LINE_SEARCH
+  lineSearch
+  {
+    minStepLength                 1e-2
+    maxStepLength                 1.0
+    hessianCorrectionStrategy     DIAGONAL_SHIFT
+    hessianCorrectionMultiple     1e-5
+  }
+}
+
+; Rollout settings
+rollout
+{
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  timeStep                        0.015
+  integratorType                  ODE45
+  maxNumStepsPerSecond            10000
+  checkNumericalStability         false
+}
+
+mpc
+{
+  timeHorizon                     1.0  ; [s]
+  solutionTimeWindow              -1   ; maximum [s]
+  coldStart                       false
+
+  debugPrint                      false
+
+  mpcDesiredFrequency             100  ; [Hz]
+  mrtDesiredFrequency             500 ; [Hz]
+}
+
+initialState
+{
+   ;; Normalized Centroidal Momentum: [linear, angular] ;;
+   (0,0)  0.0     ; vcom_x
+   (1,0)  0.0     ; vcom_y
+   (2,0)  0.0     ; vcom_z
+   (3,0)  0.0     ; L_x / robotMass
+   (4,0)  0.0     ; L_y / robotMass
+   (5,0)  0.0     ; L_z / robotMass
+
+   ;; Base Pose: [position, orientation] ;;
+   (6,0)  0.0     ; p_base_x
+   (7,0)  0.0     ; p_base_y
+   (8,0)  0.575   ; p_base_z
+   (9,0)  0.0     ; theta_base_z
+   (10,0) 0.0     ; theta_base_y
+   (11,0) 0.0     ; theta_base_x
+
+   ;; Leg Joint Positions: [LF, LH, RF, RH] ;;
+   (12,0) -0.25   ; LF_HAA
+   (13,0)  0.60   ; LF_HFE
+   (14,0) -0.85   ; LF_KFE
+   (15,0) -0.25   ; LH_HAA
+   (16,0) -0.60   ; LH_HFE
+   (17,0)  0.85   ; LH_KFE
+   (18,0)  0.25   ; RF_HAA
+   (19,0)  0.60   ; RF_HFE
+   (20,0) -0.85   ; RF_KFE
+   (21,0)  0.25   ; RH_HAA
+   (22,0) -0.60   ; RH_HFE
+   (23,0)  0.85   ; RH_KFE
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   15.0     ; vcom_x
+  (1,1)   15.0     ; vcom_y
+  (2,2)   30.0     ; vcom_z
+  (3,3)   5.0      ; L_x / robotMass
+  (4,4)   10.0     ; L_y / robotMass
+  (5,5)   10.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   500.0    ; p_base_x
+  (7,7)   500.0    ; p_base_y
+  (8,8)   500.0    ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 200.0    ; theta_base_y
+  (11,11) 200.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [LF, LH, RF, RH] ;;
+  (12,12) 20.0     ; LF_HAA
+  (13,13) 20.0     ; LF_HFE
+  (14,14) 20.0     ; LF_KFE
+  (15,15) 20.0     ; LH_HAA
+  (16,16) 20.0     ; LH_HFE
+  (17,17) 20.0     ; LH_KFE
+  (18,18) 20.0     ; RF_HAA
+  (19,19) 20.0     ; RF_HFE
+  (20,20) 20.0     ; RF_KFE
+  (21,21) 20.0     ; RH_HAA
+  (22,22) 20.0     ; RH_HFE
+  (23,23) 20.0     ; RH_KFE
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [LF, RF, LH, RH] ;;
+  (0,0)   1.0       ; left_front_force
+  (1,1)   1.0       ; left_front_force
+  (2,2)   1.0       ; left_front_force
+  (3,3)   1.0       ; right_front_force
+  (4,4)   1.0       ; right_front_force
+  (5,5)   1.0       ; right_front_force
+  (6,6)   1.0       ; left_hind_force
+  (7,7)   1.0       ; left_hind_force
+  (8,8)   1.0       ; left_hind_force
+  (9,9)   1.0       ; right_hind_force
+  (10,10) 1.0       ; right_hind_force
+  (11,11) 1.0       ; right_hind_force
+
+  ;; foot velocity relative to base: [LF, LH, RF, RH] (uses the Jacobian at nominal configuration) ;;
+  (12,12) 5000.0    ; x
+  (13,13) 5000.0    ; y
+  (14,14) 5000.0    ; z
+  (15,15) 5000.0    ; x
+  (16,16) 5000.0    ; y
+  (17,17) 5000.0    ; z
+  (18,18) 5000.0    ; x
+  (19,19) 5000.0    ; y
+  (20,20) 5000.0    ; z
+  (21,21) 5000.0    ; x
+  (22,22) 5000.0    ; y
+  (23,23) 5000.0    ; z
+}
+
+frictionConeSoftConstraint
+{
+  frictionCoefficient    0.5
+  
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "LF_shank_fixed, RF_shank_fixed"
+    [1] "LH_shank_fixed, RH_shank_fixed"
+    [2] "LF_shank_fixed, LH_shank_fixed"
+    [3] "RF_shank_fixed, RH_shank_fixed"
+    [4] "LF_FOOT, RF_FOOT"
+    [5] "LH_FOOT, RH_FOOT"
+    [6] "LF_FOOT, LH_FOOT"
+    [7] "RF_FOOT, RH_FOOT"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  80.0     ; HAA
+   (1,0)  80.0     ; HFE
+   (2,0)  80.0     ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.015
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}

src/robot_descriptions/anybotics/anymal_c_description/config/robot_control.yaml

@@ -0,0 +1,249 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 200  # Hz
+
+    # Define the available controllers
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
+    ocs2_quadruped_controller:
+      type: ocs2_quadruped_controller/Ocs2QuadrupedController
+
+    rl_quadruped_controller:
+      type: rl_quadruped_controller/LeggedGymController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    joints:
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+
+    down_pos:
+      - -0.0
+      - 1.41
+      - -2.58
+      - 0.0
+      - 1.41
+      - -2.58
+      - -0.0
+      - -1.41
+      - 2.58
+      - 0.0
+      - -1.41
+      - 2.58
+
+    stand_pos:
+      - 0.2
+      - 0.6
+      - -0.85
+      - -0.2
+      - 0.6
+      - -0.85
+      - 0.2
+      - -0.6
+      - 0.85
+      - -0.2
+      - -0.6
+      - 0.85
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - LF_FOOT
+      - RF_FOOT
+      - LH_FOOT
+      - RH_FOOT
+
+    imu_name: "imu_sensor"
+    base_name: "base"
+
+    imu_interfaces:
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z
+
+ocs2_quadruped_controller:
+  ros__parameters:
+    update_rate: 100  # Hz
+    default_kd: 1.5
+    joints:
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet:
+      - LF_FOOT
+      - RF_FOOT
+      - LH_FOOT
+      - RH_FOOT
+
+
+    imu_name: "imu_sensor"
+    base_name: "base"
+
+    imu_interfaces:
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - LF
+      - RF
+      - LH
+      - RH
+
+rl_quadruped_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    robot_pkg: "anymal_c_description"
+    model_folder: "legged_gym"
+    joints:
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+
+    down_pos:
+      - -0.0
+      - 1.41
+      - -2.58
+      - 0.0
+      - 1.41
+      - -2.58
+      - -0.0
+      - -1.41
+      - 2.58
+      - 0.0
+      - -1.41
+      - 2.58
+
+    stand_pos:
+      - 0.2
+      - 0.6
+      - -0.85
+      - -0.2
+      - 0.6
+      - -0.85
+      - 0.2
+      - -0.6
+      - 0.85
+      - -0.2
+      - -0.6
+      - 0.85
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - LF_FOOT
+      - RF_FOOT
+      - LH_FOOT
+      - RH_FOOT
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - LF
+      - RF
+      - LH
+      - RH
+
+    imu_name: "imu_sensor"
+    base_name: "base"
+
+    imu_interfaces:
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z

src/robot_descriptions/anybotics/anymal_c_description/config/visualize_urdf.rviz

@@ -0,0 +1,383 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 303
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        FL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_chin:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_laserscan_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_laserscan_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_optical_chin:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_rearDown:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_rearDown:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        trunk:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 0.8687032461166382
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.017344314604997635
+        Y: -0.0033522010780870914
+        Z: -0.09058035165071487
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.49539798498153687
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8403961062431335
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 630
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003130000005efc0100000002fb0000000800540069006d0065010000000000000313000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000313000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 787
+  X: 763
+  Y: 351