From c7dc5f90cf2de3925f6d8cc25382efbeceb6f61a Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 26 Jun 2024 15:49:50 +0200 Subject: [PATCH 01/34] Initialized packages Signed-off-by: Juancams --- go2_bringup/CMakeLists.txt | 17 +++++++++++++++++ go2_bringup/config/empty | 0 go2_bringup/launch/empty | 0 go2_bringup/package.xml | 18 ++++++++++++++++++ go2_driver/CMakeLists.txt | 17 +++++++++++++++++ go2_driver/include/empty | 0 go2_driver/package.xml | 18 ++++++++++++++++++ go2_driver/src/empty | 0 go2_hardware/CMakeLists.txt | 17 +++++++++++++++++ go2_hardware/include/empty | 0 go2_hardware/package.xml | 18 ++++++++++++++++++ go2_hardware/src/empty | 0 go2_simulation/CMakeLists.txt | 17 +++++++++++++++++ go2_simulation/launch/empty | 0 go2_simulation/package.xml | 18 ++++++++++++++++++ go2_simulation/robots/empty | 0 go2_simulation/worlds/empty | 0 17 files changed, 140 insertions(+) create mode 100644 go2_bringup/CMakeLists.txt create mode 100644 go2_bringup/config/empty create mode 100644 go2_bringup/launch/empty create mode 100644 go2_bringup/package.xml create mode 100644 go2_driver/CMakeLists.txt create mode 100644 go2_driver/include/empty create mode 100644 go2_driver/package.xml create mode 100644 go2_driver/src/empty create mode 100644 go2_hardware/CMakeLists.txt create mode 100644 go2_hardware/include/empty create mode 100644 go2_hardware/package.xml create mode 100644 go2_hardware/src/empty create mode 100644 go2_simulation/CMakeLists.txt create mode 100644 go2_simulation/launch/empty create mode 100644 go2_simulation/package.xml create mode 100644 go2_simulation/robots/empty create mode 100644 go2_simulation/worlds/empty diff --git a/go2_bringup/CMakeLists.txt b/go2_bringup/CMakeLists.txt new file mode 100644 index 0000000..541e22c --- /dev/null +++ b/go2_bringup/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_bringup/config/empty b/go2_bringup/config/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_bringup/launch/empty b/go2_bringup/launch/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_bringup/package.xml b/go2_bringup/package.xml new file mode 100644 index 0000000..683674c --- /dev/null +++ b/go2_bringup/package.xml @@ -0,0 +1,18 @@ + + + + go2_bringup + 0.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_driver/CMakeLists.txt b/go2_driver/CMakeLists.txt new file mode 100644 index 0000000..c4c0413 --- /dev/null +++ b/go2_driver/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_driver) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_driver/include/empty b/go2_driver/include/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_driver/package.xml b/go2_driver/package.xml new file mode 100644 index 0000000..f78ae3a --- /dev/null +++ b/go2_driver/package.xml @@ -0,0 +1,18 @@ + + + + go2_driver + 0.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_driver/src/empty b/go2_driver/src/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_hardware/CMakeLists.txt b/go2_hardware/CMakeLists.txt new file mode 100644 index 0000000..63c9d38 --- /dev/null +++ b/go2_hardware/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_hardware) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_hardware/include/empty b/go2_hardware/include/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_hardware/package.xml b/go2_hardware/package.xml new file mode 100644 index 0000000..fe6dc5c --- /dev/null +++ b/go2_hardware/package.xml @@ -0,0 +1,18 @@ + + + + go2_hardware + 0.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_hardware/src/empty b/go2_hardware/src/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_simulation/CMakeLists.txt b/go2_simulation/CMakeLists.txt new file mode 100644 index 0000000..5b8b301 --- /dev/null +++ b/go2_simulation/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_simulation) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_simulation/launch/empty b/go2_simulation/launch/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_simulation/package.xml b/go2_simulation/package.xml new file mode 100644 index 0000000..b8c584e --- /dev/null +++ b/go2_simulation/package.xml @@ -0,0 +1,18 @@ + + + + go2_simulation + 0.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_simulation/robots/empty b/go2_simulation/robots/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_simulation/worlds/empty b/go2_simulation/worlds/empty new file mode 100644 index 0000000..e69de29 From 8427a6559262cb5189044c3ac03b6bf26053b0ca Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 26 Jun 2024 16:04:32 +0200 Subject: [PATCH 02/34] Deleted worlds Signed-off-by: Juancams --- go2_simulation/worlds/empty | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 go2_simulation/worlds/empty diff --git a/go2_simulation/worlds/empty b/go2_simulation/worlds/empty deleted file mode 100644 index e69de29..0000000 From 2d45a802a47221a81d3ec190d89ef22b217142d2 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 26 Jun 2024 16:04:43 +0200 Subject: [PATCH 03/34] Flake8 errors solved Signed-off-by: Juancams --- go2_description/launch/robot.launch.py | 106 +++++++++++++------------ 1 file changed, 54 insertions(+), 52 deletions(-) diff --git a/go2_description/launch/robot.launch.py b/go2_description/launch/robot.launch.py index 51991fc..0a306f7 100644 --- a/go2_description/launch/robot.launch.py +++ b/go2_description/launch/robot.launch.py @@ -32,79 +32,81 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare import launch_ros.descriptions +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - declared_arguments = [] - nodes = [] + declared_arguments = [] + nodes = [] - declared_arguments.append( + declared_arguments.append( + DeclareLaunchArgument( + 'description_package', + default_value='go2_description', + description='Description package with robot URDF/xacro files made by manufacturer.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'description_file', + default_value='go2_description.urdf', + description='URDF/XACRO description file with the robot.', + ) + ) + declared_arguments.append( DeclareLaunchArgument( - "description_package", - default_value="go2_description", - description="Description package with robot URDF/xacro files made by manufacturer.", + 'prefix', + default_value='', + description='Prefix to be added to the robot description.', ) ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="go2_description.urdf", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value="", - description="Prefix to be added to the robot description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_sim_time", - default_value="True", - description="Use simulation/Gazebo clock if true", - ) - ) - - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - use_sim_time = LaunchConfiguration('use_sim_time') - - robot_description_content = Command( + declared_arguments.append( + DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation/Gazebo clock if true', + ) + ) + + description_file = LaunchConfiguration('description_file') + prefix = LaunchConfiguration('prefix') + use_sim_time = LaunchConfiguration('use_sim_time') + + robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution([FindPackageShare("go2_description"), "urdf", description_file]), - " ", - "prefix:=", prefix + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('go2_description'), 'urdf', description_file]), + ' ', + 'prefix:=', prefix ] ) - - robot_description_param = launch_ros.descriptions.ParameterValue(robot_description_content, value_type=str) - nodes.append(Node( + robot_description_param = launch_ros.descriptions.ParameterValue(robot_description_content, + value_type=str) + + nodes.append(Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', parameters=[{ - 'use_sim_time': use_sim_time, - 'robot_description': robot_description_param, - 'publish_frequency': 100.0, - 'frame_prefix': '', - }], + 'use_sim_time': use_sim_time, + 'robot_description': robot_description_param, + 'publish_frequency': 100.0, + 'frame_prefix': '', + }], + ) ) - ) - nodes.append(Node( + nodes.append(Node( package='rviz2', namespace='', executable='rviz2', name='rviz2', ) - ) - - return LaunchDescription(declared_arguments + nodes) + ) + + return LaunchDescription(declared_arguments + nodes) From e076f5b0df092175453a061deb1433c3fabeab6e Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 27 Jun 2024 15:42:05 +0200 Subject: [PATCH 04/34] Renamed Signed-off-by: Juancams --- go2_interfaces/CMakeLists.txt | 79 ----------------------- go2_interfaces/msg/AudioData.msg | 2 - go2_interfaces/msg/BmsCmd.msg | 2 - go2_interfaces/msg/BmsState.msg | 9 --- go2_interfaces/msg/Error.msg | 2 - go2_interfaces/msg/Go2FrontVideoData.msg | 4 -- go2_interfaces/msg/HeightMap.msg | 15 ----- go2_interfaces/msg/IMUState.msg | 5 -- go2_interfaces/msg/InterfaceConfig.msg | 3 - go2_interfaces/msg/LidarState.msg | 17 ----- go2_interfaces/msg/LowCmd.msg | 15 ----- go2_interfaces/msg/LowState.msg | 22 ------- go2_interfaces/msg/MotorCmd.msg | 7 -- go2_interfaces/msg/MotorCmds.msg | 1 - go2_interfaces/msg/MotorState.msg | 11 ---- go2_interfaces/msg/MotorStates.msg | 1 - go2_interfaces/msg/PathPoint.msg | 7 -- go2_interfaces/msg/Req.msg | 2 - go2_interfaces/msg/Res.msg | 3 - go2_interfaces/msg/SportModeCmd.msg | 13 ---- go2_interfaces/msg/SportModeState.msg | 17 ----- go2_interfaces/msg/TimeSpec.msg | 5 -- go2_interfaces/msg/UwbState.msg | 19 ------ go2_interfaces/msg/UwbSwitch.msg | 1 - go2_interfaces/msg/WirelessController.msg | 5 -- go2_interfaces/package.xml | 23 ------- 26 files changed, 290 deletions(-) delete mode 100755 go2_interfaces/CMakeLists.txt delete mode 100755 go2_interfaces/msg/AudioData.msg delete mode 100755 go2_interfaces/msg/BmsCmd.msg delete mode 100755 go2_interfaces/msg/BmsState.msg delete mode 100755 go2_interfaces/msg/Error.msg delete mode 100755 go2_interfaces/msg/Go2FrontVideoData.msg delete mode 100755 go2_interfaces/msg/HeightMap.msg delete mode 100755 go2_interfaces/msg/IMUState.msg delete mode 100755 go2_interfaces/msg/InterfaceConfig.msg delete mode 100755 go2_interfaces/msg/LidarState.msg delete mode 100755 go2_interfaces/msg/LowCmd.msg delete mode 100755 go2_interfaces/msg/LowState.msg delete mode 100755 go2_interfaces/msg/MotorCmd.msg delete mode 100644 go2_interfaces/msg/MotorCmds.msg delete mode 100755 go2_interfaces/msg/MotorState.msg delete mode 100644 go2_interfaces/msg/MotorStates.msg delete mode 100755 go2_interfaces/msg/PathPoint.msg delete mode 100755 go2_interfaces/msg/Req.msg delete mode 100755 go2_interfaces/msg/Res.msg delete mode 100755 go2_interfaces/msg/SportModeCmd.msg delete mode 100755 go2_interfaces/msg/SportModeState.msg delete mode 100755 go2_interfaces/msg/TimeSpec.msg delete mode 100755 go2_interfaces/msg/UwbState.msg delete mode 100755 go2_interfaces/msg/UwbSwitch.msg delete mode 100755 go2_interfaces/msg/WirelessController.msg delete mode 100755 go2_interfaces/package.xml diff --git a/go2_interfaces/CMakeLists.txt b/go2_interfaces/CMakeLists.txt deleted file mode 100755 index 7e22eb1..0000000 --- a/go2_interfaces/CMakeLists.txt +++ /dev/null @@ -1,79 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(go2_interfaces) - -# 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( 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() diff --git a/go2_interfaces/msg/AudioData.msg b/go2_interfaces/msg/AudioData.msg deleted file mode 100755 index 6e889bc..0000000 --- a/go2_interfaces/msg/AudioData.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint64 time_frame -uint8[] data \ No newline at end of file diff --git a/go2_interfaces/msg/BmsCmd.msg b/go2_interfaces/msg/BmsCmd.msg deleted file mode 100755 index bdbf9c3..0000000 --- a/go2_interfaces/msg/BmsCmd.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 off -uint8[3] reserve \ No newline at end of file diff --git a/go2_interfaces/msg/BmsState.msg b/go2_interfaces/msg/BmsState.msg deleted file mode 100755 index 052e4d5..0000000 --- a/go2_interfaces/msg/BmsState.msg +++ /dev/null @@ -1,9 +0,0 @@ -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 \ No newline at end of file diff --git a/go2_interfaces/msg/Error.msg b/go2_interfaces/msg/Error.msg deleted file mode 100755 index 4a5a631..0000000 --- a/go2_interfaces/msg/Error.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint32 source -uint32 state \ No newline at end of file diff --git a/go2_interfaces/msg/Go2FrontVideoData.msg b/go2_interfaces/msg/Go2FrontVideoData.msg deleted file mode 100755 index 3c294f3..0000000 --- a/go2_interfaces/msg/Go2FrontVideoData.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 time_frame -uint8[] video720p -uint8[] video360p -uint8[] video180p \ No newline at end of file diff --git a/go2_interfaces/msg/HeightMap.msg b/go2_interfaces/msg/HeightMap.msg deleted file mode 100755 index 4e6f3b2..0000000 --- a/go2_interfaces/msg/HeightMap.msg +++ /dev/null @@ -1,15 +0,0 @@ -# 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 \ No newline at end of file diff --git a/go2_interfaces/msg/IMUState.msg b/go2_interfaces/msg/IMUState.msg deleted file mode 100755 index e065d04..0000000 --- a/go2_interfaces/msg/IMUState.msg +++ /dev/null @@ -1,5 +0,0 @@ -float32[4] quaternion -float32[3] gyroscope -float32[3] accelerometer -float32[3] rpy -int8 temperature diff --git a/go2_interfaces/msg/InterfaceConfig.msg b/go2_interfaces/msg/InterfaceConfig.msg deleted file mode 100755 index c43bd20..0000000 --- a/go2_interfaces/msg/InterfaceConfig.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 mode -uint8 value -uint8[2] reserve \ No newline at end of file diff --git a/go2_interfaces/msg/LidarState.msg b/go2_interfaces/msg/LidarState.msg deleted file mode 100755 index d8b4eb2..0000000 --- a/go2_interfaces/msg/LidarState.msg +++ /dev/null @@ -1,17 +0,0 @@ -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 diff --git a/go2_interfaces/msg/LowCmd.msg b/go2_interfaces/msg/LowCmd.msg deleted file mode 100755 index 464a31d..0000000 --- a/go2_interfaces/msg/LowCmd.msg +++ /dev/null @@ -1,15 +0,0 @@ -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 - diff --git a/go2_interfaces/msg/LowState.msg b/go2_interfaces/msg/LowState.msg deleted file mode 100755 index ceaed32..0000000 --- a/go2_interfaces/msg/LowState.msg +++ /dev/null @@ -1,22 +0,0 @@ -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 \ No newline at end of file diff --git a/go2_interfaces/msg/MotorCmd.msg b/go2_interfaces/msg/MotorCmd.msg deleted file mode 100755 index 67cd1bf..0000000 --- a/go2_interfaces/msg/MotorCmd.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint8 mode -float32 q -float32 dq -float32 tau -float32 kp -float32 kd -uint32[3] reserve \ No newline at end of file diff --git a/go2_interfaces/msg/MotorCmds.msg b/go2_interfaces/msg/MotorCmds.msg deleted file mode 100644 index a1d4726..0000000 --- a/go2_interfaces/msg/MotorCmds.msg +++ /dev/null @@ -1 +0,0 @@ -MotorCmd[] cmds \ No newline at end of file diff --git a/go2_interfaces/msg/MotorState.msg b/go2_interfaces/msg/MotorState.msg deleted file mode 100755 index 2e1a739..0000000 --- a/go2_interfaces/msg/MotorState.msg +++ /dev/null @@ -1,11 +0,0 @@ -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 \ No newline at end of file diff --git a/go2_interfaces/msg/MotorStates.msg b/go2_interfaces/msg/MotorStates.msg deleted file mode 100644 index e4d9e7f..0000000 --- a/go2_interfaces/msg/MotorStates.msg +++ /dev/null @@ -1 +0,0 @@ -MotorState[] states \ No newline at end of file diff --git a/go2_interfaces/msg/PathPoint.msg b/go2_interfaces/msg/PathPoint.msg deleted file mode 100755 index 08b4945..0000000 --- a/go2_interfaces/msg/PathPoint.msg +++ /dev/null @@ -1,7 +0,0 @@ -float32 t_from_start -float32 x -float32 y -float32 yaw -float32 vx -float32 vy -float32 vyaw \ No newline at end of file diff --git a/go2_interfaces/msg/Req.msg b/go2_interfaces/msg/Req.msg deleted file mode 100755 index af14a7d..0000000 --- a/go2_interfaces/msg/Req.msg +++ /dev/null @@ -1,2 +0,0 @@ -string uuid -string body \ No newline at end of file diff --git a/go2_interfaces/msg/Res.msg b/go2_interfaces/msg/Res.msg deleted file mode 100755 index 8070258..0000000 --- a/go2_interfaces/msg/Res.msg +++ /dev/null @@ -1,3 +0,0 @@ -string uuid -uint8[] data -string body \ No newline at end of file diff --git a/go2_interfaces/msg/SportModeCmd.msg b/go2_interfaces/msg/SportModeCmd.msg deleted file mode 100755 index e5ccb46..0000000 --- a/go2_interfaces/msg/SportModeCmd.msg +++ /dev/null @@ -1,13 +0,0 @@ -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 - - diff --git a/go2_interfaces/msg/SportModeState.msg b/go2_interfaces/msg/SportModeState.msg deleted file mode 100755 index 7a6c1e6..0000000 --- a/go2_interfaces/msg/SportModeState.msg +++ /dev/null @@ -1,17 +0,0 @@ -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 - - diff --git a/go2_interfaces/msg/TimeSpec.msg b/go2_interfaces/msg/TimeSpec.msg deleted file mode 100755 index 188b3c1..0000000 --- a/go2_interfaces/msg/TimeSpec.msg +++ /dev/null @@ -1,5 +0,0 @@ -# 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 diff --git a/go2_interfaces/msg/UwbState.msg b/go2_interfaces/msg/UwbState.msg deleted file mode 100755 index ed1da3a..0000000 --- a/go2_interfaces/msg/UwbState.msg +++ /dev/null @@ -1,19 +0,0 @@ -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 - - diff --git a/go2_interfaces/msg/UwbSwitch.msg b/go2_interfaces/msg/UwbSwitch.msg deleted file mode 100755 index db8ec91..0000000 --- a/go2_interfaces/msg/UwbSwitch.msg +++ /dev/null @@ -1 +0,0 @@ -uint8 enabled \ No newline at end of file diff --git a/go2_interfaces/msg/WirelessController.msg b/go2_interfaces/msg/WirelessController.msg deleted file mode 100755 index 12965ff..0000000 --- a/go2_interfaces/msg/WirelessController.msg +++ /dev/null @@ -1,5 +0,0 @@ -float32 lx -float32 ly -float32 rx -float32 ry -uint16 keys \ No newline at end of file diff --git a/go2_interfaces/package.xml b/go2_interfaces/package.xml deleted file mode 100755 index 8c7bbcc..0000000 --- a/go2_interfaces/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - go2_interfaces - 0.0.1 - Communication interfaces of the unitree go 2 robots - Unitree - BSD 3-Clause License - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - ament_cmake - - geometry_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - From 928c8327e3ab0c98bcdd27de01225bb50151279e Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 27 Jun 2024 15:42:20 +0200 Subject: [PATCH 05/34] unitree_go/ Signed-off-by: Juancams --- unitree_go/CMakeLists.txt | 80 +++++++++++++++++++++++++++ unitree_go/msg/AudioData.msg | 2 + unitree_go/msg/BmsCmd.msg | 2 + unitree_go/msg/BmsState.msg | 9 +++ unitree_go/msg/Error.msg | 2 + unitree_go/msg/Go2FrontVideoData.msg | 4 ++ unitree_go/msg/Go2State.msg | 12 ++++ unitree_go/msg/HeightMap.msg | 15 +++++ unitree_go/msg/IMUState.msg | 5 ++ unitree_go/msg/InterfaceConfig.msg | 3 + unitree_go/msg/LidarState.msg | 17 ++++++ unitree_go/msg/LowCmd.msg | 15 +++++ unitree_go/msg/LowState.msg | 22 ++++++++ unitree_go/msg/MotorCmd.msg | 7 +++ unitree_go/msg/MotorCmds.msg | 1 + unitree_go/msg/MotorState.msg | 11 ++++ unitree_go/msg/MotorStates.msg | 1 + unitree_go/msg/PathPoint.msg | 7 +++ unitree_go/msg/Req.msg | 2 + unitree_go/msg/Res.msg | 3 + unitree_go/msg/SportModeCmd.msg | 13 +++++ unitree_go/msg/SportModeState.msg | 17 ++++++ unitree_go/msg/TimeSpec.msg | 5 ++ unitree_go/msg/UwbState.msg | 19 +++++++ unitree_go/msg/UwbSwitch.msg | 1 + unitree_go/msg/WirelessController.msg | 5 ++ unitree_go/package.xml | 23 ++++++++ 27 files changed, 303 insertions(+) create mode 100755 unitree_go/CMakeLists.txt create mode 100755 unitree_go/msg/AudioData.msg create mode 100755 unitree_go/msg/BmsCmd.msg create mode 100755 unitree_go/msg/BmsState.msg create mode 100755 unitree_go/msg/Error.msg create mode 100755 unitree_go/msg/Go2FrontVideoData.msg create mode 100644 unitree_go/msg/Go2State.msg create mode 100755 unitree_go/msg/HeightMap.msg create mode 100755 unitree_go/msg/IMUState.msg create mode 100755 unitree_go/msg/InterfaceConfig.msg create mode 100755 unitree_go/msg/LidarState.msg create mode 100755 unitree_go/msg/LowCmd.msg create mode 100755 unitree_go/msg/LowState.msg create mode 100755 unitree_go/msg/MotorCmd.msg create mode 100644 unitree_go/msg/MotorCmds.msg create mode 100755 unitree_go/msg/MotorState.msg create mode 100644 unitree_go/msg/MotorStates.msg create mode 100755 unitree_go/msg/PathPoint.msg create mode 100755 unitree_go/msg/Req.msg create mode 100755 unitree_go/msg/Res.msg create mode 100755 unitree_go/msg/SportModeCmd.msg create mode 100755 unitree_go/msg/SportModeState.msg create mode 100755 unitree_go/msg/TimeSpec.msg create mode 100755 unitree_go/msg/UwbState.msg create mode 100755 unitree_go/msg/UwbSwitch.msg create mode 100755 unitree_go/msg/WirelessController.msg create mode 100755 unitree_go/package.xml diff --git a/unitree_go/CMakeLists.txt b/unitree_go/CMakeLists.txt new file mode 100755 index 0000000..4ad71e6 --- /dev/null +++ b/unitree_go/CMakeLists.txt @@ -0,0 +1,80 @@ +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( 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/Go2State.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() diff --git a/unitree_go/msg/AudioData.msg b/unitree_go/msg/AudioData.msg new file mode 100755 index 0000000..6e889bc --- /dev/null +++ b/unitree_go/msg/AudioData.msg @@ -0,0 +1,2 @@ +uint64 time_frame +uint8[] data \ No newline at end of file diff --git a/unitree_go/msg/BmsCmd.msg b/unitree_go/msg/BmsCmd.msg new file mode 100755 index 0000000..bdbf9c3 --- /dev/null +++ b/unitree_go/msg/BmsCmd.msg @@ -0,0 +1,2 @@ +uint8 off +uint8[3] reserve \ No newline at end of file diff --git a/unitree_go/msg/BmsState.msg b/unitree_go/msg/BmsState.msg new file mode 100755 index 0000000..052e4d5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/unitree_go/msg/Error.msg b/unitree_go/msg/Error.msg new file mode 100755 index 0000000..4a5a631 --- /dev/null +++ b/unitree_go/msg/Error.msg @@ -0,0 +1,2 @@ +uint32 source +uint32 state \ No newline at end of file diff --git a/unitree_go/msg/Go2FrontVideoData.msg b/unitree_go/msg/Go2FrontVideoData.msg new file mode 100755 index 0000000..3c294f3 --- /dev/null +++ b/unitree_go/msg/Go2FrontVideoData.msg @@ -0,0 +1,4 @@ +uint64 time_frame +uint8[] video720p +uint8[] video360p +uint8[] video180p \ No newline at end of file diff --git a/unitree_go/msg/Go2State.msg b/unitree_go/msg/Go2State.msg new file mode 100644 index 0000000..175498c --- /dev/null +++ b/unitree_go/msg/Go2State.msg @@ -0,0 +1,12 @@ +uint8 mode +int32 progress +uint8 gait_type +float32 foot_raise_height +float32[3] position +float32 body_height +float32[3] velocity +float32[4] range_obstacle +int16[4] foot_force +float32[12] foot_position_body +float32[12] foot_speed_body + diff --git a/unitree_go/msg/HeightMap.msg b/unitree_go/msg/HeightMap.msg new file mode 100755 index 0000000..4e6f3b2 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/unitree_go/msg/IMUState.msg b/unitree_go/msg/IMUState.msg new file mode 100755 index 0000000..e065d04 --- /dev/null +++ b/unitree_go/msg/IMUState.msg @@ -0,0 +1,5 @@ +float32[4] quaternion +float32[3] gyroscope +float32[3] accelerometer +float32[3] rpy +int8 temperature diff --git a/unitree_go/msg/InterfaceConfig.msg b/unitree_go/msg/InterfaceConfig.msg new file mode 100755 index 0000000..c43bd20 --- /dev/null +++ b/unitree_go/msg/InterfaceConfig.msg @@ -0,0 +1,3 @@ +uint8 mode +uint8 value +uint8[2] reserve \ No newline at end of file diff --git a/unitree_go/msg/LidarState.msg b/unitree_go/msg/LidarState.msg new file mode 100755 index 0000000..d8b4eb2 --- /dev/null +++ b/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 diff --git a/unitree_go/msg/LowCmd.msg b/unitree_go/msg/LowCmd.msg new file mode 100755 index 0000000..464a31d --- /dev/null +++ b/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 + diff --git a/unitree_go/msg/LowState.msg b/unitree_go/msg/LowState.msg new file mode 100755 index 0000000..ceaed32 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/unitree_go/msg/MotorCmd.msg b/unitree_go/msg/MotorCmd.msg new file mode 100755 index 0000000..67cd1bf --- /dev/null +++ b/unitree_go/msg/MotorCmd.msg @@ -0,0 +1,7 @@ +uint8 mode +float32 q +float32 dq +float32 tau +float32 kp +float32 kd +uint32[3] reserve \ No newline at end of file diff --git a/unitree_go/msg/MotorCmds.msg b/unitree_go/msg/MotorCmds.msg new file mode 100644 index 0000000..a1d4726 --- /dev/null +++ b/unitree_go/msg/MotorCmds.msg @@ -0,0 +1 @@ +MotorCmd[] cmds \ No newline at end of file diff --git a/unitree_go/msg/MotorState.msg b/unitree_go/msg/MotorState.msg new file mode 100755 index 0000000..2e1a739 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/unitree_go/msg/MotorStates.msg b/unitree_go/msg/MotorStates.msg new file mode 100644 index 0000000..e4d9e7f --- /dev/null +++ b/unitree_go/msg/MotorStates.msg @@ -0,0 +1 @@ +MotorState[] states \ No newline at end of file diff --git a/unitree_go/msg/PathPoint.msg b/unitree_go/msg/PathPoint.msg new file mode 100755 index 0000000..08b4945 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/unitree_go/msg/Req.msg b/unitree_go/msg/Req.msg new file mode 100755 index 0000000..af14a7d --- /dev/null +++ b/unitree_go/msg/Req.msg @@ -0,0 +1,2 @@ +string uuid +string body \ No newline at end of file diff --git a/unitree_go/msg/Res.msg b/unitree_go/msg/Res.msg new file mode 100755 index 0000000..8070258 --- /dev/null +++ b/unitree_go/msg/Res.msg @@ -0,0 +1,3 @@ +string uuid +uint8[] data +string body \ No newline at end of file diff --git a/unitree_go/msg/SportModeCmd.msg b/unitree_go/msg/SportModeCmd.msg new file mode 100755 index 0000000..e5ccb46 --- /dev/null +++ b/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 + + diff --git a/unitree_go/msg/SportModeState.msg b/unitree_go/msg/SportModeState.msg new file mode 100755 index 0000000..7a6c1e6 --- /dev/null +++ b/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 + + diff --git a/unitree_go/msg/TimeSpec.msg b/unitree_go/msg/TimeSpec.msg new file mode 100755 index 0000000..188b3c1 --- /dev/null +++ b/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 diff --git a/unitree_go/msg/UwbState.msg b/unitree_go/msg/UwbState.msg new file mode 100755 index 0000000..ed1da3a --- /dev/null +++ b/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 + + diff --git a/unitree_go/msg/UwbSwitch.msg b/unitree_go/msg/UwbSwitch.msg new file mode 100755 index 0000000..db8ec91 --- /dev/null +++ b/unitree_go/msg/UwbSwitch.msg @@ -0,0 +1 @@ +uint8 enabled \ No newline at end of file diff --git a/unitree_go/msg/WirelessController.msg b/unitree_go/msg/WirelessController.msg new file mode 100755 index 0000000..12965ff --- /dev/null +++ b/unitree_go/msg/WirelessController.msg @@ -0,0 +1,5 @@ +float32 lx +float32 ly +float32 rx +float32 ry +uint16 keys \ No newline at end of file diff --git a/unitree_go/package.xml b/unitree_go/package.xml new file mode 100755 index 0000000..4111ce5 --- /dev/null +++ b/unitree_go/package.xml @@ -0,0 +1,23 @@ + + + + unitree_go + 0.0.1 + Communication interfaces of the unitree go 2 robots + Unitree + BSD 3-Clause License + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_cmake + + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From a3bf6e12170581762e61067e38ad3010f573d7e8 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 27 Jun 2024 15:43:57 +0200 Subject: [PATCH 06/34] Cyclonedds supported Signed-off-by: Juancams --- go2_driver/CMakeLists.txt | 50 +++++++ go2_driver/include/go2_driver/go2_driver.hpp | 88 +++++++++++++ go2_driver/package.xml | 8 ++ go2_driver/src/go2_driver/go2_driver.cpp | 132 +++++++++++++++++++ 4 files changed, 278 insertions(+) create mode 100644 go2_driver/include/go2_driver/go2_driver.hpp create mode 100644 go2_driver/src/go2_driver/go2_driver.cpp diff --git a/go2_driver/CMakeLists.txt b/go2_driver/CMakeLists.txt index c4c0413..781952a 100644 --- a/go2_driver/CMakeLists.txt +++ b/go2_driver/CMakeLists.txt @@ -6,6 +6,56 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(go2_interfaces REQUIRED) +find_package(tf2_ros REQUIRED) + +set(dependencies + rclcpp + rclcpp_lifecycle + rclcpp_components + sensor_msgs + geometry_msgs + nav_msgs + go2_interfaces + tf2_ros +) + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED + src/go2_driver/go2_driver.cpp +) + +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) + +rclcpp_components_register_nodes(${PROJECT_NAME} "go2_driver::Go2Driver") + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + DESTINATION lib + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/go2_driver/include/go2_driver/go2_driver.hpp b/go2_driver/include/go2_driver/go2_driver.hpp new file mode 100644 index 0000000..5ec9c3c --- /dev/null +++ b/go2_driver/include/go2_driver/go2_driver.hpp @@ -0,0 +1,88 @@ +// BSD 3-Clause License + +// Copyright (c) 2024, Intelligent Robotics Lab +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. + +// * 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. + +// * 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. + +#ifndef GO2_DRIVER__GO2_DRIVER_HPP_ +#define GO2_DRIVER__GO2_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "go2_interfaces/msg/go2_state.hpp" +#include "go2_interfaces/msg/low_state.hpp" +#include "go2_interfaces/msg/imu_state.hpp" +#include "tf2_ros/transform_broadcaster.h" + +namespace go2_driver +{ + +class Go2Driver : public rclcpp::Node +{ +public: + Go2Driver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void publish_lidar_cyclonedds(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void publish_body_poss_cyclonedds(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); + void publish_joint_state_cyclonedds(const go2_interfaces::msg::LowState::SharedPtr msg); + void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); + + + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Subscription::SharedPtr robot_pose_sub_; + rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Subscription::SharedPtr low_state_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr joint_state_pub_; + rclcpp::Publisher::SharedPtr go2_state_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + + std::string robot_ip_; + std::string token_; + std::string conn_type_; + std::unordered_map conn_; + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer_lidar_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + sensor_msgs::msg::Joy joy_state_; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_DRIVER_HPP_ diff --git a/go2_driver/package.xml b/go2_driver/package.xml index f78ae3a..3f59f63 100644 --- a/go2_driver/package.xml +++ b/go2_driver/package.xml @@ -9,6 +9,14 @@ ament_cmake + rclcpp + rclcpp_lifecycle + rclcpp_components + sensor_msgs + geometry_msgs + nav_msgs + go2_interfaces + ament_lint_auto ament_lint_common diff --git a/go2_driver/src/go2_driver/go2_driver.cpp b/go2_driver/src/go2_driver/go2_driver.cpp new file mode 100644 index 0000000..bdcfef0 --- /dev/null +++ b/go2_driver/src/go2_driver/go2_driver.cpp @@ -0,0 +1,132 @@ +// BSD 3-Clause License + +// Copyright (c) 2024, Intelligent Robotics Lab +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. + +// * 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. + +// * 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. + +#include + +namespace go2_driver +{ + +Go2Driver::Go2Driver( + const rclcpp::NodeOptions & options) +: Node("go2_driver", options), + tf_broadcaster_(this) +{ + // declare_parameter("robot_ip", std::getenv("ROBOT_IP")); + // declare_parameter("token", std::getenv("ROBOT_TOKEN")); + // declare_parameter("conn_type", std::getenv("CONN_TYPE")); + + // robot_ip_ = get_parameter("robot_ip").as_string(); + // token_ = get_parameter("token").as_string(); + // conn_type_ = get_parameter("conn_type").as_string(); + + RCLCPP_INFO(get_logger(), "Received ip: %s", robot_ip_.c_str()); + RCLCPP_INFO(get_logger(), "Connection type if: %s", conn_type_.c_str()); + + pointcloud_pub_ = create_publisher("pointcloud", 10); + joint_state_pub_ = create_publisher("joint_states", 10); + go2_state_pub_ = create_publisher("go2_states", 10); + odom_pub_ = create_publisher("odom", 10); + imu_pub_ = create_publisher("imu", 10); + + pointcloud_sub_ = create_subscription( + "/utlidar/cloud", 10, + std::bind(&Go2Driver::publish_lidar_cyclonedds, this, std::placeholders::_1)); + + robot_pose_sub_ = create_subscription( + "/utlidar/robot_pose", 10, + std::bind(&Go2Driver::publish_body_poss_cyclonedds, this, std::placeholders::_1)); + + joy_sub_ = create_subscription( + "joy", 10, std::bind(&Go2Driver::joy_callback, this, std::placeholders::_1)); + + low_state_sub_ = create_subscription( + "lowstate", 10, + std::bind(&Go2Driver::publish_joint_state_cyclonedds, this, std::placeholders::_1)); + + cmd_vel_sub_ = create_subscription( + "cmd_vel", 10, std::bind(&Go2Driver::cmd_vel_callback, this, std::placeholders::_1)); +} + +void Go2Driver::publish_lidar_cyclonedds(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + msg->header.stamp = now(); + msg->header.frame_id = "radar"; + pointcloud_pub_->publish(*msg); +} + +void Go2Driver::publish_body_poss_cyclonedds(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = now(); + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = msg->pose.position.x; + transform.transform.translation.y = msg->pose.position.y; + transform.transform.translation.z = msg->pose.position.z + 0.07; + transform.transform.rotation.x = msg->pose.orientation.x; + transform.transform.rotation.y = msg->pose.orientation.y; + transform.transform.rotation.z = msg->pose.orientation.z; + transform.transform.rotation.w = msg->pose.orientation.w; + tf_broadcaster_.sendTransform(transform); +} + +void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) +{ + joy_state_ = *msg; +} + +void Go2Driver::publish_joint_state_cyclonedds(const go2_interfaces::msg::LowState::SharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Received lowstate message"); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = now(); + joint_state.name = {"FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"}; + + joint_state.position = {msg->motor_state[3].q, msg->motor_state[4].q, msg->motor_state[5].q, + msg->motor_state[0].q, msg->motor_state[1].q, msg->motor_state[2].q, + msg->motor_state[9].q, msg->motor_state[10].q, msg->motor_state[11].q, + msg->motor_state[6].q, msg->motor_state[7].q, msg->motor_state[8].q}; + + joint_state_pub_->publish(joint_state); +} + +void Go2Driver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Received cmd_vel message"); + (void) msg; +} + +} // namespace go2_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(go2_driver::Go2Driver) From 8371fa8070ded3e4ab625f74cf80d9c54fff0279 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 27 Jun 2024 15:44:51 +0200 Subject: [PATCH 07/34] Renamed msgs Signed-off-by: Juancams --- go2_driver/include/go2_driver/go2_driver.hpp | 14 +++++++------- go2_driver/src/go2_driver/go2_driver.cpp | 8 ++++---- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/go2_driver/include/go2_driver/go2_driver.hpp b/go2_driver/include/go2_driver/go2_driver.hpp index 5ec9c3c..3723cb6 100644 --- a/go2_driver/include/go2_driver/go2_driver.hpp +++ b/go2_driver/include/go2_driver/go2_driver.hpp @@ -39,9 +39,9 @@ #include #include #include -#include "go2_interfaces/msg/go2_state.hpp" -#include "go2_interfaces/msg/low_state.hpp" -#include "go2_interfaces/msg/imu_state.hpp" +#include "unitree_go/msg/go2_state.hpp" +#include "unitree_go/msg/low_state.hpp" +#include "unitree_go/msg/imu_state.hpp" #include "tf2_ros/transform_broadcaster.h" namespace go2_driver @@ -56,21 +56,21 @@ class Go2Driver : public rclcpp::Node void publish_lidar_cyclonedds(const sensor_msgs::msg::PointCloud2::SharedPtr msg); void publish_body_poss_cyclonedds(const geometry_msgs::msg::PoseStamped::SharedPtr msg); void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); - void publish_joint_state_cyclonedds(const go2_interfaces::msg::LowState::SharedPtr msg); + void publish_joint_state_cyclonedds(const unitree_go::msg::LowState::SharedPtr msg); void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Subscription::SharedPtr robot_pose_sub_; rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Subscription::SharedPtr low_state_sub_; + rclcpp::Subscription::SharedPtr low_state_sub_; rclcpp::Subscription::SharedPtr cmd_vel_sub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr joint_state_pub_; - rclcpp::Publisher::SharedPtr go2_state_pub_; + rclcpp::Publisher::SharedPtr go2_state_pub_; rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; std::string robot_ip_; std::string token_; diff --git a/go2_driver/src/go2_driver/go2_driver.cpp b/go2_driver/src/go2_driver/go2_driver.cpp index bdcfef0..5316547 100644 --- a/go2_driver/src/go2_driver/go2_driver.cpp +++ b/go2_driver/src/go2_driver/go2_driver.cpp @@ -51,9 +51,9 @@ Go2Driver::Go2Driver( pointcloud_pub_ = create_publisher("pointcloud", 10); joint_state_pub_ = create_publisher("joint_states", 10); - go2_state_pub_ = create_publisher("go2_states", 10); + go2_state_pub_ = create_publisher("go2_states", 10); odom_pub_ = create_publisher("odom", 10); - imu_pub_ = create_publisher("imu", 10); + imu_pub_ = create_publisher("imu", 10); pointcloud_sub_ = create_subscription( "/utlidar/cloud", 10, @@ -66,7 +66,7 @@ Go2Driver::Go2Driver( joy_sub_ = create_subscription( "joy", 10, std::bind(&Go2Driver::joy_callback, this, std::placeholders::_1)); - low_state_sub_ = create_subscription( + low_state_sub_ = create_subscription( "lowstate", 10, std::bind(&Go2Driver::publish_joint_state_cyclonedds, this, std::placeholders::_1)); @@ -102,7 +102,7 @@ void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) joy_state_ = *msg; } -void Go2Driver::publish_joint_state_cyclonedds(const go2_interfaces::msg::LowState::SharedPtr msg) +void Go2Driver::publish_joint_state_cyclonedds(const unitree_go::msg::LowState::SharedPtr msg) { RCLCPP_INFO(get_logger(), "Received lowstate message"); sensor_msgs::msg::JointState joint_state; From a03b3cfc109fdebfa021933ebfffdb680128b733 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 27 Jun 2024 15:46:44 +0200 Subject: [PATCH 08/34] Renamed package Signed-off-by: Juancams --- go2_driver/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/go2_driver/CMakeLists.txt b/go2_driver/CMakeLists.txt index 781952a..1226cb0 100644 --- a/go2_driver/CMakeLists.txt +++ b/go2_driver/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) -find_package(go2_interfaces REQUIRED) +find_package(unitree_go REQUIRED) find_package(tf2_ros REQUIRED) set(dependencies @@ -22,7 +22,7 @@ set(dependencies sensor_msgs geometry_msgs nav_msgs - go2_interfaces + unitree_go tf2_ros ) From de7984d80c06afc6361e6a35b7fd9cadaaec2e50 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 27 Jun 2024 15:47:30 +0200 Subject: [PATCH 09/34] Renamed Signed-off-by: Juancams --- go2_driver/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/go2_driver/package.xml b/go2_driver/package.xml index 3f59f63..75d5012 100644 --- a/go2_driver/package.xml +++ b/go2_driver/package.xml @@ -15,7 +15,7 @@ sensor_msgs geometry_msgs nav_msgs - go2_interfaces + unitree_go ament_lint_auto ament_lint_common From 69d814a0d37d549aacfe63548c38e13723447ea4 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 12:58:04 +0200 Subject: [PATCH 10/34] Renamed link: base -> base_link Signed-off-by: Juancams --- go2_description/launch/robot.launch.py | 8 -------- go2_description/urdf/go2_description.urdf | 16 ++++++++-------- 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/go2_description/launch/robot.launch.py b/go2_description/launch/robot.launch.py index 0a306f7..89e2f47 100644 --- a/go2_description/launch/robot.launch.py +++ b/go2_description/launch/robot.launch.py @@ -101,12 +101,4 @@ def generate_launch_description(): ) ) - nodes.append(Node( - package='rviz2', - namespace='', - executable='rviz2', - name='rviz2', - ) - ) - return LaunchDescription(declared_arguments + nodes) diff --git a/go2_description/urdf/go2_description.urdf b/go2_description/urdf/go2_description.urdf index 9ada169..eeac215 100644 --- a/go2_description/urdf/go2_description.urdf +++ b/go2_description/urdf/go2_description.urdf @@ -6,7 +6,7 @@ Stephen Brawner (brawner@gmail.com) + name="base_link"> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> Date: Mon, 1 Jul 2024 12:59:24 +0200 Subject: [PATCH 11/34] Minor changes & added cmd_vel Signed-off-by: Juancams --- go2_driver/CMakeLists.txt | 2 ++ go2_driver/include/empty | 0 go2_driver/include/go2_driver/go2_driver.hpp | 14 ++++---- go2_driver/launch/go2_driver.launch.py | 31 ++++++++++++++++ go2_driver/src/empty | 0 go2_driver/src/go2_driver/go2_driver.cpp | 38 ++++++++++---------- 6 files changed, 57 insertions(+), 28 deletions(-) delete mode 100644 go2_driver/include/empty create mode 100644 go2_driver/launch/go2_driver.launch.py delete mode 100644 go2_driver/src/empty diff --git a/go2_driver/CMakeLists.txt b/go2_driver/CMakeLists.txt index 1226cb0..25c5030 100644 --- a/go2_driver/CMakeLists.txt +++ b/go2_driver/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(unitree_go REQUIRED) +find_package(unitree_api REQUIRED) find_package(tf2_ros REQUIRED) set(dependencies @@ -23,6 +24,7 @@ set(dependencies geometry_msgs nav_msgs unitree_go + unitree_api tf2_ros ) diff --git a/go2_driver/include/empty b/go2_driver/include/empty deleted file mode 100644 index e69de29..0000000 diff --git a/go2_driver/include/go2_driver/go2_driver.hpp b/go2_driver/include/go2_driver/go2_driver.hpp index 3723cb6..fc06c63 100644 --- a/go2_driver/include/go2_driver/go2_driver.hpp +++ b/go2_driver/include/go2_driver/go2_driver.hpp @@ -43,6 +43,8 @@ #include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/imu_state.hpp" #include "tf2_ros/transform_broadcaster.h" +#include "nlohmann/json.hpp" +#include "unitree_api/msg/request.hpp" namespace go2_driver { @@ -53,10 +55,10 @@ class Go2Driver : public rclcpp::Node Go2Driver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: - void publish_lidar_cyclonedds(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void publish_body_poss_cyclonedds(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void publish_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg); void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); - void publish_joint_state_cyclonedds(const unitree_go::msg::LowState::SharedPtr msg); + void publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg); void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); @@ -71,11 +73,7 @@ class Go2Driver : public rclcpp::Node rclcpp::Publisher::SharedPtr go2_state_pub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr imu_pub_; - - std::string robot_ip_; - std::string token_; - std::string conn_type_; - std::unordered_map conn_; + rclcpp::Publisher::SharedPtr request_pub_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_lidar_; diff --git a/go2_driver/launch/go2_driver.launch.py b/go2_driver/launch/go2_driver.launch.py new file mode 100644 index 0000000..f429488 --- /dev/null +++ b/go2_driver/launch/go2_driver.launch.py @@ -0,0 +1,31 @@ +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + composable_nodes = [] + + composable_node = ComposableNode( + package='go2_driver', + plugin='go2_driver::Go2Driver', + name='test_node', + namespace='', + + ) + composable_nodes.append(composable_node) + + container = ComposableNodeContainer( + name='go2_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen', + ) + + ld = LaunchDescription() + ld.add_action(container) + + return ld \ No newline at end of file diff --git a/go2_driver/src/empty b/go2_driver/src/empty deleted file mode 100644 index e69de29..0000000 diff --git a/go2_driver/src/go2_driver/go2_driver.cpp b/go2_driver/src/go2_driver/go2_driver.cpp index 5316547..e8ed349 100644 --- a/go2_driver/src/go2_driver/go2_driver.cpp +++ b/go2_driver/src/go2_driver/go2_driver.cpp @@ -38,50 +38,41 @@ Go2Driver::Go2Driver( : Node("go2_driver", options), tf_broadcaster_(this) { - // declare_parameter("robot_ip", std::getenv("ROBOT_IP")); - // declare_parameter("token", std::getenv("ROBOT_TOKEN")); - // declare_parameter("conn_type", std::getenv("CONN_TYPE")); - - // robot_ip_ = get_parameter("robot_ip").as_string(); - // token_ = get_parameter("token").as_string(); - // conn_type_ = get_parameter("conn_type").as_string(); - - RCLCPP_INFO(get_logger(), "Received ip: %s", robot_ip_.c_str()); - RCLCPP_INFO(get_logger(), "Connection type if: %s", conn_type_.c_str()); - pointcloud_pub_ = create_publisher("pointcloud", 10); joint_state_pub_ = create_publisher("joint_states", 10); go2_state_pub_ = create_publisher("go2_states", 10); odom_pub_ = create_publisher("odom", 10); imu_pub_ = create_publisher("imu", 10); + request_pub_ = create_publisher("api/sport/request", 10); + pointcloud_sub_ = create_subscription( "/utlidar/cloud", 10, - std::bind(&Go2Driver::publish_lidar_cyclonedds, this, std::placeholders::_1)); + std::bind(&Go2Driver::publish_lidar, this, std::placeholders::_1)); robot_pose_sub_ = create_subscription( "/utlidar/robot_pose", 10, - std::bind(&Go2Driver::publish_body_poss_cyclonedds, this, std::placeholders::_1)); + std::bind(&Go2Driver::publish_pose_stamped, this, std::placeholders::_1)); joy_sub_ = create_subscription( "joy", 10, std::bind(&Go2Driver::joy_callback, this, std::placeholders::_1)); low_state_sub_ = create_subscription( "lowstate", 10, - std::bind(&Go2Driver::publish_joint_state_cyclonedds, this, std::placeholders::_1)); + std::bind(&Go2Driver::publish_joint_states, this, std::placeholders::_1)); cmd_vel_sub_ = create_subscription( "cmd_vel", 10, std::bind(&Go2Driver::cmd_vel_callback, this, std::placeholders::_1)); } -void Go2Driver::publish_lidar_cyclonedds(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +void Go2Driver::publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { msg->header.stamp = now(); msg->header.frame_id = "radar"; pointcloud_pub_->publish(*msg); } -void Go2Driver::publish_body_poss_cyclonedds(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +void Go2Driver::publish_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { geometry_msgs::msg::TransformStamped transform; transform.header.stamp = now(); @@ -102,9 +93,8 @@ void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) joy_state_ = *msg; } -void Go2Driver::publish_joint_state_cyclonedds(const unitree_go::msg::LowState::SharedPtr msg) +void Go2Driver::publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg) { - RCLCPP_INFO(get_logger(), "Received lowstate message"); sensor_msgs::msg::JointState joint_state; joint_state.header.stamp = now(); joint_state.name = {"FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", @@ -122,8 +112,16 @@ void Go2Driver::publish_joint_state_cyclonedds(const unitree_go::msg::LowState:: void Go2Driver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) { - RCLCPP_INFO(get_logger(), "Received cmd_vel message"); - (void) msg; + nlohmann::json js; + js["x"] = msg->linear.x; + js["y"] = msg->linear.y; + js["z"] = msg->angular.z; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = 1008; + + request_pub_->publish(req); } } // namespace go2_driver From a96e7e393854e76f58a132691ab00b9b985d53d6 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 13:01:03 +0200 Subject: [PATCH 12/34] Added unitree api msg Signed-off-by: Juancams --- unitree_api/CMakeLists.txt | 64 +++++++++++++++++++++++++++++ unitree_api/msg/Request.msg | 3 ++ unitree_api/msg/RequestHeader.msg | 3 ++ unitree_api/msg/RequestIdentity.msg | 2 + unitree_api/msg/RequestLease.msg | 1 + unitree_api/msg/RequestPolicy.msg | 2 + unitree_api/msg/Response.msg | 3 ++ unitree_api/msg/ResponseHeader.msg | 2 + unitree_api/msg/ResponseStatus.msg | 1 + unitree_api/package.xml | 23 +++++++++++ 10 files changed, 104 insertions(+) create mode 100755 unitree_api/CMakeLists.txt create mode 100755 unitree_api/msg/Request.msg create mode 100755 unitree_api/msg/RequestHeader.msg create mode 100755 unitree_api/msg/RequestIdentity.msg create mode 100755 unitree_api/msg/RequestLease.msg create mode 100755 unitree_api/msg/RequestPolicy.msg create mode 100755 unitree_api/msg/Response.msg create mode 100755 unitree_api/msg/ResponseHeader.msg create mode 100755 unitree_api/msg/ResponseStatus.msg create mode 100755 unitree_api/package.xml diff --git a/unitree_api/CMakeLists.txt b/unitree_api/CMakeLists.txt new file mode 100755 index 0000000..4e7c4b9 --- /dev/null +++ b/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( 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() diff --git a/unitree_api/msg/Request.msg b/unitree_api/msg/Request.msg new file mode 100755 index 0000000..b1df58a --- /dev/null +++ b/unitree_api/msg/Request.msg @@ -0,0 +1,3 @@ +RequestHeader header +string parameter +uint8[] binary \ No newline at end of file diff --git a/unitree_api/msg/RequestHeader.msg b/unitree_api/msg/RequestHeader.msg new file mode 100755 index 0000000..022161b --- /dev/null +++ b/unitree_api/msg/RequestHeader.msg @@ -0,0 +1,3 @@ +RequestIdentity identity +RequestLease lease +RequestPolicy policy \ No newline at end of file diff --git a/unitree_api/msg/RequestIdentity.msg b/unitree_api/msg/RequestIdentity.msg new file mode 100755 index 0000000..90011ff --- /dev/null +++ b/unitree_api/msg/RequestIdentity.msg @@ -0,0 +1,2 @@ +int64 id +int64 api_id \ No newline at end of file diff --git a/unitree_api/msg/RequestLease.msg b/unitree_api/msg/RequestLease.msg new file mode 100755 index 0000000..85f692f --- /dev/null +++ b/unitree_api/msg/RequestLease.msg @@ -0,0 +1 @@ +int64 id \ No newline at end of file diff --git a/unitree_api/msg/RequestPolicy.msg b/unitree_api/msg/RequestPolicy.msg new file mode 100755 index 0000000..89e00c2 --- /dev/null +++ b/unitree_api/msg/RequestPolicy.msg @@ -0,0 +1,2 @@ +int32 priority +bool noreply \ No newline at end of file diff --git a/unitree_api/msg/Response.msg b/unitree_api/msg/Response.msg new file mode 100755 index 0000000..2036630 --- /dev/null +++ b/unitree_api/msg/Response.msg @@ -0,0 +1,3 @@ +ResponseHeader header +string data +int8[] binary diff --git a/unitree_api/msg/ResponseHeader.msg b/unitree_api/msg/ResponseHeader.msg new file mode 100755 index 0000000..3d51649 --- /dev/null +++ b/unitree_api/msg/ResponseHeader.msg @@ -0,0 +1,2 @@ +RequestIdentity identity +ResponseStatus status diff --git a/unitree_api/msg/ResponseStatus.msg b/unitree_api/msg/ResponseStatus.msg new file mode 100755 index 0000000..1d379fa --- /dev/null +++ b/unitree_api/msg/ResponseStatus.msg @@ -0,0 +1 @@ +int32 code \ No newline at end of file diff --git a/unitree_api/package.xml b/unitree_api/package.xml new file mode 100755 index 0000000..0c3037f --- /dev/null +++ b/unitree_api/package.xml @@ -0,0 +1,23 @@ + + + + unitree_api + 0.0.0 + TODO: Package description + Unitree +BSD 3-Clause License + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_cmake + + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From f4fe1878787ff64f39b65698eda1c4edfa706c4e Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 13:01:13 +0200 Subject: [PATCH 13/34] Update CI Signed-off-by: Juancams --- .github/workflows/humble_devel.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/humble_devel.yaml b/.github/workflows/humble_devel.yaml index 8a04fba..8a2cd2c 100644 --- a/.github/workflows/humble_devel.yaml +++ b/.github/workflows/humble_devel.yaml @@ -20,7 +20,7 @@ jobs: - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: go2_robot go2_description go2_interfaces + package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go target-ros2-distro: humble colcon-defaults: | { From f0c73a2586c19bf87f7633f127499b57a57f4db9 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 13:07:29 +0200 Subject: [PATCH 14/34] Added dependencies Signed-off-by: Juancams --- go2_driver/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/go2_driver/package.xml b/go2_driver/package.xml index 75d5012..1effe8c 100644 --- a/go2_driver/package.xml +++ b/go2_driver/package.xml @@ -16,6 +16,7 @@ geometry_msgs nav_msgs unitree_go + unitree_api ament_lint_auto ament_lint_common From 899b22df1e827cb73690624a79d37cdf97312dec Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 13:37:39 +0200 Subject: [PATCH 15/34] Update package.xml Signed-off-by: Juancams --- unitree_api/package.xml | 1 + unitree_go/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/unitree_api/package.xml b/unitree_api/package.xml index 0c3037f..8211947 100755 --- a/unitree_api/package.xml +++ b/unitree_api/package.xml @@ -13,6 +13,7 @@ ament_cmake geometry_msgs + rosidl_generator_dds_idl ament_lint_auto ament_lint_common diff --git a/unitree_go/package.xml b/unitree_go/package.xml index 4111ce5..d964f07 100755 --- a/unitree_go/package.xml +++ b/unitree_go/package.xml @@ -9,6 +9,7 @@ rosidl_default_generators rosidl_default_runtime + rosidl_generator_dds_idl rosidl_interface_packages ament_cmake From 417f71ce3b5e85784d58a77cbdd4dc31092672c8 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 15:09:45 +0200 Subject: [PATCH 16/34] Update package.xml Signed-off-by: Juancams --- go2_driver/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/go2_driver/package.xml b/go2_driver/package.xml index 1effe8c..0848793 100644 --- a/go2_driver/package.xml +++ b/go2_driver/package.xml @@ -17,6 +17,7 @@ nav_msgs unitree_go unitree_api + tf2_ros ament_lint_auto ament_lint_common From 6029fa260477e0774119a550af6518fda54579e3 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 15:33:32 +0200 Subject: [PATCH 17/34] Flake8 errors Signed-off-by: Juancams --- go2_driver/launch/go2_driver.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/go2_driver/launch/go2_driver.launch.py b/go2_driver/launch/go2_driver.launch.py index f429488..3cf7796 100644 --- a/go2_driver/launch/go2_driver.launch.py +++ b/go2_driver/launch/go2_driver.launch.py @@ -28,4 +28,4 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(container) - return ld \ No newline at end of file + return ld From 84a6af81455095e29156809c9806a2242316a123 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 15:33:45 +0200 Subject: [PATCH 18/34] Update CI Signed-off-by: Juancams --- .github/workflows/humble_devel.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/humble_devel.yaml b/.github/workflows/humble_devel.yaml index 8a2cd2c..1185d0c 100644 --- a/.github/workflows/humble_devel.yaml +++ b/.github/workflows/humble_devel.yaml @@ -12,6 +12,9 @@ jobs: - name: Checkout uses: actions/checkout@v2 + - name: Install nlohmann + run: sudo apt -y install nlohmann-json3-dev + - name: Setup ROS 2 environment uses: ros-tooling/setup-ros@0.7.0 with: From a18a48c1d8673d13a03513c6786ff925373cca4f Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 21:07:31 +0200 Subject: [PATCH 19/34] Added launch to bringup the robot with sensors Signed-off-by: Juancams --- go2_bringup/CMakeLists.txt | 5 ++ go2_bringup/launch/empty | 0 go2_bringup/launch/go2.launch.py | 91 ++++++++++++++++++++++++++++++++ go2_bringup/package.xml | 3 ++ 4 files changed, 99 insertions(+) delete mode 100644 go2_bringup/launch/empty create mode 100644 go2_bringup/launch/go2.launch.py diff --git a/go2_bringup/CMakeLists.txt b/go2_bringup/CMakeLists.txt index 541e22c..d4303ee 100644 --- a/go2_bringup/CMakeLists.txt +++ b/go2_bringup/CMakeLists.txt @@ -7,6 +7,11 @@ endif() find_package(ament_cmake REQUIRED) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) diff --git a/go2_bringup/launch/empty b/go2_bringup/launch/empty deleted file mode 100644 index e69de29..0000000 diff --git a/go2_bringup/launch/go2.launch.py b/go2_bringup/launch/go2.launch.py new file mode 100644 index 0000000..0a823b3 --- /dev/null +++ b/go2_bringup/launch/go2.launch.py @@ -0,0 +1,91 @@ +# BSD 3-Clause License + +# Copyright (c) 2024, Intelligent Robotics Lab +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * 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. + +# * 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression + + +def generate_launch_description(): + lidar = LaunchConfiguration('lidar') + realsense = LaunchConfiguration('realsense') + + declare_lidar_cmd = DeclareLaunchArgument( + 'lidar', + default_value='true', + description='Launch hesai lidar driver' + ) + + declare_realsense_cmd = DeclareLaunchArgument( + 'realsense', + default_value='true', + description='Launch realsense driver' + ) + + robot_description_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_description'), + 'launch/'), 'robot.launch.py']) + ) + + driver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_driver'), + 'launch/'), 'go2_driver.launch.py']) + ) + + lidar_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('hesai_ros_driver'), + 'launch/'), 'start.py']), + condition=IfCondition(PythonExpression([lidar])) + ) + + realsense_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), + 'launch/'), 'rs_launch.py']), + condition=IfCondition(PythonExpression([realsense])) + ) + + ld = LaunchDescription() + ld.add_action(declare_lidar_cmd) + ld.add_action(declare_realsense_cmd) + ld.add_action(robot_description_cmd) + ld.add_action(driver_cmd) + ld.add_action(lidar_cmd) + ld.add_action(realsense_cmd) + + return ld \ No newline at end of file diff --git a/go2_bringup/package.xml b/go2_bringup/package.xml index 683674c..50e4dcc 100644 --- a/go2_bringup/package.xml +++ b/go2_bringup/package.xml @@ -9,6 +9,9 @@ ament_cmake + go2_description + go2_driver + ament_lint_auto ament_lint_common From 010d313885a79a851962a2292b87787206babcbf Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 1 Jul 2024 21:07:49 +0200 Subject: [PATCH 20/34] Added license Signed-off-by: Juancams --- go2_driver/launch/go2_driver.launch.py | 30 ++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/go2_driver/launch/go2_driver.launch.py b/go2_driver/launch/go2_driver.launch.py index 3cf7796..df69e94 100644 --- a/go2_driver/launch/go2_driver.launch.py +++ b/go2_driver/launch/go2_driver.launch.py @@ -1,3 +1,33 @@ +# BSD 3-Clause License + +# Copyright (c) 2024, Intelligent Robotics Lab +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * 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. + +# * 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. + from launch import LaunchDescription from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode From 245d63b5888000d1367cdc2b2f42459ef6f22cf3 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Jul 2024 18:51:41 +0200 Subject: [PATCH 21/34] Flake8 errors solved Signed-off-by: Juancams --- go2_bringup/launch/go2.launch.py | 52 ++++++++++++++++---------------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/go2_bringup/launch/go2.launch.py b/go2_bringup/launch/go2.launch.py index 0a823b3..23d3ab4 100644 --- a/go2_bringup/launch/go2.launch.py +++ b/go2_bringup/launch/go2.launch.py @@ -39,53 +39,53 @@ def generate_launch_description(): - lidar = LaunchConfiguration('lidar') - realsense = LaunchConfiguration('realsense') + lidar = LaunchConfiguration('lidar') + realsense = LaunchConfiguration('realsense') - declare_lidar_cmd = DeclareLaunchArgument( + declare_lidar_cmd = DeclareLaunchArgument( 'lidar', default_value='true', description='Launch hesai lidar driver' - ) + ) - declare_realsense_cmd = DeclareLaunchArgument( + declare_realsense_cmd = DeclareLaunchArgument( 'realsense', default_value='true', description='Launch realsense driver' - ) + ) - robot_description_cmd = IncludeLaunchDescription( + robot_description_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('go2_description'), - 'launch/'), 'robot.launch.py']) - ) + get_package_share_directory('go2_description'), + 'launch/'), 'robot.launch.py']) + ) - driver_cmd = IncludeLaunchDescription( + driver_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('go2_driver'), - 'launch/'), 'go2_driver.launch.py']) - ) + get_package_share_directory('go2_driver'), + 'launch/'), 'go2_driver.launch.py']) + ) - lidar_cmd = IncludeLaunchDescription( + lidar_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('hesai_ros_driver'), 'launch/'), 'start.py']), condition=IfCondition(PythonExpression([lidar])) - ) + ) - realsense_cmd = IncludeLaunchDescription( + realsense_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('realsense2_camera'), 'launch/'), 'rs_launch.py']), condition=IfCondition(PythonExpression([realsense])) - ) + ) - ld = LaunchDescription() - ld.add_action(declare_lidar_cmd) - ld.add_action(declare_realsense_cmd) - ld.add_action(robot_description_cmd) - ld.add_action(driver_cmd) - ld.add_action(lidar_cmd) - ld.add_action(realsense_cmd) + ld = LaunchDescription() + ld.add_action(declare_lidar_cmd) + ld.add_action(declare_realsense_cmd) + ld.add_action(robot_description_cmd) + ld.add_action(driver_cmd) + ld.add_action(lidar_cmd) + ld.add_action(realsense_cmd) - return ld \ No newline at end of file + return ld From b63074052435ada2d650fc70a051e796fcf136c6 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 2 Jul 2024 21:57:33 +0200 Subject: [PATCH 22/34] Deleted msgs Signed-off-by: Juancams --- unitree_go/CMakeLists.txt | 2 -- unitree_go/msg/Go2FrontVideoData.msg | 4 ---- unitree_go/msg/Go2State.msg | 12 ------------ 3 files changed, 18 deletions(-) delete mode 100755 unitree_go/msg/Go2FrontVideoData.msg delete mode 100644 unitree_go/msg/Go2State.msg diff --git a/unitree_go/CMakeLists.txt b/unitree_go/CMakeLists.txt index 4ad71e6..c369295 100755 --- a/unitree_go/CMakeLists.txt +++ b/unitree_go/CMakeLists.txt @@ -30,8 +30,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BmsCmd.msg" "msg/BmsState.msg" "msg/Error.msg" - "msg/Go2FrontVideoData.msg" - "msg/Go2State.msg" "msg/HeightMap.msg" "msg/IMUState.msg" "msg/InterfaceConfig.msg" diff --git a/unitree_go/msg/Go2FrontVideoData.msg b/unitree_go/msg/Go2FrontVideoData.msg deleted file mode 100755 index 3c294f3..0000000 --- a/unitree_go/msg/Go2FrontVideoData.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 time_frame -uint8[] video720p -uint8[] video360p -uint8[] video180p \ No newline at end of file diff --git a/unitree_go/msg/Go2State.msg b/unitree_go/msg/Go2State.msg deleted file mode 100644 index 175498c..0000000 --- a/unitree_go/msg/Go2State.msg +++ /dev/null @@ -1,12 +0,0 @@ -uint8 mode -int32 progress -uint8 gait_type -float32 foot_raise_height -float32[3] position -float32 body_height -float32[3] velocity -float32[4] range_obstacle -int16[4] foot_force -float32[12] foot_position_body -float32[12] foot_speed_body - From 390d32d71a2a4d7977b8cc8257a400b7233251c9 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 10:28:45 +0200 Subject: [PATCH 23/34] Added srv Signed-off-by: Juancams --- .github/workflows/humble_devel.yaml | 2 +- go2_interfaces/CMakeLists.txt | 33 ++++++++++++++++++++++++++ go2_interfaces/package.xml | 23 ++++++++++++++++++ go2_interfaces/srv/BodyHeight.srv | 6 +++++ go2_interfaces/srv/ContinuousGait.srv | 6 +++++ go2_interfaces/srv/Euler.srv | 9 +++++++ go2_interfaces/srv/FootRaiseHeight.srv | 7 ++++++ go2_interfaces/srv/Mode.srv | 5 ++++ go2_interfaces/srv/Pose.srv | 6 +++++ go2_interfaces/srv/SpeedLevel.srv | 7 ++++++ go2_interfaces/srv/SwitchGait.srv | 13 ++++++++++ go2_interfaces/srv/SwitchJoystick.srv | 6 +++++ 12 files changed, 122 insertions(+), 1 deletion(-) create mode 100755 go2_interfaces/CMakeLists.txt create mode 100755 go2_interfaces/package.xml create mode 100644 go2_interfaces/srv/BodyHeight.srv create mode 100644 go2_interfaces/srv/ContinuousGait.srv create mode 100644 go2_interfaces/srv/Euler.srv create mode 100644 go2_interfaces/srv/FootRaiseHeight.srv create mode 100644 go2_interfaces/srv/Mode.srv create mode 100644 go2_interfaces/srv/Pose.srv create mode 100644 go2_interfaces/srv/SpeedLevel.srv create mode 100644 go2_interfaces/srv/SwitchGait.srv create mode 100644 go2_interfaces/srv/SwitchJoystick.srv diff --git a/.github/workflows/humble_devel.yaml b/.github/workflows/humble_devel.yaml index 1185d0c..f67d1ea 100644 --- a/.github/workflows/humble_devel.yaml +++ b/.github/workflows/humble_devel.yaml @@ -23,7 +23,7 @@ jobs: - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go + package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go go2_interfaces target-ros2-distro: humble colcon-defaults: | { diff --git a/go2_interfaces/CMakeLists.txt b/go2_interfaces/CMakeLists.txt new file mode 100755 index 0000000..1a22ec5 --- /dev/null +++ b/go2_interfaces/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(rosidl_default_generators REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/BodyHeight.srv" + "srv/ContinuousGait.srv" + "srv/Euler.srv" + "srv/FootRaiseHeight.srv" + "srv/Mode.srv" + "srv/Pose.srv" + "srv/SpeedLevel.srv" + "srv/SwitchGait.srv" + "srv/SwitchJoystick.srv" + DEPENDENCIES +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/go2_interfaces/package.xml b/go2_interfaces/package.xml new file mode 100755 index 0000000..bdecdd8 --- /dev/null +++ b/go2_interfaces/package.xml @@ -0,0 +1,23 @@ + + + + go2_interfaces + 0.0.1 + Go2 Interfaces + Juan Carlos Manzanares + BSD 3-Clause License + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_interfaces/srv/BodyHeight.srv b/go2_interfaces/srv/BodyHeight.srv new file mode 100644 index 0000000..4cc898d --- /dev/null +++ b/go2_interfaces/srv/BodyHeight.srv @@ -0,0 +1,6 @@ +# Set the relative height of the body above the ground when standing and walking. +float32 height + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/ContinuousGait.srv b/go2_interfaces/srv/ContinuousGait.srv new file mode 100644 index 0000000..44f3607 --- /dev/null +++ b/go2_interfaces/srv/ContinuousGait.srv @@ -0,0 +1,6 @@ +# Continuous movement + +bool flag + +--- +bool success \ No newline at end of file diff --git a/go2_interfaces/srv/Euler.srv b/go2_interfaces/srv/Euler.srv new file mode 100644 index 0000000..f46af61 --- /dev/null +++ b/go2_interfaces/srv/Euler.srv @@ -0,0 +1,9 @@ +# Posture when standing and walking + +float32 roll +float32 pitch +float32 yaw + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/FootRaiseHeight.srv b/go2_interfaces/srv/FootRaiseHeight.srv new file mode 100644 index 0000000..3f59531 --- /dev/null +++ b/go2_interfaces/srv/FootRaiseHeight.srv @@ -0,0 +1,7 @@ +# Set the relative height of foot lift during movement + +float32 height + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/Mode.srv b/go2_interfaces/srv/Mode.srv new file mode 100644 index 0000000..9e540fd --- /dev/null +++ b/go2_interfaces/srv/Mode.srv @@ -0,0 +1,5 @@ +string mode + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/Pose.srv b/go2_interfaces/srv/Pose.srv new file mode 100644 index 0000000..1afd315 --- /dev/null +++ b/go2_interfaces/srv/Pose.srv @@ -0,0 +1,6 @@ +# Set true to pose and false to restore + +bool flag + +--- +bool success \ No newline at end of file diff --git a/go2_interfaces/srv/SpeedLevel.srv b/go2_interfaces/srv/SpeedLevel.srv new file mode 100644 index 0000000..6d240c3 --- /dev/null +++ b/go2_interfaces/srv/SpeedLevel.srv @@ -0,0 +1,7 @@ +# Set the speed range + +int32 level + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/SwitchGait.srv b/go2_interfaces/srv/SwitchGait.srv new file mode 100644 index 0000000..eef6b6e --- /dev/null +++ b/go2_interfaces/srv/SwitchGait.srv @@ -0,0 +1,13 @@ +# Switch gait + +int32 IDLE = 0 +int32 TROT = 1 +int32 TROT_RUN_NING = 2 +int32 FORWARD_CLIMBING_MODE = 3 +int32 REVERSE_CLIMBING_MODE = 4 + +int32 d + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/SwitchJoystick.srv b/go2_interfaces/srv/SwitchJoystick.srv new file mode 100644 index 0000000..329f4d6 --- /dev/null +++ b/go2_interfaces/srv/SwitchJoystick.srv @@ -0,0 +1,6 @@ +# Native remote control response switch + +bool flag + +--- +bool success \ No newline at end of file From fad9cf1dbe525f78f9855be987fd0f62b6dcbaf5 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 10:29:16 +0200 Subject: [PATCH 24/34] Added services to change mode & configs Signed-off-by: Juancams --- go2_driver/CMakeLists.txt | 2 + go2_driver/include/go2_driver/go2_api_id.hpp | 79 +++++ go2_driver/include/go2_driver/go2_driver.hpp | 68 +++- go2_driver/launch/go2_driver.launch.py | 2 +- go2_driver/package.xml | 3 +- go2_driver/src/go2_driver/go2_driver.cpp | 338 ++++++++++++++++++- 6 files changed, 485 insertions(+), 7 deletions(-) create mode 100644 go2_driver/include/go2_driver/go2_api_id.hpp diff --git a/go2_driver/CMakeLists.txt b/go2_driver/CMakeLists.txt index 25c5030..8cfb2ca 100644 --- a/go2_driver/CMakeLists.txt +++ b/go2_driver/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(nav_msgs REQUIRED) find_package(unitree_go REQUIRED) find_package(unitree_api REQUIRED) find_package(tf2_ros REQUIRED) +find_package(go2_interfaces REQUIRED) set(dependencies rclcpp @@ -26,6 +27,7 @@ set(dependencies unitree_go unitree_api tf2_ros + go2_interfaces ) include_directories(include) diff --git a/go2_driver/include/go2_driver/go2_api_id.hpp b/go2_driver/include/go2_driver/go2_api_id.hpp new file mode 100644 index 0000000..e9517f5 --- /dev/null +++ b/go2_driver/include/go2_driver/go2_api_id.hpp @@ -0,0 +1,79 @@ +// BSD 3-Clause License + +// Copyright (c) 2024, Intelligent Robotics Lab +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. + +// * 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. + +// * 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. + +#ifndef GO2_DRIVER__GO2_API_ID_HPP +#define GO2_DRIVER__GO2_API_ID_HPP + +namespace go2_driver +{ + +enum class Mode +{ + Damp = 1001, + BalanceStand = 1002, + StopMove = 1003, + StandUp = 1004, + StandDown = 1005, + RecoveryStand = 1006, + Euler = 1007, + Move = 1008, + Sit = 1009, + RiseSit = 1010, + SwitchGait = 1011, + Trigger = 1012, + BodyHeight = 1013, + FootRaiseHeight = 1014, + SpeedLevel = 1015, + Hello = 1016, + Stretch = 1017, + TrajectoryFollow = 1018, + ContinuousGait = 1019, + Content = 1020, + Wallow = 1021, + Dance1 = 1022, + Dance2 = 1023, + GetBodyHeight = 1024, + GetFootRaiseHeight = 1025, + GetSpeedLevel = 1026, + SwitchJoystick = 1027, + Pose = 1028, + Scrape = 1029, + FrontFlip = 1030, + FrontJump = 1031, + FrontPounce = 1032, + WiggleHips = 1033, + GetState = 1034, + EconomicGait = 1035, + FingerHeart = 1036, +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_API_ID_HPP diff --git a/go2_driver/include/go2_driver/go2_driver.hpp b/go2_driver/include/go2_driver/go2_driver.hpp index fc06c63..aad7ff6 100644 --- a/go2_driver/include/go2_driver/go2_driver.hpp +++ b/go2_driver/include/go2_driver/go2_driver.hpp @@ -39,12 +39,21 @@ #include #include #include -#include "unitree_go/msg/go2_state.hpp" #include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/imu_state.hpp" #include "tf2_ros/transform_broadcaster.h" #include "nlohmann/json.hpp" #include "unitree_api/msg/request.hpp" +#include "go2_driver/go2_api_id.hpp" +#include "go2_interfaces/srv/body_height.hpp" +#include "go2_interfaces/srv/continuous_gait.hpp" +#include "go2_interfaces/srv/euler.hpp" +#include "go2_interfaces/srv/foot_raise_height.hpp" +#include "go2_interfaces/srv/mode.hpp" +#include "go2_interfaces/srv/pose.hpp" +#include "go2_interfaces/srv/speed_level.hpp" +#include "go2_interfaces/srv/switch_gait.hpp" +#include "go2_interfaces/srv/switch_joystick.hpp" namespace go2_driver { @@ -61,6 +70,50 @@ class Go2Driver : public rclcpp::Node void publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg); void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); + void handleBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleContinuousGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleEuler( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleMode( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handlePose( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSwitchGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSwitchJoystick( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Subscription::SharedPtr robot_pose_sub_; @@ -70,15 +123,26 @@ class Go2Driver : public rclcpp::Node rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr joint_state_pub_; - rclcpp::Publisher::SharedPtr go2_state_pub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr imu_pub_; rclcpp::Publisher::SharedPtr request_pub_; + rclcpp::Service::SharedPtr set_body_height_service_; + rclcpp::Service::SharedPtr set_continuous_gait_service_; + rclcpp::Service::SharedPtr set_euler_service_; + rclcpp::Service::SharedPtr set_foot_raise_height_service_; + rclcpp::Service::SharedPtr set_mode_service_; + rclcpp::Service::SharedPtr set_pose_service_; + rclcpp::Service::SharedPtr set_speed_level_service_; + rclcpp::Service::SharedPtr set_switch_gait_service_; + rclcpp::Service::SharedPtr set_switch_joystick_service_; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_lidar_; tf2_ros::TransformBroadcaster tf_broadcaster_; sensor_msgs::msg::Joy joy_state_; + + bool odom_published_{false}; }; } // namespace go2_driver diff --git a/go2_driver/launch/go2_driver.launch.py b/go2_driver/launch/go2_driver.launch.py index df69e94..56bd117 100644 --- a/go2_driver/launch/go2_driver.launch.py +++ b/go2_driver/launch/go2_driver.launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): composable_node = ComposableNode( package='go2_driver', plugin='go2_driver::Go2Driver', - name='test_node', + name='go2_driver', namespace='', ) diff --git a/go2_driver/package.xml b/go2_driver/package.xml index 0848793..9d7729f 100644 --- a/go2_driver/package.xml +++ b/go2_driver/package.xml @@ -3,7 +3,7 @@ go2_driver 0.0.0 - TODO: Package description + GO2 robot driver package Juan Carlos Manzanares Serrano BSD 3-Clause License @@ -18,6 +18,7 @@ unitree_go unitree_api tf2_ros + go2_interfaces ament_lint_auto ament_lint_common diff --git a/go2_driver/src/go2_driver/go2_driver.cpp b/go2_driver/src/go2_driver/go2_driver.cpp index e8ed349..dbf061e 100644 --- a/go2_driver/src/go2_driver/go2_driver.cpp +++ b/go2_driver/src/go2_driver/go2_driver.cpp @@ -40,12 +40,10 @@ Go2Driver::Go2Driver( { pointcloud_pub_ = create_publisher("pointcloud", 10); joint_state_pub_ = create_publisher("joint_states", 10); - go2_state_pub_ = create_publisher("go2_states", 10); odom_pub_ = create_publisher("odom", 10); imu_pub_ = create_publisher("imu", 10); request_pub_ = create_publisher("api/sport/request", 10); - pointcloud_sub_ = create_subscription( "/utlidar/cloud", 10, std::bind(&Go2Driver::publish_lidar, this, std::placeholders::_1)); @@ -63,6 +61,69 @@ Go2Driver::Go2Driver( cmd_vel_sub_ = create_subscription( "cmd_vel", 10, std::bind(&Go2Driver::cmd_vel_callback, this, std::placeholders::_1)); + + set_body_height_service_ = + this->create_service( + "body_height", + std::bind( + &Go2Driver::handleBodyHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_continuous_gait_service_ = + this->create_service( + "continuous_gait", + std::bind( + &Go2Driver::handleContinuousGait, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_euler_service_ = + this->create_service( + "euler", + std::bind( + &Go2Driver::handleEuler, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_foot_raise_height_service_ = + this->create_service( + "foot_raise_height", + std::bind( + &Go2Driver::handleFootRaiseHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_mode_service_ = + this->create_service( + "mode", + std::bind( + &Go2Driver::handleMode, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_pose_service_ = + this->create_service( + "pose", + std::bind( + &Go2Driver::handlePose, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_speed_level_service_ = + this->create_service( + "speed_level", + std::bind( + &Go2Driver::handleSpeedLevel, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_gait_service_ = + this->create_service( + "switch_gait", + std::bind( + &Go2Driver::handleSwitchGait, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_joystick_service_ = + this->create_service( + "switch_joystick", + std::bind( + &Go2Driver::handleSwitchJoystick, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } void Go2Driver::publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg) @@ -86,6 +147,22 @@ void Go2Driver::publish_pose_stamped(const geometry_msgs::msg::PoseStamped::Shar transform.transform.rotation.z = msg->pose.orientation.z; transform.transform.rotation.w = msg->pose.orientation.w; tf_broadcaster_.sendTransform(transform); + + if (!odom_published_) { + nav_msgs::msg::Odometry odom; + odom.header.stamp = now(); + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + odom.pose.pose.position.x = msg->pose.position.x; + odom.pose.pose.position.y = msg->pose.position.y; + odom.pose.pose.position.z = msg->pose.position.z + 0.07; + odom.pose.pose.orientation.x = msg->pose.orientation.x; + odom.pose.pose.orientation.y = msg->pose.orientation.y; + odom.pose.pose.orientation.z = msg->pose.orientation.z; + odom.pose.pose.orientation.w = msg->pose.orientation.w; + odom_pub_->publish(odom); + odom_published_ = true; + } } void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) @@ -119,9 +196,264 @@ void Go2Driver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) unitree_api::msg::Request req; req.parameter = js.dump(); - req.header.identity.api_id = 1008; + req.header.identity.api_id = static_cast(go2_driver::Mode::Move); + + request_pub_->publish(req); +} + +void Go2Driver::handleBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->height < -0.18 || request->height > 0.03) { + response->success = false; + response->message = "Height value is out of range [0.3 ~ 0.5]"; + return; + } + + nlohmann::json js; + js["data"] = request->height; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::BodyHeight); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleContinuousGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::ContinuousGait); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleEuler( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + if (request->roll < -0.75 || request->roll > 0.75) { + response->success = false; + response->message = "Roll value is out of range [-0.75 ~ 0.75]"; + return; + } else if (request->pitch < -0.75 || request->pitch > 0.75) { + response->success = false; + response->message = "Pitch value is out of range [-0.75 ~ 0.75]"; + return; + } else if (request->yaw < -0.6 || request->yaw > 0.6) { + response->success = false; + response->message = "Yaw value is out of range [-1.5 ~ 1.5]"; + return; + } + + js["x"] = request->roll; + js["y"] = request->pitch; + js["z"] = request->yaw; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Euler); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->height < 0 || request->height > 0.1) { + response->success = false; + response->message = "Height value is out of range [-0.06 ~ 0.03]"; + return; + } + + nlohmann::json js; + js["data"] = request->height; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::FootRaiseHeight); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleMode( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + std::string mode = request->mode; + + unitree_api::msg::Request req; + + if (mode == "damp") { + response->message = "Change the mode to Damp"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Damp); + } else if (mode == "balance_stand") { + response->message = "Change the mode to BalanceStand"; + req.header.identity.api_id = static_cast(go2_driver::Mode::BalanceStand); + } else if (mode == "stop_move") { + response->message = "Change the mode to StopMove"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StopMove); + } else if (mode == "stand_up") { + response->message = "Change the mode to StandUp"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StandUp); + } else if (mode == "stand_down") { + response->message = "Change the mode to StandDown"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StandDown); + } else if (mode == "sit") { + response->message = "Change the mode to Sit"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Sit); + } else if (mode == "rise_sit") { + response->message = "Change the mode to RiseSit"; + req.header.identity.api_id = static_cast(go2_driver::Mode::RiseSit); + } else if (mode == "hello") { + response->message = "Change the mode to Hello. Say hello to your robot!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Hello); + } else if (mode == "stretch") { + response->message = "Change the mode to Stretch"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Stretch); + } else if (mode == "wallow") { + response->message = "Change the mode to Wallow"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Wallow); + } else if (mode == "scrape") { + response->message = "Change the mode to Scrape"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Scrape); + } else if (mode == "front_flip") { + response->message = "Front flip??? Really? You want to break your robot? Crazy!"; + // req.header.identity.api_id = static_cast(go2_driver::Mode::FrontFlip); + } else if (mode == "front_jump") { + response->message = "Change the mode to Front Jump"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FrontJump); + } else if (mode == "front_pounce") { + response->message = "Change the mode to Front Pounce"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FrontPounce); + } else if (mode == "dance1") { + response->message = "Change the mode to Dance 1. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance1); + } else if (mode == "dance2") { + response->message = "Change the mode to Dance 2. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance2); + } else if (mode == "finger_heart") { + response->message = "Change the mode to Finger Heart"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FingerHeart); + } else { + response->success = false; + response->message = "Invalid mode"; + return; + } + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handlePose( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Pose); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->level < -1 || request->level > 1) { + response->success = false; + response->message = "Speed level is out of range [-1 ~ 1]"; + return; + } + + nlohmann::json js; + js["data"] = request->level; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SpeedLevel); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleSwitchGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->d < 0 || request->d > 4) { + response->success = false; + response->message = "Invalid gait type [0 - 4]"; + return; + } + + nlohmann::json js; + js["data"] = request->d; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchGait); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleSwitchJoystick( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchJoystick); request_pub_->publish(req); + response->success = true; } } // namespace go2_driver From ce5184aec00080db0750324e89ded7f1cb99d416 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 10:38:54 +0200 Subject: [PATCH 25/34] Flake8 Signed-off-by: Juancams --- go2_bringup/launch/go2.launch.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/go2_bringup/launch/go2.launch.py b/go2_bringup/launch/go2.launch.py index 23d3ab4..83107a4 100644 --- a/go2_bringup/launch/go2.launch.py +++ b/go2_bringup/launch/go2.launch.py @@ -43,27 +43,27 @@ def generate_launch_description(): realsense = LaunchConfiguration('realsense') declare_lidar_cmd = DeclareLaunchArgument( - 'lidar', - default_value='true', - description='Launch hesai lidar driver' + 'lidar', + default_value='true', + description='Launch hesai lidar driver' ) declare_realsense_cmd = DeclareLaunchArgument( - 'realsense', - default_value='true', - description='Launch realsense driver' + 'realsense', + default_value='true', + description='Launch realsense driver' ) robot_description_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('go2_description'), - 'launch/'), 'robot.launch.py']) + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_description'), + 'launch/'), 'robot.launch.py']) ) driver_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('go2_driver'), - 'launch/'), 'go2_driver.launch.py']) + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_driver'), + 'launch/'), 'go2_driver.launch.py']) ) lidar_cmd = IncludeLaunchDescription( From be8b93bd64c3feca1e4e83a1a84e9436188e0b7f Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 12:32:53 +0200 Subject: [PATCH 26/34] Added go2_rviz & visualization in bringup Signed-off-by: Juancams --- go2_bringup/launch/go2.launch.py | 22 +- go2_rviz/CMakeLists.txt | 21 ++ go2_rviz/config/go2_rviz.rviz | 471 +++++++++++++++++++++++++++++++ go2_rviz/launch/rviz.launch.py | 50 ++++ go2_rviz/package.xml | 18 ++ 5 files changed, 579 insertions(+), 3 deletions(-) create mode 100644 go2_rviz/CMakeLists.txt create mode 100644 go2_rviz/config/go2_rviz.rviz create mode 100644 go2_rviz/launch/rviz.launch.py create mode 100644 go2_rviz/package.xml diff --git a/go2_bringup/launch/go2.launch.py b/go2_bringup/launch/go2.launch.py index 83107a4..ab243a0 100644 --- a/go2_bringup/launch/go2.launch.py +++ b/go2_bringup/launch/go2.launch.py @@ -41,19 +41,26 @@ def generate_launch_description(): lidar = LaunchConfiguration('lidar') realsense = LaunchConfiguration('realsense') + rviz = LaunchConfiguration('rviz') declare_lidar_cmd = DeclareLaunchArgument( 'lidar', - default_value='true', + default_value='False', description='Launch hesai lidar driver' ) declare_realsense_cmd = DeclareLaunchArgument( 'realsense', - default_value='true', + default_value='False', description='Launch realsense driver' ) + declare_rviz_cmd = DeclareLaunchArgument( + 'rviz', + default_value='False', + description='Launch rviz' + ) + robot_description_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('go2_description'), @@ -80,12 +87,21 @@ def generate_launch_description(): condition=IfCondition(PythonExpression([realsense])) ) + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_rviz'), + 'launch/'), 'rviz.launch.py']), + condition=IfCondition(PythonExpression([rviz])) + ) + ld = LaunchDescription() ld.add_action(declare_lidar_cmd) ld.add_action(declare_realsense_cmd) + ld.add_action(declare_rviz_cmd) ld.add_action(robot_description_cmd) - ld.add_action(driver_cmd) ld.add_action(lidar_cmd) ld.add_action(realsense_cmd) + ld.add_action(driver_cmd) + ld.add_action(rviz_cmd) return ld diff --git a/go2_rviz/CMakeLists.txt b/go2_rviz/CMakeLists.txt new file mode 100644 index 0000000..69e6adb --- /dev/null +++ b/go2_rviz/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_rviz) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_rviz/config/go2_rviz.rviz b/go2_rviz/config/go2_rviz.rviz new file mode 100644 index 0000000..cdf3c7e --- /dev/null +++ b/go2_rviz/config/go2_rviz.rviz @@ -0,0 +1,471 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 89 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 443 + - 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: PointCloud2 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + FL_calf: + Value: true + FL_calflower: + Value: true + FL_calflower1: + Value: true + FL_foot: + Value: true + FL_hip: + Value: true + FL_thigh: + Value: true + FR_calf: + Value: true + FR_calflower: + Value: true + FR_calflower1: + Value: true + FR_foot: + Value: true + FR_hip: + Value: true + FR_thigh: + Value: true + Head_lower: + Value: true + Head_upper: + Value: true + RL_calf: + Value: true + RL_calflower: + Value: true + RL_calflower1: + Value: true + RL_foot: + Value: true + RL_hip: + Value: true + RL_thigh: + Value: true + RR_calf: + Value: true + RR_calflower: + Value: true + RR_calflower1: + Value: true + RR_foot: + Value: true + RR_hip: + Value: true + RR_thigh: + Value: true + base_link: + Value: true + camera_link: + Value: true + hesai_lidar: + Value: true + imu: + Value: true + odom: + Value: true + radar: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + odom: + base_link: + FL_hip: + FL_thigh: + FL_calf: + FL_calflower: + FL_calflower1: + {} + FL_foot: + {} + FR_hip: + FR_thigh: + FR_calf: + FR_calflower: + FR_calflower1: + {} + FR_foot: + {} + Head_upper: + Head_lower: + {} + camera_link: + {} + RL_hip: + RL_thigh: + RL_calf: + RL_calflower: + RL_calflower1: + {} + RL_foot: + {} + RR_hip: + RR_thigh: + RR_calf: + RR_calflower: + RR_calflower1: + {} + RR_foot: + {} + hesai_lidar: + {} + imu: + {} + radar: + {} + Update Interval: 0 + 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_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_calflower1: + 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_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calflower1: + 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_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Head_lower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Head_upper: + 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_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_calflower1: + 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_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calflower1: + 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_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hesai_lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu: + Alpha: 1 + Show Axes: false + Show Trail: false + radar: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 216 + Min Color: 0; 0; 0 + Min Intensity: 74 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + 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: 2.064781665802002 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.02936464361846447 + Y: 0.031766824424266815 + Z: 0.3274531662464142 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4752010107040405 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.166723251342773 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 751 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000003000000000000015600000251fc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000820000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000251000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000480043006f0072006500730065006e0073006500200049006e0073007400720075006d0065006e0074006100740069006f006e0020005200760069007a002000500061006e0065006c01000002de000000b50000000000000000fb0000001e0043006f0072006500730065006e00730065005f00500061006e0065006c0100000176000001180000000000000000fb0000000a0049006d00610067006501000002e8000000ab0000000000000000000000010000015d0000037cfc0200000005fb0000000a00560069006500770073000000003d0000037c000000a400fffffffb0000003e0043006f0072006500730065006e0073006500200049006e0073007400720075006d0065006e0074006100740069006f006e002000500061006e0065006c000000003d0000037c0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000251000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a0000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004440000025100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1440 + X: 2208 + Y: 74 diff --git a/go2_rviz/launch/rviz.launch.py b/go2_rviz/launch/rviz.launch.py new file mode 100644 index 0000000..1244881 --- /dev/null +++ b/go2_rviz/launch/rviz.launch.py @@ -0,0 +1,50 @@ +# BSD 3-Clause License + +# Copyright (c) 2024, Intelligent Robotics Lab +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * 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. + +# * 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', + os.path.join(get_package_share_directory('go2_rviz'), + 'config', 'go2_rviz.rviz')] + ) + ]) \ No newline at end of file diff --git a/go2_rviz/package.xml b/go2_rviz/package.xml new file mode 100644 index 0000000..4b2fde9 --- /dev/null +++ b/go2_rviz/package.xml @@ -0,0 +1,18 @@ + + + + go2_rviz + 0.0.1 + Unitree go2 rviz + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From a79408cb83b657559ed9c156807cf982ffe15cda Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 15:55:02 +0200 Subject: [PATCH 27/34] Changed qos for odom Signed-off-by: Juancams --- go2_driver/src/go2_driver/go2_driver.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/go2_driver/src/go2_driver/go2_driver.cpp b/go2_driver/src/go2_driver/go2_driver.cpp index dbf061e..beb5804 100644 --- a/go2_driver/src/go2_driver/go2_driver.cpp +++ b/go2_driver/src/go2_driver/go2_driver.cpp @@ -38,9 +38,12 @@ Go2Driver::Go2Driver( : Node("go2_driver", options), tf_broadcaster_(this) { + rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + pointcloud_pub_ = create_publisher("pointcloud", 10); joint_state_pub_ = create_publisher("joint_states", 10); - odom_pub_ = create_publisher("odom", 10); + odom_pub_ = create_publisher("odom", qos_profile); imu_pub_ = create_publisher("imu", 10); request_pub_ = create_publisher("api/sport/request", 10); From 88b4f7a53c45bf50cbbd12967dc47d908ebd6c76 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 15:55:20 +0200 Subject: [PATCH 28/34] Added pointcloud to laser Signed-off-by: Juancams --- go2_driver/launch/go2_driver.launch.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/go2_driver/launch/go2_driver.launch.py b/go2_driver/launch/go2_driver.launch.py index 56bd117..fc0541e 100644 --- a/go2_driver/launch/go2_driver.launch.py +++ b/go2_driver/launch/go2_driver.launch.py @@ -29,7 +29,7 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode @@ -55,7 +55,21 @@ def generate_launch_description(): output='screen', ) + pointclod_to_laserscan_cmd = Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='pointcloud_to_laserscan', + namespace='', + output='screen', + remappings=[('/cloud_in', '/pointcloud')], + parameters=[{ + 'target_frame': 'radar', + 'transform_tolerance': 0.01, + }], + ) + ld = LaunchDescription() ld.add_action(container) + ld.add_action(pointclod_to_laserscan_cmd) return ld From 446236bab2d4ee2fc285605578e3b6929aad869f Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 3 Jul 2024 15:56:01 +0200 Subject: [PATCH 29/34] Works but problems with amcl Signed-off-by: Juancams --- go2_nav/CMakeLists.txt | 21 ++ go2_nav/launch/navigation.launch.py | 126 ++++++++++ go2_nav/launch/slam.launch.py | 0 go2_nav/package.xml | 18 ++ go2_nav/params/go2_nav_params.yaml | 358 ++++++++++++++++++++++++++++ 5 files changed, 523 insertions(+) create mode 100644 go2_nav/CMakeLists.txt create mode 100644 go2_nav/launch/navigation.launch.py create mode 100644 go2_nav/launch/slam.launch.py create mode 100644 go2_nav/package.xml create mode 100644 go2_nav/params/go2_nav_params.yaml diff --git a/go2_nav/CMakeLists.txt b/go2_nav/CMakeLists.txt new file mode 100644 index 0000000..a9a26f4 --- /dev/null +++ b/go2_nav/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_nav) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch params + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_nav/launch/navigation.launch.py b/go2_nav/launch/navigation.launch.py new file mode 100644 index 0000000..3b05186 --- /dev/null +++ b/go2_nav/launch/navigation.launch.py @@ -0,0 +1,126 @@ +# BSD 3-Clause License + +# Copyright (c) 2024, Intelligent Robotics Lab +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * 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. + +# * 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import SetRemap + +def generate_launch_description(): + package_dir = get_package_share_directory('go2_nav') + nav2_dir = get_package_share_directory('nav2_bringup') + + # Configuration Variables + use_sim_time = LaunchConfiguration('use_sim_time') + slam = LaunchConfiguration('slam') + rviz = LaunchConfiguration('rviz') + map_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + namespace = LaunchConfiguration('namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', default_value='false') + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', default_value='False') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'rviz', default_value='True') + + declare_map_cmd = DeclareLaunchArgument( + 'map', default_value='') + + declare_nav_params_cmd = DeclareLaunchArgument( + 'params_file', default_value=os.path.join( + package_dir, + 'params', + 'go2_nav_params.yaml') + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='' + ) + + # Actions + localization_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_dir, 'launch', 'localization_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'slam': slam, + 'map': map_file, + 'params_file': params_file, + 'namespace': namespace + }.items() + ) + + navigation_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_dir, 'launch', 'navigation_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'namespace': namespace + }.items() + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'rviz': rviz, + 'namespace': namespace + }.items() + ) + + # Remappings + cmd_vel_remap = SetRemap(src='cmd_vel_nav', dst='cmd_vel') + + ld = LaunchDescription() + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_nav_params_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_map_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(localization_cmd) + ld.add_action(navigation_cmd) + ld.add_action(rviz_cmd) + ld.add_action(cmd_vel_remap) + + return ld \ No newline at end of file diff --git a/go2_nav/launch/slam.launch.py b/go2_nav/launch/slam.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/go2_nav/package.xml b/go2_nav/package.xml new file mode 100644 index 0000000..76e0bae --- /dev/null +++ b/go2_nav/package.xml @@ -0,0 +1,18 @@ + + + + go2_nav + 0.0.1 + Unitree go2 robot navigation + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_nav/params/go2_nav_params.yaml b/go2_nav/params/go2_nav_params.yaml new file mode 100644 index 0000000..9de5dd5 --- /dev/null +++ b/go2_nav/params/go2_nav_params.yaml @@ -0,0 +1,358 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::OmniMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: /scan + set_initial_pose: true + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 + + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.30 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.30 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.4 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 1.5 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.4 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 1.5 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file From 1ce05e51cf4fcb27ad925bd9c9f61d115408a2ff Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Wed, 3 Jul 2024 17:49:43 +0200 Subject: [PATCH 30/34] Update README.md --- README.md | 214 +++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 213 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 558c725..2e52bd4 100644 --- a/README.md +++ b/README.md @@ -1 +1,213 @@ -# Unitree GO2 Robot +# Unitree GO2 Robot ROS 2 + +

+Go2 point cloud +

+ + + + + +[![License](https://img.shields.io/badge/license-BSD--3-yellow.svg)](https://opensource.org/licenses/BSD-3-Clause) +![distro](https://img.shields.io/badge/Ubuntu%2022-Jammy%20Jellyfish-green) +![distro](https://img.shields.io/badge/ROS2-Humble-blue) +[![humble](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble.yaml/badge.svg)](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble.yaml) +[![humble-devel](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble_devel.yaml/badge.svg)](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble_devel.yaml) + +In this package is our integration for the Unitee Go2 robot. + +## Checklist + +- [x] robot description +- [x] odom +- [x] pointcloud +- [x] joint_states +- [x] Visualization in rviz +- [x] cmd_vel +- [x] go2_interfaces +- [x] Change modes +- [x] Change configuration for robot +- [ ] SLAM (working in progress) +- [ ] Nav2 (working in progress) +- [ ] Ros2cli +- [ ] Hardware interface +- [ ] Gazebo simulation + +## Installation +You need to have previously installed ROS2. Please follow this [guide](https://docs.ros.org/en/humble/Installation.html) if you don't have it. + +```bash +source /opt/ros/humble/setup.bash +``` + +Create workspace and clone the repository + +```bash +mkdir ~/go2_ws/src +cd ~/go2_ws/src +git clone https://github.com/IntelligentRoboticsLabs/go2_robot.git -b humble +``` + +Install dependencies and build workspace +```bash +cd ~/go2_ws +sudo rosdep init +rosdep update +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install +``` + +Setup the workspace +```bash +source ~/ros2_ws/install/setup.bash +``` + +## Sensor installation +If you have purchased a hesai lidar 3d, or a realsense d435i, follow the following steps inside the robot. + +### Hesai Lidar + +```bash +sudo apt-get install libboost-all-dev +sudo apt-get install -y libyaml-cpp-dev +git clone --recurse-submodules https://github.com/HesaiTechnology/HesaiLidar_ROS_2.0.git +cd .. +source /opt/ros/$ROS_DISTRO/setup.bash +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install +source install/setup.bash +``` + +Set the lidar IP to `config/config.yaml` +```yaml +lidar: +- driver: + udp_port: 2368 #UDP port of lidar + ptc_port: 9347 #PTC port of lidar + device_ip_address: #IP address of lidar + pcap_path: "" #The path of pcap file (set during offline playback) + correction_file_path: "" #LiDAR angle file, required for offline playback of pcap/packet rosbag + firetimes_path: "" #The path of firetimes file + source_type: 2 #The type of data source, 1: real-time lidar connection, 2: pcap, 3: packet rosbag + pcap_play_synchronization: true #Pcap play rate synchronize with the host time + x: 0 #Calibration parameter + y: 0 #Calibration parameter + z: 0 #Calibration parameter + roll: 0 #Calibration parameter + pitch: 0 #Calibration parameter + yaw: 0 #Calibration parameter +ros: + ros_frame_id: hesai_lidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /lidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /lidar_packets #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /lidar_points #Topic used to send point cloud through ROS + send_packet_ros: true #true: Send packets through ROS + send_point_cloud_ros: true #true: Send point cloud through ROS +``` + +### Realsense d435i + +```bash +sudo apt install ros-humble-realsense2-camera +``` + +## Usage +### Bringup the robot +Either from your computer, or from inside the robot, execute the following: +```bash +ros2 launch go2_bringup go2.launch.py +``` +> If you have a realsense and a lidar inside the robot, use the lidar or realsense parameters. +> It will only work if you throw everything inside the robot + +If you want to see your robot through rviz, do it as follows: +```bash +ros2 launch go2_bringup go2.launch.py rviz:=True +``` + +### Change modes +If what you want is for your robot to be able to change modes, thus performing the movements predefined by the controller, use the following service: +```bash +ros2 service call /mode go2_interfaces/srv/Mode "mode: 'hello'" +``` + +
+Available modes +``` +damp +balance_stand +stop_move +stand_up +stand_down +sit +rise_sit +hello +stretch +wallow +scrape +front_flip +front_jump +front_pounce +dance1 +dance2 +finger_heart +``` +
+ +### Change configurations for robot +If you want, you can modify the ways the robot walks, the height of the base, the height of the legs when walking... I show you the different parameters that can be modified: + +- BodyHeight: Set the relative height of the body above the ground when standing and walking. [0.3 ~ 0.5] + ```bash + ros2 service call /body_height go2_interfaces/srv/BodyHeight "height: 0.0" + ``` +- ContinuousGait: Continuous movement + ```bash + ros2 service call /continuous_gait go2_interfaces/srv/ContinuousGait "flag: false" + ``` +- Euler: Posture when standing and walking. [-0.75 ~ 0.75] [-0.75 ~ 0.75] [-1.5 ~ 1.5] + ```bash + ros2 service call /euler go2_interfaces/srv/Euler "roll: 0.0 pitch: 0.0 yaw: 0.0" + ``` +- FootRaiseHeight: Set the relative height of foot lift during movement [-0.06 ~ 0.03] + ```bash + ros2 service call /foot_raise_height go2_interfaces/srv/FootRaiseHeight "height: 0.0" + ``` +- Pose: Set true to pose and false to restore + ```bash + ros2 service call /pose go2_interfaces/srv/Pose "flag: false" + ``` +- SpeedLevel: Set the speed range [-1 ~ 1] + ```bash + ros2 service call /speed_level go2_interfaces/srv/SpeedLevel "level: 0" + ``` +- SwitchGait: Switch gait [0 - 4] + ```bash + ros2 service call /switch_gait go2_interfaces/srv/SwitchGait "d: 0" + ``` +- SwitchJoystick: Native remote control response switch + ```bash + ros2 service call /switch_joystick go2_interfaces/srv/SwitchJoystick "flag: false" + ``` + +## SLAM +In the future, work in progress. + +## NAVIGATION +In the future, work in progress. + +## Acknowledgment +Thanks to [unitree](https://github.com/unitreerobotics/unitree_ros2) for providing the support and communication interfaces with the robot. + +## About +This is a project made by the [Intelligent Robotics Lab](https://intelligentroboticslab.gsyc.urjc.es/), a research group from the [Universidad Rey Juan Carlos](https://www.urjc.es/). +Copyright © 2024. + +Maintainers: + +* [Juan Carlos Manzanares Serrano](https://github.com/Juancams) +* [Juan S. Cely](https://github.com/juanscelyg) + +## License + +This project is licensed under the BSD 3-clause License - see the [LICENSE](https://github.com/IntelligentRoboticsLabs/go2_robot/blob/humble/LICENSE) file for details. From 94ff69b9b44abc73b741087e1c81fd83e46921e8 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Thu, 4 Jul 2024 16:03:27 +0200 Subject: [PATCH 31/34] Update README.md --- README.md | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 2e52bd4..9fd926b 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,6 @@ Go2 point cloud

- - - - [![License](https://img.shields.io/badge/license-BSD--3-yellow.svg)](https://opensource.org/licenses/BSD-3-Clause) ![distro](https://img.shields.io/badge/Ubuntu%2022-Jammy%20Jellyfish-green) ![distro](https://img.shields.io/badge/ROS2-Humble-blue) @@ -196,6 +192,15 @@ In the future, work in progress. ## NAVIGATION In the future, work in progress. +## Demos +### Robot description + +[go2_tf.webm](https://github.com/IntelligentRoboticsLabs/go2_robot/assets/44479765/b019e6fc-875f-4870-a510-afa6e1353e08) + +### Robot PointCloud + +https://github.com/IntelligentRoboticsLabs/go2_robot/assets/44479765/c164840a-6857-4d95-a50e-023c5bb44edb + ## Acknowledgment Thanks to [unitree](https://github.com/unitreerobotics/unitree_ros2) for providing the support and communication interfaces with the robot. From a5ee1670c924aa7db87a5118e9046edf52555edc Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 4 Jul 2024 16:04:59 +0200 Subject: [PATCH 32/34] Update CI Signed-off-by: Juancams --- .github/workflows/humble.yaml | 2 +- .github/workflows/humble_devel.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 5ebc51e..79d738c 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -20,7 +20,7 @@ jobs: - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: go2_robot go2_description go2_interfaces + package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go go2_interfaces go2_nav go2_rviz target-ros2-distro: humble colcon-defaults: | { diff --git a/.github/workflows/humble_devel.yaml b/.github/workflows/humble_devel.yaml index f67d1ea..4873282 100644 --- a/.github/workflows/humble_devel.yaml +++ b/.github/workflows/humble_devel.yaml @@ -23,7 +23,7 @@ jobs: - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go go2_interfaces + package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go go2_interfaces go2_nav go2_rviz target-ros2-distro: humble colcon-defaults: | { From 827e0585165e34280a03c6ca95cdf9ee75f0e68a Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 4 Jul 2024 16:13:05 +0200 Subject: [PATCH 33/34] Flake8 Signed-off-by: Juancams --- go2_nav/launch/navigation.launch.py | 3 ++- go2_rviz/launch/rviz.launch.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/go2_nav/launch/navigation.launch.py b/go2_nav/launch/navigation.launch.py index 3b05186..15a4c07 100644 --- a/go2_nav/launch/navigation.launch.py +++ b/go2_nav/launch/navigation.launch.py @@ -37,6 +37,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import SetRemap + def generate_launch_description(): package_dir = get_package_share_directory('go2_nav') nav2_dir = get_package_share_directory('nav2_bringup') @@ -123,4 +124,4 @@ def generate_launch_description(): ld.add_action(rviz_cmd) ld.add_action(cmd_vel_remap) - return ld \ No newline at end of file + return ld diff --git a/go2_rviz/launch/rviz.launch.py b/go2_rviz/launch/rviz.launch.py index 1244881..369a4dc 100644 --- a/go2_rviz/launch/rviz.launch.py +++ b/go2_rviz/launch/rviz.launch.py @@ -47,4 +47,4 @@ def generate_launch_description(): os.path.join(get_package_share_directory('go2_rviz'), 'config', 'go2_rviz.rviz')] ) - ]) \ No newline at end of file + ]) From db86993b87cc2d4e207b6ee49b02091d8df7d5d0 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 4 Jul 2024 16:53:40 +0200 Subject: [PATCH 34/34] 1.0.0 Signed-off-by: Juancams --- go2_bringup/package.xml | 3 ++- go2_description/package.xml | 2 +- go2_driver/package.xml | 2 +- go2_interfaces/package.xml | 2 +- go2_robot/package.xml | 2 +- go2_rviz/package.xml | 2 +- 6 files changed, 7 insertions(+), 6 deletions(-) diff --git a/go2_bringup/package.xml b/go2_bringup/package.xml index 50e4dcc..64ee390 100644 --- a/go2_bringup/package.xml +++ b/go2_bringup/package.xml @@ -2,7 +2,7 @@ go2_bringup - 0.0.0 + 1.0.0 TODO: Package description Juan Carlos Manzanares Serrano BSD 3-Clause License @@ -11,6 +11,7 @@ go2_description go2_driver + go2_rviz ament_lint_auto ament_lint_common diff --git a/go2_description/package.xml b/go2_description/package.xml index ec4616c..7f72d06 100644 --- a/go2_description/package.xml +++ b/go2_description/package.xml @@ -2,7 +2,7 @@ go2_description - 0.0.1 + 1.0.0 Unitree go2 robot description Juan Carlos Manzanares Serrano BSD 3-Clause License diff --git a/go2_driver/package.xml b/go2_driver/package.xml index 9d7729f..480d135 100644 --- a/go2_driver/package.xml +++ b/go2_driver/package.xml @@ -2,7 +2,7 @@ go2_driver - 0.0.0 + 1.0.0 GO2 robot driver package Juan Carlos Manzanares Serrano BSD 3-Clause License diff --git a/go2_interfaces/package.xml b/go2_interfaces/package.xml index bdecdd8..f0fd5c8 100755 --- a/go2_interfaces/package.xml +++ b/go2_interfaces/package.xml @@ -2,7 +2,7 @@ go2_interfaces - 0.0.1 + 1.0.0 Go2 Interfaces Juan Carlos Manzanares BSD 3-Clause License diff --git a/go2_robot/package.xml b/go2_robot/package.xml index 75845ab..14aa5c2 100644 --- a/go2_robot/package.xml +++ b/go2_robot/package.xml @@ -1,7 +1,7 @@ go2_robot - 0.0.1 + 1.0.0 The unitree go2 robot package Juan Carlos Manzanares Serrano BSD 3-Clause License diff --git a/go2_rviz/package.xml b/go2_rviz/package.xml index 4b2fde9..f94028b 100644 --- a/go2_rviz/package.xml +++ b/go2_rviz/package.xml @@ -2,7 +2,7 @@ go2_rviz - 0.0.1 + 1.0 Unitree go2 rviz Juan Carlos Manzanares Serrano BSD 3-Clause License