feat: 初始化 Go2 导航代码与配置

unitree_ros2/cyclonedds_ws/src/cyclonedds.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS>
+    <Domain Id="any">
+        <General>
+            <Interfaces>
+                <NetworkInterface name="enp2s0" priority="default" multicast="default" />
+            </Interfaces>
+        </General>
+    </Domain>
+</CycloneDDS>

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(unitree_api)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rosidl_generator_dds_idl REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/Request.msg"
+        "msg/RequestHeader.msg"
+        "msg/RequestIdentity.msg"
+        "msg/RequestLease.msg"
+        "msg/RequestPolicy.msg"
+        "msg/Response.msg"
+        "msg/ResponseHeader.msg"
+        "msg/ResponseStatus.msg"
+
+  DEPENDENCIES geometry_msgs
+)
+
+rosidl_generate_dds_interfaces(
+  ${rosidl_generate_interfaces_TARGET}__dds_connext_idl
+  IDL_TUPLES ${rosidl_generate_interfaces_IDL_TUPLES}
+  OUTPUT_SUBFOLDERS "dds_connext"
+)
+add_dependencies(
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}__dds_connext_idl
+)
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/Request.msg

@@ -0,0 +1,3 @@
+RequestHeader header
+string parameter
+uint8[] binary

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/RequestHeader.msg

@@ -0,0 +1,3 @@
+RequestIdentity identity
+RequestLease lease
+RequestPolicy policy

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/RequestIdentity.msg

@@ -0,0 +1,2 @@
+int64  id
+int64 api_id

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/RequestLease.msg

@@ -0,0 +1 @@
+int64 id

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/RequestPolicy.msg

@@ -0,0 +1,2 @@
+int32 priority
+bool noreply

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/Response.msg

@@ -0,0 +1,3 @@
+ResponseHeader header
+string data
+int8[] binary

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/ResponseHeader.msg

@@ -0,0 +1,2 @@
+RequestIdentity identity
+ResponseStatus status

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/msg/ResponseStatus.msg

@@ -0,0 +1 @@
+int32 code

unitree_ros2/cyclonedds_ws/src/unitree/unitree_api/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href=""?>
+<package format="3">
+  <name>unitree_api</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="unitree@unitree.com">Unitree</maintainer>
+<license>BSD 3-Clause License</license>
+  
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/CMakeLists.txt

@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.5)
+project(unitree_go)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rosidl_generator_dds_idl REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/AudioData.msg"
+        "msg/BmsCmd.msg"
+        "msg/BmsState.msg"
+        "msg/Error.msg"
+        "msg/Go2FrontVideoData.msg"
+        "msg/HeightMap.msg"
+        "msg/IMUState.msg"
+        "msg/InterfaceConfig.msg"
+        "msg/LidarState.msg"
+        "msg/LowCmd.msg"
+        "msg/LowState.msg"
+        "msg/MotorCmd.msg"
+        "msg/MotorCmds.msg"
+        "msg/MotorState.msg"
+        "msg/MotorStates.msg"
+        "msg/PathPoint.msg"
+        "msg/Req.msg"
+        "msg/Res.msg"
+        "msg/SportModeCmd.msg"
+        "msg/SportModeState.msg"
+        "msg/TimeSpec.msg"
+        "msg/UwbState.msg"
+        "msg/UwbSwitch.msg"
+        "msg/WirelessController.msg"
+  DEPENDENCIES geometry_msgs
+)
+
+rosidl_generate_dds_interfaces(
+  ${rosidl_generate_interfaces_TARGET}__dds_connext_idl
+  IDL_TUPLES ${rosidl_generate_interfaces_IDL_TUPLES}
+  OUTPUT_SUBFOLDERS "dds_connext"
+)
+add_dependencies(
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}__dds_connext_idl
+)
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/AudioData.msg

@@ -0,0 +1,2 @@
+uint64 time_frame
+uint8[] data

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/BmsCmd.msg

@@ -0,0 +1,2 @@
+uint8 off
+uint8[3] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/BmsState.msg

@@ -0,0 +1,9 @@
+uint8 version_high
+uint8 version_low
+uint8 status
+uint8 soc
+int32 current
+uint16 cycle
+int8[2] bq_ntc
+int8[2] mcu_ntc
+uint16[15] cell_vol

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/Error.msg

@@ -0,0 +1,2 @@
+uint32 source
+uint32 state

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/Go2FrontVideoData.msg

@@ -0,0 +1,4 @@
+uint64 time_frame
+uint8[] video720p
+uint8[] video360p
+uint8[] video180p 

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/HeightMap.msg

@@ -0,0 +1,15 @@
+# Header
+float64 stamp         # timestamp
+string frame_id      # world frame id
+
+# Map info
+float32 resolution     # The map resolution [m/cell]
+uint32 width  # Map width along x-axis [cells]
+uint32 height # Map height alonge y-axis [cells]
+float32[2] origin      # Map frame origin xy-position [m], the xyz-axis direction of map frame is aligned with the world frame
+
+# Map data, in x-major order, starting with [0,0], ending with [width, height]
+# For a cell whose 2d-array-index is [ix, iy]，
+#    its position in world frame is: [ix * resolution + origin[0], iy * resolution + origin[1]]
+#    its cell value is: data[width * iy + ix]
+float32[] data

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/IMUState.msg

@@ -0,0 +1,5 @@
+float32[4] quaternion
+float32[3] gyroscope
+float32[3] accelerometer
+float32[3] rpy
+int8 temperature

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/InterfaceConfig.msg

@@ -0,0 +1,3 @@
+uint8 mode
+uint8 value
+uint8[2] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/LidarState.msg

@@ -0,0 +1,17 @@
+float64 stamp
+string firmware_version
+string software_version
+string sdk_version
+float32 sys_rotation_speed
+float32 com_rotation_speed
+uint8 error_state
+float32 cloud_frequency
+float32 cloud_packet_loss_rate
+uint32 cloud_size
+uint32 cloud_scan_num
+float32 imu_frequency
+float32 imu_packet_loss_rate
+float32[3] imu_rpy
+float64 serial_recv_stamp
+uint32 serial_buffer_size
+uint32 serial_buffer_read

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/LowCmd.msg

@@ -0,0 +1,15 @@
+uint8[2] head
+uint8 level_flag
+uint8 frame_reserve
+uint32[2] sn
+uint32[2] version
+uint16 bandwidth
+MotorCmd[20] motor_cmd
+BmsCmd bms_cmd
+uint8[40] wireless_remote
+uint8[12] led
+uint8[2] fan
+uint8 gpio
+uint32 reserve
+uint32 crc
+

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/LowState.msg

@@ -0,0 +1,22 @@
+uint8[2] head
+uint8 level_flag
+uint8 frame_reserve
+uint32[2] sn
+uint32[2] version
+uint16 bandwidth
+IMUState imu_state
+MotorState[20] motor_state
+BmsState bms_state
+int16[4] foot_force
+int16[4] foot_force_est
+uint32 tick
+uint8[40] wireless_remote
+uint8 bit_flag
+float32 adc_reel
+int8 temperature_ntc1
+int8 temperature_ntc2
+float32 power_v
+float32 power_a
+uint16[4] fan_frequency
+uint32 reserve
+uint32 crc

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/MotorCmd.msg

@@ -0,0 +1,7 @@
+uint8 mode
+float32 q
+float32 dq
+float32 tau
+float32 kp
+float32 kd
+uint32[3] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/MotorCmds.msg

@@ -0,0 +1 @@
+MotorCmd[] cmds

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/MotorState.msg

@@ -0,0 +1,11 @@
+uint8 mode
+float32 q
+float32 dq
+float32 ddq
+float32 tau_est
+float32 q_raw
+float32 dq_raw
+float32 ddq_raw
+int8 temperature
+uint32 lost
+uint32[2] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/MotorStates.msg

@@ -0,0 +1 @@
+MotorState[] states

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/PathPoint.msg

@@ -0,0 +1,7 @@
+float32 t_from_start
+float32 x
+float32 y
+float32 yaw
+float32 vx
+float32 vy
+float32 vyaw

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/Req.msg

@@ -0,0 +1,2 @@
+string uuid
+string body

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/Res.msg

@@ -0,0 +1,3 @@
+string uuid
+uint8[] data
+string body

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/SportModeCmd.msg

@@ -0,0 +1,13 @@
+uint8 mode
+uint8 gait_type
+uint8 speed_level
+float32 foot_raise_height
+float32 body_height
+float32[2] position
+float32[3] euler
+float32[2] velocity
+float32 yaw_speed
+BmsCmd bms_cmd
+PathPoint[30] path_point
+
+

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/SportModeState.msg

@@ -0,0 +1,17 @@
+TimeSpec stamp
+uint32 error_code
+IMUState imu_state
+uint8 mode
+float32 progress
+uint8 gait_type
+float32 foot_raise_height
+float32[3] position
+float32 body_height
+float32[3] velocity
+float32 yaw_speed
+float32[4] range_obstacle
+int16[4] foot_force
+float32[12] foot_position_body
+float32[12] foot_speed_body
+
+

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/TimeSpec.msg

@@ -0,0 +1,5 @@
+# Time indicates a specific point in time, relative to a clock's 0 point.
+# The seconds component, valid over all int32 values.
+int32 sec
+# The nanoseconds component, valid in the range [0, 10e9).
+uint32 nanosec

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/UwbState.msg

@@ -0,0 +1,19 @@
+uint8[2] version
+uint8 channel
+uint8 joy_mode
+float32 orientation_est
+float32 pitch_est
+float32 distance_est
+float32 yaw_est
+float32 tag_roll
+float32 tag_pitch
+float32 tag_yaw
+float32 base_roll
+float32 base_pitch
+float32 base_yaw
+float32[2] joystick
+uint8 error_state
+uint8 buttons
+uint8 enabled_from_app
+
+

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/UwbSwitch.msg

@@ -0,0 +1 @@
+uint8 enabled

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg/WirelessController.msg

@@ -0,0 +1,5 @@
+float32 lx
+float32 ly
+float32 rx
+float32 ry
+uint16 keys

unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href=""?>
+<package format="3">
+  <name>unitree_go</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="unitree@unitree.com">Unitree</maintainer>
+<license>BSD 3-Clause License</license>
+  
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.5)
+project(unitree_hg)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rosidl_generator_dds_idl REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+	"msg/BmsCmd.msg"
+	"msg/BmsState.msg"
+	"msg/HandCmd.msg"
+	"msg/HandState.msg"
+	"msg/IMUState.msg"
+	"msg/LowCmd.msg"
+	"msg/LowState.msg"
+	"msg/MainBoardState.msg"
+	"msg/MotorCmd.msg"
+	"msg/MotorState.msg"
+	"msg/PressSensorState.msg"
+  DEPENDENCIES geometry_msgs
+)
+
+
+rosidl_generate_dds_interfaces(
+  ${rosidl_generate_interfaces_TARGET}__dds_connext_idl
+  IDL_TUPLES ${rosidl_generate_interfaces_IDL_TUPLES}
+  OUTPUT_SUBFOLDERS "dds_connext"
+)
+add_dependencies(
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}__dds_connext_idl
+)
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/BmsCmd.msg

@@ -0,0 +1,2 @@
+uint8 cmd
+uint8[40] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/BmsState.msg

@@ -0,0 +1,13 @@
+uint8 version_high
+uint8 version_low
+uint8 fn
+uint16[40] cell_vol
+uint32[3] bmsvoltage
+int32 current
+uint8 soc
+uint8 soh
+int16[12] temperature
+uint16 cycle
+uint16 manufacturer_date
+uint32[5] bmsstate
+uint32[3] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/HandCmd.msg

@@ -0,0 +1,2 @@
+MotorCmd[] motor_cmd
+uint32[4] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/HandState.msg

@@ -0,0 +1,10 @@
+MotorState[] motor_state
+PressSensorState[] press_sensor_state
+IMUState imu_state
+
+float32 power_v
+float32 power_a
+float32 system_v
+float32 device_v
+uint32[2] error
+uint32[2] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/IMUState.msg

@@ -0,0 +1,5 @@
+float32[4] quaternion
+float32[3] gyroscope
+float32[3] accelerometer
+float32[3] rpy
+int16 temperature

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/LowCmd.msg

@@ -0,0 +1,7 @@
+uint8 mode_pr
+uint8 mode_machine
+
+MotorCmd[35] motor_cmd
+
+uint32[4] reserve
+uint32 crc

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/LowState.msg

@@ -0,0 +1,9 @@
+uint32[2] version
+uint8 mode_pr
+uint8 mode_machine
+uint32 tick
+IMUState imu_state
+MotorState[35] motor_state
+uint8[40] wireless_remote
+uint32[4] reserve
+uint32 crc

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/MainBoardState.msg

@@ -0,0 +1,4 @@
+uint16[6] fan_state
+int16[6] temperature
+float32[6] value
+uint32[6] state

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/MotorCmd.msg

@@ -0,0 +1,7 @@
+uint8 mode
+float32 q
+float32 dq
+float32 tau
+float32 kp
+float32 kd
+uint32 reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/MotorState.msg

@@ -0,0 +1,11 @@
+uint8 mode
+float32 q
+float32 dq
+float32 ddq
+float32 tau_est
+
+int16[2] temperature
+float32 vol
+uint32[2] sensor
+uint32 motorstate
+uint32[4] reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg/PressSensorState.msg

@@ -0,0 +1,4 @@
+float32[12] pressure
+float32[12] temperature
+uint32 lost
+uint32 reserve

unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href=""?>
+<package format="3">
+  <name>unitree_hg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="unitree@unitree.com">Unitree</maintainer>
+<license>BSD 3-Clause License</license>
+  
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>