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

.vscode/settings.json

@@ -1,8 +1,5 @@
 {
     "files.associations": {
-        "array": "cpp",
-        "string": "cpp",
-        "string_view": "cpp",
-        "chrono": "cpp"
+        "functional": "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,4 @@
 # CM_DOG
 
-A simple ROS2 program for a legged robot. Developed for Robocon2025.
+A simple ros2 program for legged robot . Robocon2025
 
-## 硬件配置
-
-- **关节电机**: Unitree GO-8010-6 (12个)
-- **IMU传感器**: 轮趣科技 N100陀螺仪
-- **通信接口**: USB转思路RS485
-
-## 软件依赖
-
-- Ros2 humble
-
-## 使用说明
-
-1. 克隆仓库：
-
-2. 构建项目：

build.sh

@@ -1,2 +0,0 @@
-source install/setup.bash 
-colcon build

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,19 +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>simple_quadruped_control</name>
+  <name>joystick_input</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="1683502971@qq.com">robofish</maintainer>
-  <license>TODO: License declaration</license>
+  <description>A package to convert joystick signal to control input</description>
+  <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>rc_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <depend>sensor_msgs</depend>
+  <depend>control_input_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/commands/joystick_input/src/JoystickInput.cpp

@@ -0,0 +1,50 @@
+//
+// 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[1] && msg->buttons[4]) {
+        inputs_.command = 1; // LB + B
+    } else if (msg->buttons[0] && msg->buttons[4]) {
+        inputs_.command = 2; // LB + A
+    } else if (msg->buttons[2] && msg->buttons[4]) {
+        inputs_.command = 3; // LB + X
+    } else if (msg->buttons[3] && msg->buttons[4]) {
+        inputs_.command = 4; // LB + Y
+    } else if (msg->axes[2] != 1 && msg->buttons[1]) {
+        inputs_.command = 5; // LT + B
+    } else if (msg->axes[2] != 1 && msg->buttons[0]) {
+        inputs_.command = 6; // LT + A
+    } else if (msg->axes[2] != 1 && msg->buttons[2]) {
+        inputs_.command = 7; // LT + X
+    } else if (msg->axes[2] != 1 && msg->buttons[3]) {
+        inputs_.command = 8; // LT + Y
+    } else if (msg->buttons[7]) {
+        inputs_.command = 9; // START
+    } else {
+        inputs_.command = 0;
+        inputs_.lx = -msg->axes[0];
+        inputs_.ly = msg->axes[1];
+        inputs_.rx = -msg->axes[3];
+        inputs_.ry = msg->axes[4];
+    }
+    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/hardware/mdog_hardware/CMakeLists.txt

@@ -0,0 +1,93 @@
+cmake_minimum_required(VERSION 3.8)
+project(mdog_hardware)
+
+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(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+add_library(
+  mdog_hardware
+  SHARED
+  src/mdog_hardware_interface.cpp
+)
+target_include_directories(
+  mdog_hardware
+  PUBLIC
+  include
+)
+ament_target_dependencies(
+  mdog_hardware
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+)
+# prevent pluginlib from using boost
+target_compile_definitions(mdog_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(
+  hardware_interface mdog_hardware.xml)
+
+install(
+  TARGETS
+  mdog_hardware
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+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()
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_mdog_hardware_interface test/test_mdog_hardware_interface.cpp)
+  target_include_directories(test_mdog_hardware_interface PRIVATE include)
+  ament_target_dependencies(
+    test_mdog_hardware_interface
+    hardware_interface
+    pluginlib
+    ros2_control_test_assets
+  )
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  mdog_hardware
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+ament_package()

src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
+#define MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
+
+#include <string>
+#include <vector>
+
+#include "mdog_hardware/visibility_control.h"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+namespace mdog_hardware
+{
+class MDogHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_init(
+    const hardware_interface::HardwareInfo & info) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::return_type read(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::return_type write(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  std::vector<double> hw_commands_;
+  std::vector<double> hw_states_;
+  std::vector<double> imu_states_;
+
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
+
+  void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
+};
+
+}  // namespace mdog_hardware
+
+#endif  // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_

src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h

@@ -0,0 +1,49 @@
+// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MDOG_HARDWARE__VISIBILITY_CONTROL_H_
+#define MDOG_HARDWARE__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport)
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
+#endif
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default")))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default")))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
+#endif
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif  // MDOG_HARDWARE__VISIBILITY_CONTROL_H_

src/hardware/mdog_hardware/launch/mdog_hardware.launch.py

@@ -0,0 +1,43 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import xacro  # 添加 xacro 模块
+
+
+def process_xacro():
+    # 获取 xacro 文件路径
+    pkg_path = os.path.join(get_package_share_directory('mdog_description'))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    # 处理 xacro 文件
+    robot_description_config = xacro.process_file(xacro_file)
+    return robot_description_config.toxml()
+
+
+def generate_launch_description():
+    # 生成机器人描述
+    robot_description = {'robot_description': process_xacro()}
+
+    # 启动机器人状态发布器
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[robot_description]
+    )
+
+    # 启动控制器管理器
+    controller_manager_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        output='screen',
+        parameters=[robot_description],
+        arguments=['--ros-args', '--log-level', 'info']
+    )
+
+    return LaunchDescription([
+        robot_state_publisher_node,
+        controller_manager_node
+    ])

src/hardware/mdog_hardware/mdog_hardware.xml

@@ -0,0 +1,9 @@
+<library path="mdog_hardware">
+  <class name="mdog_hardware/MDogHardwareInterface"
+         type="mdog_hardware::MDogHardwareInterface"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      ros2_control hardware interface.
+    </description>
+  </class>
+</library>

src/hardware/mdog_hardware/package.xml

@@ -1,7 +1,7 @@
 <?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_hardware</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="1683502971@qq.com">robofish</maintainer>
@@ -9,13 +9,20 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>hardware_interface</depend>
+
+  <depend>pluginlib</depend>
+
+  <depend>rclcpp_lifecycle</depend>
+
   <depend>rclcpp</depend>
-  <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
-  <depend>rc_msgs</depend>
-  
+  <depend>sensor_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp

@@ -0,0 +1,165 @@
+// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <limits>
+#include <vector>
+
+#include "mdog_hardware/mdog_hardware_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+namespace mdog_hardware
+{
+
+void MDogHardwareInterface::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
+{
+  imu_states_[0] = msg->orientation.w;
+  imu_states_[1] = msg->orientation.x;
+  imu_states_[2] = msg->orientation.y;
+  imu_states_[3] = msg->orientation.z;
+
+  imu_states_[4] = msg->angular_velocity.x;
+  imu_states_[5] = msg->angular_velocity.y;
+  imu_states_[6] = msg->angular_velocity.z;
+
+  imu_states_[7] = msg->linear_acceleration.x;
+  imu_states_[8] = msg->linear_acceleration.y;
+  imu_states_[9] = msg->linear_acceleration.z;
+}
+
+hardware_interface::CallbackReturn MDogHardwareInterface::on_init(
+  const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+  {
+    return CallbackReturn::ERROR;
+  }
+
+  // 初始化硬件状态和命令
+  hw_states_.resize(info_.joints.size() * 3, std::numeric_limits<double>::quiet_NaN());
+  hw_commands_.resize(info_.joints.size() * 5, std::numeric_limits<double>::quiet_NaN());
+  imu_states_.resize(10, std::numeric_limits<double>::quiet_NaN());
+
+  // 创建 ROS 2 节点和订阅器
+  node_ = std::make_shared<rclcpp::Node>("mdog_hardware_interface");
+  imu_subscription_ = node_->create_subscription<sensor_msgs::msg::Imu>(
+    "/imu", 10, std::bind(&MDogHardwareInterface::imu_callback, this, std::placeholders::_1));
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn MDogHardwareInterface::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
+
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> MDogHardwareInterface::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  for (size_t i = 0; i < info_.joints.size(); ++i)
+  {
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i * 3]));
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_[i * 3 + 1]));
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_[i * 3 + 2]));
+    
+  }
+  
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "orientation.w", &imu_states_[0]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "orientation.x", &imu_states_[1]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "orientation.y", &imu_states_[2]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "orientation.z", &imu_states_[3]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "angular_velocity.x", &imu_states_[4]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "angular_velocity.y", &imu_states_[5]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "angular_velocity.z", &imu_states_[6]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "linear_acceleration.x", &imu_states_[7]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "linear_acceleration.y", &imu_states_[8]));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+    "imu_sensor", "linear_acceleration.z", &imu_states_[9]));
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> MDogHardwareInterface::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+  for (size_t i = 0; i < info_.joints.size(); ++i)
+  {
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i * 5]));
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i * 5 + 1]));
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i * 5 + 2]));
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, "kp", &hw_commands_[i * 5 + 3]));
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, "kd", &hw_commands_[i * 5 + 4]));
+  }
+  return command_interfaces;
+}
+
+hardware_interface::CallbackReturn MDogHardwareInterface::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  // TODO(anyone): prepare the robot to receive commands
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn MDogHardwareInterface::on_deactivate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  // TODO(anyone): prepare the robot to stop receiving commands
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type MDogHardwareInterface::read(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  // TODO(anyone): read robot states
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type MDogHardwareInterface::write(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  // TODO(anyone): write robot's commands'
+
+  return hardware_interface::return_type::OK;
+}
+
+}  // namespace mdog_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  mdog_hardware::MDogHardwareInterface, hardware_interface::SystemInterface)

src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp

@@ -0,0 +1,57 @@
+// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+
+#include <string>
+
+#include "hardware_interface/resource_manager.hpp"
+#include "ros2_control_test_assets/components_urdfs.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+class TestMDogHardwareInterface : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    // TODO(anyone): Extend this description to your robot
+    mdog_hardware_interface_2dof_ =
+      R"(
+        <ros2_control name="MDogHardwareInterface2dof" type="system">
+          <hardware>
+            <plugin>mdog_hardware/MDogHardwareInterface</plugin>
+          </hardware>
+          <joint name="joint1">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+            <param name="initial_position">1.57</param>
+          </joint>
+          <joint name="joint2">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+            <param name="initial_position">0.7854</param>
+          </joint>
+        </ros2_control>
+    )";
+  }
+
+  std::string mdog_hardware_interface_2dof_;
+};
+
+TEST_F(TestMDogHardwareInterface, load_mdog_hardware_interface_2dof)
+{
+  auto urdf = ros2_control_test_assets::urdf_head + mdog_hardware_interface_2dof_ +
+              ros2_control_test_assets::urdf_tail;
+  ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf));
+}

src/hardware/unitree_leg_serial_driver/CMakeLists.txt

@@ -0,0 +1,53 @@
+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,58 @@
+#pragma once
+
+#include <serial/serial.h>
+#include <atomic>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <thread>
+#include "unitree_leg_serial_driver/crc_ccitt.hpp"
+#include "unitree_leg_serial_driver/gom_protocol.hpp"
+#include "rc_msgs/msg/go_motor_cmd.hpp"
+#include "rc_msgs/msg/go_motor_fdb.hpp"
+namespace unitree_leg_serial
+{
+
+class UnitreeLegSerial : public rclcpp::Node
+{
+public:
+    explicit UnitreeLegSerial(const rclcpp::NodeOptions &options);
+    ~UnitreeLegSerial() override;
+
+private:
+    void receive_data();
+    void reopen_port();
+    void send_motor_data(const MotorCmd_t &cmd);
+    void motor_update();
+    void motor_cmd_reset();
+    bool open_serial_port();
+    void close_serial_port();
+    void motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg);
+    
+    int send_count_;
+    int recv_count_;
+    rclcpp::Time last_freq_time_;
+
+    rclcpp::Publisher<rc_msgs::msg::GoMotorFdb>::SharedPtr motor_fdb_pub_;
+    rclcpp::Subscription<rc_msgs::msg::GoMotorCmd>::SharedPtr motor_cmd_sub_;
+    
+
+    std::unique_ptr<serial::Serial> serial_port_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    std::thread read_thread_;
+    std::atomic<bool> running_{false};
+
+    // 状态管理
+    enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 };
+    std::atomic<StatusFlag> status_flag_{OFFLINE};
+    std::atomic<int8_t> tick_{0};
+
+    // 串口参数
+    std::string serial_port_name_;
+    int baud_rate_;
+
+    // 电机数据
+    MotorCmd_t motor_cmd_;
+    MotorData_t motor_fbk_;
+};
+
+} // namespace unitree_leg_serial

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

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    return LaunchDescription([
+        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"
+                )
+            ],
+            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,67 +1,68 @@
-#ifndef __CRC_CCITT_H
-#define __CRC_CCITT_H
-
-/*
- * 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.
- * Add the implicit x^16, and you have the standard CRC-CCITT.
- * 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,
-	0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
-	0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
-	0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
-	0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
-	0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
-	0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
-	0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
-	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
-	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
-	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
-	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
-	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
-	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
-	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
-	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
-	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
-	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
-	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
-	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
-	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
-	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
-	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
-	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
-	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
-	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
-	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
-	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
-	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
-	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
-};
-
-
-static 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
- *	buffer
- *	@crc: previous CRC value
- *	@buffer: data pointer
- *	@len: number of bytes in the buffer
- */
-inline uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
-{
-	while (len--)
-		crc = crc_ccitt_byte(crc, *buffer++);
-	return crc;
-}
-
-
-#endif
+#include "unitree_leg_serial_driver/crc_ccitt.hpp"
+
+namespace crc_ccitt
+{
+
+// CRC-CCITT 预计算表
+const uint16_t 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,
+	0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
+	0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
+	0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+	0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
+	0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
+	0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+	0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
+	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
+	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
+	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
+	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
+	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
+	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
+	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
+	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
+	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
+	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
+	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
+	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
+	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
+	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
+	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
+};
+
+/**
+ * @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];
+ }
+
+/**
+ * @brief 计算数据缓冲区的 CRC-CCITT 值
+ * @param crc 初始 CRC 值
+ * @param buffer 数据缓冲区指针
+ * @param len 缓冲区长度
+ * @return 计算后的 CRC 值
+ */
+ uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len)
+ {
+	 while (len--)
+		 crc = crc_ccitt_byte(crc, *buffer++);
+	 return crc;
+ }
+ 
+
+} // namespace crc_ccitt

src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp

@@ -0,0 +1,226 @@
+#include "unitree_leg_serial_driver/unitree_leg_serial.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "rc_msgs/msg/go_motor_cmd.hpp"
+#include "rc_msgs/msg/go_motor_fdb.hpp"
+namespace unitree_leg_serial
+{
+
+UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options)
+    : Node("unitree_leg_serial", options)
+{
+    serial_port_name_ = "/dev/ttyACM0";
+    baud_rate_ = 4000000;
+
+    send_count_ = 0;
+    recv_count_ = 0;
+    last_freq_time_ = this->now();
+
+    // 发布器
+    motor_fdb_pub_ = this->create_publisher<rc_msgs::msg::GoMotorFdb>("motor_fdb", 10);
+    // 订阅器
+    motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::GoMotorCmd>(
+        "motor_cmd", 10,
+        std::bind(&UnitreeLegSerial::motor_cmd_callback, this, std::placeholders::_1));
+
+    if (!open_serial_port()) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_.c_str());
+        return;
+    }
+
+    running_ = true;
+    status_flag_ = OFFLINE;
+    tick_ = 0;
+    read_thread_ = std::thread(&UnitreeLegSerial::receive_data, this);
+
+    timer_ = this->create_wall_timer(
+        std::chrono::microseconds(500),
+        [this]() { motor_update(); });
+}
+
+UnitreeLegSerial::~UnitreeLegSerial()
+{
+    running_ = false;
+    if (read_thread_.joinable()) {
+        read_thread_.join();
+    }
+    close_serial_port();
+}
+
+void UnitreeLegSerial::motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg)
+{
+    // 填充motor_cmd_结构体
+    motor_cmd_.T = msg->torque_des;
+    motor_cmd_.W = msg->speed_des;
+    motor_cmd_.Pos = msg->pos_des;
+    motor_cmd_.K_P = msg->kp;
+    motor_cmd_.K_W = msg->kd;
+    status_flag_ = CONTROLED;
+    tick_ = 0;
+}
+
+bool UnitreeLegSerial::open_serial_port()
+{
+    try {
+        serial_port_ = std::make_unique<serial::Serial>(serial_port_name_, baud_rate_, serial::Timeout::simpleTimeout(1000));
+        return serial_port_->isOpen();
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(this->get_logger(), "Serial open exception: %s", e.what());
+        return false;
+    }
+}
+
+void UnitreeLegSerial::close_serial_port()
+{
+    if (serial_port_ && serial_port_->isOpen()) {
+        serial_port_->close();
+    }
+}
+
+void UnitreeLegSerial::motor_update()
+{
+    if (tick_ < 10) {
+        ++tick_;
+    } else {
+        status_flag_ = OFFLINE;
+    }
+
+    if (status_flag_ != CONTROLED) {
+        motor_cmd_reset();
+    }
+    send_motor_data(motor_cmd_);
+    send_count_++;
+
+    // 每秒打印一次频率
+    auto now = this->now();
+    if ((now - last_freq_time_).seconds() >= 1.0) {
+        RCLCPP_INFO(this->get_logger(), "发送频率: %d Hz, 接收频率: %d Hz", send_count_, recv_count_);
+        send_count_ = 0;
+        recv_count_ = 0;
+        last_freq_time_ = now;
+    }
+}
+
+void UnitreeLegSerial::motor_cmd_reset()
+{
+    motor_cmd_ = MotorCmd_t{};
+    motor_cmd_.id = 2;
+    motor_cmd_.mode = 1;
+}
+
+void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd)
+{
+    if (!serial_port_ || !serial_port_->isOpen()) {
+        RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Serial port is not open.");
+        return;
+    }
+
+    auto &out = motor_cmd_.motor_send_data;
+    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_->write(reinterpret_cast<const uint8_t *>(&out), sizeof(RIS_ControlData_t));
+}
+
+void UnitreeLegSerial::receive_data()
+{
+    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_->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;
+
+            std::memcpy(&motor_fbk_.motor_recv_data, buffer.data(), packet_size);
+
+            if (motor_fbk_.motor_recv_data.head[0] != 0xFD || motor_fbk_.motor_recv_data.head[1] != 0xEE) {
+                motor_fbk_.correct = 0;
+            } else {
+                motor_fbk_.calc_crc = crc_ccitt::crc_ccitt(
+                    0, (uint8_t *)&motor_fbk_.motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_fbk_.motor_recv_data.CRC16));
+                if (motor_fbk_.motor_recv_data.CRC16 != motor_fbk_.calc_crc) {
+                    memset(&motor_fbk_.motor_recv_data, 0, sizeof(RIS_MotorData_t));
+                    motor_fbk_.correct = 0;
+                    motor_fbk_.bad_msg++;
+                } else {
+                    motor_fbk_.motor_id = motor_fbk_.motor_recv_data.mode.id;
+                    motor_fbk_.mode = motor_fbk_.motor_recv_data.mode.status;
+                    motor_fbk_.Temp = motor_fbk_.motor_recv_data.fbk.temp;
+                    motor_fbk_.MError = motor_fbk_.motor_recv_data.fbk.MError;
+                    motor_fbk_.W = ((float)motor_fbk_.motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
+                    motor_fbk_.T = ((float)motor_fbk_.motor_recv_data.fbk.torque) / 256.0f;
+                    motor_fbk_.Pos = 6.28318f * ((float)motor_fbk_.motor_recv_data.fbk.pos) / 32768.0f;
+                    motor_fbk_.footForce = motor_fbk_.motor_recv_data.fbk.force;
+                    motor_fbk_.correct = 1;
+                }
+            }
+            if (motor_fbk_.correct) {
+                // 发布反馈消息
+                rc_msgs::msg::GoMotorFdb msg;
+                msg.torque = motor_fbk_.T;
+                msg.speed = motor_fbk_.W;
+                msg.pos = motor_fbk_.Pos;
+                motor_fdb_pub_->publish(msg);
+                recv_count_++;
+                RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Motor ID: %d, Position: %f", motor_fbk_.motor_id, motor_fbk_.Pos);
+            }
+            std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size);
+            buffer_offset -= packet_size;
+        } catch (const std::exception &e) {
+            RCLCPP_ERROR(this->get_logger(), "Serial read exception: %s", e.what());
+            reopen_port();
+        }
+    }
+}
+
+void UnitreeLegSerial::reopen_port()
+{
+    close_serial_port();
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    open_serial_port();
+}
+
+} // 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

@@ -3,13 +3,14 @@ project(rc_msgs)
 
 find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/DataMCU.msg"
+        "msg/DataRef.msg"
+        "msg/DataAI.msg"
         "msg/Ps2Data.msg"
-        "msg/DataControl.msg"
-        "msg/DataMotor.msg"
-        "msg/DataLeg.msg"
-        "msg/DataMotors.msg"
-        "msg/MotorCmd.msg"
-        "msg/MotorCmds.msg"
+        "msg/GoalPose.msg"
+        "msg/DataNav.msg"
+        "msg/GoMotorCmd.msg"
+        "msg/GoMotorFdb.msg"
 )
 
 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/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/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_bringup/package.xml

@@ -1,20 +0,0 @@
-<?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>robot_bringup</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>launch</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

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

src/robot_descriptions/anybotics/anymal_c_description/launch/unitree_guide.launch.py

@@ -0,0 +1,112 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+package_description = "anymal_c_description"
+
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[unitree_guide_controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='a1',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])

src/robot_descriptions/anybotics/anymal_c_description/launch/visualize.launch.py

@@ -0,0 +1,49 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import xacro
+
+package_description = "anymal_c_description"
+
+def process_xacro():
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file)
+    return robot_description_config.toxml()
+def generate_launch_description():
+    
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+    robot_description = process_xacro()
+
+    return LaunchDescription([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        ),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[
+                {
+                    'publish_frequency': 100.0,
+                    'use_tf_static': True,
+                    'robot_description': robot_description
+                }
+            ],
+        ),
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher',
+            output='screen',
+        )
+    ])

src/robot_descriptions/anybotics/anymal_c_description/meshes/battery.dae

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3</authoring_tool>
+    </contributor>
+    <created>2020-01-07T14:47:32</created>
+    <modified>2020-01-07T14:47:32</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="battery-effect">
+      <profile_COMMON>
+        <newparam sid="battery-surface">
+          <surface type="2D">
+            <init_from>battery</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="battery-sampler">
+          <sampler2D>
+            <source>battery-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <texture texture="battery-sampler" texcoord="UVMap"/>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images>
+    <image id="battery" name="battery">
+      <init_from>battery.jpg</init_from>
+    </image>
+  </library_images>
+  <library_materials>
+    <material id="battery-material" name="battery">
+      <instance_effect url="#battery-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube-mesh" name="Cube">
+      <mesh>
+        <source id="Cube-mesh-positions">
+          <float_array id="Cube-mesh-positions-array" count="24">-0.225881 -0.06249898 -0.03684729 -0.225881 -0.06249898 0.03725165 -0.225881 0.06249898 -0.03684729 -0.225881 0.06249898 0.03725165 0.232213 -0.06249898 -0.03684729 0.232213 -0.06249898 0.03725165 0.232213 0.06249898 -0.03684729 0.232213 0.06249898 0.03725165</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-normals">
+          <float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-map">
+          <float_array id="Cube-mesh-map-array" count="72">0.8431081 0.2728654 1 0 1 0.2728654 0.6862162 1 0.8431079 0 0.8431081 1 0.8431081 0.5457308 1 0.2728654 1 0.5457308 0.6862159 0 0.5293241 1 0.5293239 0 0.264662 0 0.5293239 0.9999999 0.2646622 0.9999999 0.264662 0.9999999 0 0 0.2646618 0 0.8431081 0.2728654 0.8431081 0 1 0 0.6862162 1 0.6862161 0 0.8431079 0 0.8431081 0.5457308 0.8431081 0.2728654 1 0.2728654 0.6862159 0 0.6862161 1 0.5293241 1 0.264662 0 0.5293238 0 0.5293239 0.9999999 0.264662 0.9999999 1.73529e-7 0.9999999 0 0</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-map-array" count="36" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube-mesh-vertices">
+          <input semantic="POSITION" source="#Cube-mesh-positions"/>
+        </vertices>
+        <triangles material="battery-material" count="12">
+          <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#Cube-mesh-map" offset="2" set="0"/>
+          <p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="AM3_Battery_LP" name="AM3 Battery LP" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 -0.03329167 0 0 0 1</matrix>
+        <instance_geometry url="#Cube-mesh" name="AM3 Battery LP">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="battery-material" target="#battery-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

src/robot_descriptions/anybotics/anymal_c_description/meshes/depth_camera.dae

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.82.7 commit date:2020-02-12, commit time:16:20, hash:77d23b0bd76f</authoring_tool>
+    </contributor>
+    <created>2020-02-20T10:55:16</created>
+    <modified>2020-02-20T10:55:16</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="depth_camera-effect">
+      <profile_COMMON>
+        <newparam sid="depth_camera-surface">
+          <surface type="2D">
+            <init_from>depth_camera</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="depth_camera-sampler">
+          <sampler2D>
+            <source>depth_camera-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <texture texture="depth_camera-sampler" texcoord="UVMap"/>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images>
+    <image id="depth_camera" name="depth_camera">
+      <init_from>depth_camera.jpg</init_from>
+    </image>
+  </library_images>
+  <library_materials>
+    <material id="depth_camera-material" name="depth_camera">
+      <instance_effect url="#depth_camera-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube-mesh" name="Cube">
+      <mesh>
+        <source id="Cube-mesh-positions">
+          <float_array id="Cube-mesh-positions-array" count="24">0 -0.04699999 -0.01968461 0 -0.04699999 0.01951932 0 0.04699999 -0.01968461 0 0.04699999 0.01951932 0.03039997 -0.04699999 -0.01968461 0.03039997 -0.04699999 0.01951932 0.03039997 0.04699999 -0.01968461 0.03039997 0.04699999 0.01951932</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-normals">
+          <float_array id="Cube-mesh-normals-array" count="18">-1 0 0 1 0 0 0 0 1 0 -1 0 0 1 0 0 0 -1</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-map">
+          <float_array id="Cube-mesh-map-array" count="72">0.2816218 0.7056847 0.5632433 0 0.5632433 0.7056847 1.60564e-7 0.7056847 0.2816215 0 0.2816217 0.7056847 0.7816217 0.7056846 1 0 1 0.7056846 0.5632433 0.7056847 0.344865 1 0.344865 0.7056847 0.7816216 0.7056847 0.5632434 1 0.5632433 0.7056847 0.7816217 0 0.5632433 0.7056846 0.5632434 0 0.2816218 0.7056847 0.2816217 0 0.5632433 0 1.60564e-7 0.7056847 0 0 0.2816215 0 0.7816217 0.7056846 0.7816217 0 1 0 0.5632433 0.7056847 0.5632433 1 0.344865 1 0.7816216 0.7056847 0.7816218 1 0.5632434 1 0.7816217 0 0.7816216 0.7056846 0.5632433 0.7056846</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-map-array" count="36" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube-mesh-vertices">
+          <input semantic="POSITION" source="#Cube-mesh-positions"/>
+        </vertices>
+        <triangles material="depth_camera-material" count="12">
+          <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#Cube-mesh-map" offset="2" set="0"/>
+          <p>1 0 0 2 0 1 0 0 2 7 1 3 4 1 4 6 1 5 3 2 6 5 2 7 7 2 8 1 3 9 4 3 10 5 3 11 7 4 12 2 4 13 3 4 14 6 5 15 0 5 16 2 5 17 1 0 18 3 0 19 2 0 20 7 1 21 5 1 22 4 1 23 3 2 24 1 2 25 5 2 26 1 3 27 0 3 28 4 3 29 7 4 30 6 4 31 2 4 32 6 5 33 4 5 34 0 5 35</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Realsense_LP" name="Realsense LP" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+        <instance_geometry url="#Cube-mesh" name="Realsense LP">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="depth_camera-material" target="#depth_camera-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

src/robot_descriptions/anybotics/anymal_c_description/meshes/hatch.dae

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3</authoring_tool>
+    </contributor>
+    <created>2020-01-13T19:28:23</created>
+    <modified>2020-01-13T19:28:23</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="hatch-effect">
+      <profile_COMMON>
+        <newparam sid="hatch-surface">
+          <surface type="2D">
+            <init_from>hatch</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="hatch-sampler">
+          <sampler2D>
+            <source>hatch-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <texture texture="hatch-sampler" texcoord="UVMap"/>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images>
+    <image id="hatch" name="hatch">
+      <init_from>hatch.jpg</init_from>
+    </image>
+  </library_images>
+  <library_materials>
+    <material id="hatch-material" name="hatch">
+      <instance_effect url="#hatch-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="AM3_Application_Hatch_Detailed_HD_002-mesh" name="AM3 Application Hatch Detailed HD.002">
+      <mesh>
+        <source id="AM3_Application_Hatch_Detailed_HD_002-mesh-positions">
+          <float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-positions-array" count="96">-76.90127 -64.71254 0 -76.90127 -64.71254 5.500071 -76.90127 64.71254 0 -76.90127 64.71254 5.500071 76.90133 -64.71254 0 76.90133 -64.71254 5.500071 76.90133 64.71254 0 76.90133 64.71254 5.500071 -80.46286 11.54165 0 -91.00833 3.847216 0 -91.00833 -3.847217 0 -80.46286 -11.54165 0 -80.46286 -11.54165 5.500071 -91.00833 -3.847217 5.500071 -91.00833 3.847217 5.500071 -80.46286 11.54165 5.500071 80.46292 -11.54165 0 91.00839 -3.847217 0 91.00839 3.847217 0 80.46292 11.54165 0 80.46292 11.54165 5.500071 91.00839 3.847216 5.500071 91.00839 -3.847217 5.500071 80.46292 -11.54165 5.500071 -80.46286 38.12709 5.500071 80.46292 38.12709 0 -80.46286 38.12709 0 80.46292 38.12709 5.500071 -80.46286 -38.12709 0 80.46292 -38.12709 5.500071 -80.46286 -38.12709 5.500071 80.46292 -38.12709 0</float_array>
+          <technique_common>
+            <accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-positions-array" count="32" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="AM3_Application_Hatch_Detailed_HD_002-mesh-normals">
+          <float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-normals-array" count="63">-0.9911455 0.1327809 0 0 1 0 0.9911455 -0.1327809 0 0 -1 0 0 0 1 0 0 1 1 0 0 0.5894234 0.8078244 0 0.5894234 -0.8078244 0 -1 0 0 -0.5894234 -0.8078244 0 -0.5894234 0.8078243 -2.12521e-7 0.9911454 0.1327813 0 -0.9911454 -0.1327813 0 -0.9911454 0.1327813 0 0.9911454 -0.1327813 0 0 0 1 0.5894235 0.8078243 2.12521e-7 -0.5894234 0.8078244 0 0.9911455 0.1327809 0 -0.9911455 -0.1327809 0</float_array>
+          <technique_common>
+            <accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-normals-array" count="21" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="AM3_Application_Hatch_Detailed_HD_002-mesh-map">
+          <float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-map-array" count="276">0.8547119 0.7845032 0.8910338 0.6826047 0.8910338 0.7845032 0.8547118 0.6826048 0.8910338 0 0.8910338 0.6826047 0.963678 0.5650011 0.927356 0.7110616 0.927356 0.5650011 0.891034 0.7845032 0.927356 0.1018984 0.927356 0.7845032 0.1755681 0.9420632 0 0.07750421 0.1755678 0.05793678 0.679144 0.9420632 0.5035759 0.05793678 0.6791438 0.05793678 0.503576 0.9420632 0.4527625 0 0.5035759 0.05793678 0.4527627 1 0.4019491 0 0.4527625 0 0.4019494 1 0.3511358 0.05793678 0.4019491 0 0.963678 0.1460605 0.927356 0.2921209 0.927356 0.1460605 0.963678 0.2921209 0.927356 0.3343942 0.927356 0.2921209 0.963678 0.3343942 0.927356 0.3766674 0.927356 0.3343942 0.963678 0.3766674 0.927356 0.4189407 0.927356 0.3766674 0.927356 0.7971531 0.963678 0.7110617 0.963678 0.7971532 0.963678 0.07171952 1 0 1 0.07171952 0.8547119 0.9672312 0.8910339 0.9423143 0.8910339 0.9672312 0.8547119 0.9423143 0.8910339 0.8705947 0.8910339 0.9423143 0.963678 0 0.927356 0.1460605 0.9273561 0 0.8547118 0.9224958 0.6791438 0.05793678 0.8547117 0.07750409 0.8547119 0.8705947 0.8910338 0.7845032 0.8910339 0.8705947 0.891034 0.1018984 0.927356 0 0.927356 0.1018984 0.351136 0.9420632 0.1755678 0.05793678 0.3511358 0.05793678 0.963678 0.4189407 0.927356 0.5650011 0.927356 0.4189407 0.8547119 0.7845032 0.8547118 0.6826048 0.8910338 0.6826047 0.8547118 0.6826048 0.8547118 0 0.8910338 0 0.963678 0.5650011 0.963678 0.7110616 0.927356 0.7110616 0.891034 0.7845032 0.891034 0.1018984 0.927356 0.1018984 0.1755681 0.9420632 0 0.9224959 0 0.07750421 0.679144 0.9420632 0.503576 0.9420632 0.5035759 0.05793678 0.503576 0.9420632 0.4527627 1 0.4527625 0 0.4527627 1 0.4019494 1 0.4019491 0 0.4019494 1 0.351136 0.9420632 0.3511358 0.05793678 0.963678 0.1460605 0.963678 0.2921209 0.927356 0.2921209 0.963678 0.2921209 0.963678 0.3343942 0.927356 0.3343942 0.963678 0.3343942 0.963678 0.3766674 0.927356 0.3766674 0.963678 0.3766674 0.963678 0.4189407 0.927356 0.4189407 0.927356 0.7971531 0.927356 0.7110616 0.963678 0.7110617 0.963678 0.07171952 0.963678 0 1 0 0.8547119 0.9672312 0.8547119 0.9423143 0.8910339 0.9423143 0.8547119 0.9423143 0.8547119 0.8705947 0.8910339 0.8705947 0.963678 0 0.963678 0.1460605 0.927356 0.1460605 0.8547118 0.9224958 0.679144 0.9420632 0.6791438 0.05793678 0.8547119 0.8705947 0.8547119 0.7845032 0.8910338 0.7845032 0.891034 0.1018984 0.891034 0 0.927356 0 0.351136 0.9420632 0.1755681 0.9420632 0.1755678 0.05793678 0.963678 0.4189407 0.963678 0.5650011 0.927356 0.5650011</float_array>
+          <technique_common>
+            <accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-map-array" count="138" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="AM3_Application_Hatch_Detailed_HD_002-mesh-vertices">
+          <input semantic="POSITION" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-positions"/>
+        </vertices>
+        <triangles material="hatch-material" count="46">
+          <input semantic="VERTEX" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-map" offset="2" set="0"/>
+          <p>24 0 0 2 0 1 26 0 2 3 1 3 6 1 4 2 1 5 29 2 6 4 2 7 31 2 8 5 3 9 0 3 10 4 3 11 30 4 12 5 4 13 29 4 14 24 4 15 20 4 16 27 4 17 15 4 18 21 4 19 20 4 20 14 5 21 22 5 22 21 5 23 13 4 24 23 4 25 22 4 26 27 6 27 19 6 28 25 6 29 20 7 30 18 7 31 19 7 32 21 6 33 17 6 34 18 6 35 22 8 36 16 8 37 17 8 38 30 9 39 11 9 40 28 9 41 12 10 42 10 10 43 11 10 44 13 9 45 9 9 46 10 9 47 14 11 48 8 11 49 9 11 50 7 12 51 25 12 52 6 12 53 3 4 54 27 4 55 7 4 56 15 9 57 26 9 58 8 9 59 1 13 60 28 13 61 0 13 62 12 4 63 29 4 64 23 4 65 23 6 66 31 6 67 16 6 68 24 14 69 3 14 70 2 14 71 3 1 72 7 1 73 6 1 74 29 15 75 5 15 76 4 15 77 5 3 78 1 3 79 0 3 80 30 4 81 1 4 82 5 4 83 24 4 84 15 4 85 20 4 86 15 16 87 14 16 88 21 16 89 14 4 90 13 4 91 22 4 92 13 4 93 12 4 94 23 4 95 27 6 96 20 6 97 19 6 98 20 17 99 21 17 100 18 17 101 21 6 102 22 6 103 17 6 104 22 8 105 23 8 106 16 8 107 30 9 108 12 9 109 11 9 110 12 10 111 13 10 112 10 10 113 13 9 114 14 9 115 9 9 116 14 18 117 15 18 118 8 18 119 7 19 120 27 19 121 25 19 122 3 4 123 24 4 124 27 4 125 15 9 126 24 9 127 26 9 128 1 20 129 30 20 130 28 20 131 12 4 132 30 4 133 29 4 134 23 6 135 29 6 136 31 6 137</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="AM3_Application_Hatch_LP" name="AM3 Application Hatch LP" type="NODE">
+        <matrix sid="transform">0.001006675 0 0 0 0 9.99987e-4 0 0 0 0 9.99987e-4 0 0 0 0 1</matrix>
+        <instance_geometry url="#AM3_Application_Hatch_Detailed_HD_002-mesh" name="AM3 Application Hatch LP">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="hatch-material" target="#hatch-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>