diff --git a/README.md b/README.md index 9b883ea..0a9f0cf 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,20 @@ +![Ros2 SDK](https://github.com/abizovnuralem/go2_ros2_sdk/assets/33475993/49edebbe-11b6-49c6-b82d-bc46257674bd) + # Welcome to the Unitree Go2 ROS2 SDK Project! -![ROS2 CI](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg) +[![IsaacSim](https://img.shields.io/badge/IsaacSim-4.0-silver.svg)](https://docs.omniverse.nvidia.com/isaacsim/latest/overview.html) +[![Python](https://img.shields.io/badge/python-3.10-blue.svg)](https://docs.python.org/3/whatsnew/3.10.html) +[![Linux platform](https://img.shields.io/badge/platform-linux--64-orange.svg)](https://releases.ubuntu.com/22.04/) +[![Windows platform](https://img.shields.io/badge/platform-windows--64-orange.svg)](https://www.microsoft.com/en-us/) +![ROS2 Build](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg) +[![License](https://img.shields.io/badge/license-BSD--2-yellow.svg)](https://opensource.org/licenses/BSD-2-Clause) -We are delighted to present you with our integration of the Unitree Go2 ROS2 SDK, leveraging the innovative go2-WebRTC interface, originally designed by the talented @tfoldi. You can explore and utilize his groundbreaking work at go2-webrtc on GitHub. +We are happy to present you our integration of the Unitree Go2 with ROS2 over Wi-Fi, that was designed by the talented [@tfoldi](https://github.com/tfoldi). You can explore his groundbreaking work at [go2-webrtc](https://github.com/tfoldi/go2-webrtc). -This resourceful project is here to empower your Unitree GO2 AIR/PRO/EDU robots with ROS2 SDK capabilities. We're thrilled to offer an enhanced level of control and interaction, enabling you to take your robotics projects to new heights. +This repo will empower your Unitree GO2 AIR/PRO/EDU robots with ROS2 capabilities, using both WebRTC (Wi-Fi) and CycloneDDS (Ethernet) protocols. ## Project RoadMap: + 1. URDF :white_check_mark: 2. Joint states sync in real time :white_check_mark: 3. IMU sync in real time :white_check_mark: @@ -17,14 +25,17 @@ This resourceful project is here to empower your Unitree GO2 AIR/PRO/EDU robots 9. Camera stream :white_check_mark: 10. Foxglove bridge :white_check_mark: 11. Laser Scan :white_check_mark: -12. SLAM (slam_toolbox) :white_check_mark: -13. Navigation (nav2) :white_check_mark: -14. Object detection -15. AutoPilot +12. Multi robot support :white_check_mark: +13. WebRTC and CycloneDDS support :white_check_mark: +14. Creating a PointCloud map and store it +15. SLAM (slam_toolbox) (in the current version is not working, need to fix params) +16. Navigation (nav2) (in the current version is not working, need to fix params) +17. Object detection +18. AutoPilot ## Your feedback and support mean the world to us. -If you're as enthusiastic about this project as we are, please consider giving it a :star: star on our GitHub repository. +If you're as enthusiastic about this project as we are, please consider giving it a :star: star!!! Your encouragement fuels our passion and helps us develop our RoadMap further. We welcome any help or suggestions you can offer! @@ -36,35 +47,21 @@ Together, let's push the boundaries of what's possible with the Unitree Go2 and :robot: Compatible with AIR, PRO, and EDU variants -:footprints: Access to foot force sensors feedback (available on GO2 PRO/EDU) +:footprints: Access to foot force sensors feedback (available on some GO2 PRO models or EDU) -Real time Go2 Air/PRO/EDU joints sync: +## Real time Go2 Air/PRO/EDU joints sync:

Go2 joints sync

-Go2 Air/PRO/EDU lidar point cloud: - -

-Go2 point cloud -

+## Go2 Air/PRO/EDU lidar point cloud:

Go2 point cloud

-## Topic -Real time Go2 Air/PRO ROS2 topics - -

-Go2 topic list -

- -

-Go2 topic list -

## System requirements Tested systems and ROS2 distro @@ -74,41 +71,18 @@ Tested systems and ROS2 distro |Ubuntu 22.04|humble|![ROS2 CI](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg) |Ubuntu 22.04|rolling|![ROS2 CI](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg) -A single workspace can contain as many packages as you want, each in their own folder. You can also have packages of different build types in one workspace (CMake, Python, etc.). You cannot have nested packages. -Best practice is to have a src folder within your workspace, and to create your packages in there. This keeps the top level of the workspace “clean”. - -Your workspace should look like: -``` -workspace_folder/ - src/ - py_package_1/ - package.xml - resource/py_package_1 - setup.cfg - setup.py - py_package_1/ - - py_package_2/ - package.xml - resource/py_package_2 - setup.cfg - setup.py - py_package_2/ -``` - -clone this repo to src folder of your own ros2_ws repo +## Installation ``` +mkdir -p ros2_ws/src +cd ros2_ws/src git clone --recurse-submodules https://github.com/abizovnuralem/go2_ros2_sdk.git - -cd go2_ros2_sdk +cp -a go2_ros2_sdk/. . +rm -r -f go2_ros2_sdk sudo apt install python3-pip clang pip install -r requirements.txt cd .. -mkdir -p ros2_ws/src -copy all files inside go2_ros2_sdk folder to ros2_ws/src folder - ``` install rust language support in your system https://www.rust-lang.org/tools/install @@ -126,21 +100,37 @@ https://docs.ros.org/en/humble/Installation.html ``` source /opt/ros/$ROS_DISTRO/setup.bash -cd ros2_ws rosdep install --from-paths src --ignore-src -r -y colcon build ``` ## Usage -don't forget to setup your GO2-robot in Wifi-mode and get IP then +don't forget to setup your GO2-robot in Wifi-mode and get IP (You can use mobile app to get it, go to Device -> Data -> Automatic Machine Inspection, (look for STA Network: wlan0)) ``` -export ROBOT_IP="Your robot ip" -cd ros2_ws source install/setup.bash +export ROBOT_IP="robot_ip" +export CONN_TYPE="webrtc" ros2 launch go2_robot_sdk robot.launch.py ``` + +## Multi robot support + +If you want to connect several robots for collaboration: +``` +export ROBOT_IP="robot_ip_1, robot_ip_2, robot_ip_N" +``` + +## Switching between webrtc connection (Wi-Fi) to CycloneDDS (Ethernet) +``` +export CONN_TYPE="webrtc" +``` +or +``` +export CONN_TYPE="cyclonedds" +``` + ## Foxglove

@@ -154,13 +144,7 @@ sudo snap install foxglove-studio 1. Open Foxglove Studio and press "Open Connection". 2. In the "Open Connection" settings, choose "Foxglove WebSocket" and use the default configuration ws://localhost:8765, then press "Open". -3. (Optional) You can also import a default layout view from the foxglove.json file located inside this repository. -## SLAM - -

-Slam -

## WSL 2 @@ -198,21 +182,15 @@ If you are running ROS2 under WSL2 - you may need to configure Joystick\Gamepad Run `ros2 run joy joy_enumerate_devices` - You should see something like this - ``` ID : GUID : GamePad : Mapped : Joystick Device Name ------------------------------------------------------------------------------- 0 : 030000005e040000120b000007050000 : true : false : Xbox Series X Controller ``` -## Development - -To contribute or modify the project, refer to these resources for implementing additional features or improving the existing codebase. PRs are welcome! - ## Thanks -Special thanks to @legion1581, @tfoldi, @budavariam, @alex.lin and TheRoboVerse community! +Special thanks to @tfoldi, @legion1581, @budavariam, @alex.lin and TheRoboVerse community! ## License diff --git a/foxglove.json b/foxglove.json deleted file mode 100644 index a6b11ed..0000000 --- a/foxglove.json +++ /dev/null @@ -1,159 +0,0 @@ -{ - "configById": { - "3D!1z5wp7g": { - "cameraState": { - "perspective": true, - "distance": 4.51871081984833, - "phi": 44.76377952755682, - "thetaOffset": 146.33858267716917, - "targetOffset": [ - -0.7589250245619992, - 0.315563322561357, - 2.3955431119317754e-17 - ], - "target": [ - 0, - 0, - 0 - ], - "targetOrientation": [ - 0, - 0, - 0, - 1 - ], - "fovy": 45, - "near": 0.5, - "far": 5000 - }, - "followMode": "follow-pose", - "followTf": "base", - "scene": { - "transforms": { - "showLabel": false - }, - "enableStats": true, - "labelScaleFactor": 1, - "ignoreColladaUpAxis": false - }, - "transforms": { - "frame:base": { - "visible": true - }, - "frame:FL_hip": { - "visible": false - }, - "frame:FL_thigh": { - "visible": false - }, - "frame:FL_calf": { - "visible": false - }, - "frame:FL_calflower": { - "visible": false - }, - "frame:FL_calflower1": { - "visible": false - }, - "frame:FL_foot": { - "visible": false - }, - "frame:FR_hip": { - "visible": false - }, - "frame:FR_thigh": { - "visible": false - }, - "frame:FR_calf": { - "visible": false - }, - "frame:FR_calflower": { - "visible": false - }, - "frame:FR_calflower1": { - "visible": false - }, - "frame:FR_foot": { - "visible": false - }, - "frame:Head_upper": { - "visible": false - }, - "frame:Head_lower": { - "visible": false - }, - "frame:imu": { - "visible": false - }, - "frame:radar": { - "visible": false - }, - "frame:RL_hip": { - "visible": false - }, - "frame:RL_thigh": { - "visible": false - }, - "frame:RL_calf": { - "visible": false - }, - "frame:RL_calflower": { - "visible": false - }, - "frame:RL_calflower1": { - "visible": false - }, - "frame:RL_foot": { - "visible": false - }, - "frame:RR_hip": { - "visible": false - }, - "frame:RR_thigh": { - "visible": false - }, - "frame:RR_calf": { - "visible": false - }, - "frame:RR_calflower": { - "visible": false - }, - "frame:RR_calflower1": { - "visible": false - }, - "frame:RR_foot": { - "visible": false - } - }, - "topics": { - "/robot_description": { - "visible": true - }, - "/point_cloud2": { - "visible": true, - "colorField": "intensity", - "colorMode": "colormap", - "colorMap": "turbo" - } - }, - "layers": {}, - "publish": { - "type": "point", - "poseTopic": "/move_base_simple/goal", - "pointTopic": "/clicked_point", - "poseEstimateTopic": "/initialpose", - "poseEstimateXDeviation": 0.5, - "poseEstimateYDeviation": 0.5, - "poseEstimateThetaDeviation": 0.26179939 - }, - "imageMode": {}, - "foxglovePanelTitle": "GO2 Unitree" - } - }, - "globalVariables": {}, - "userNodes": {}, - "playbackConfig": { - "speed": 1 - }, - "layout": "3D!1z5wp7g" -} \ No newline at end of file diff --git a/go2_lidar_3.gif b/go2_lidar_3.gif deleted file mode 100644 index 3224fe6..0000000 Binary files a/go2_lidar_3.gif and /dev/null differ diff --git a/go2_robot_sdk/config/cyclonedds_config.rviz b/go2_robot_sdk/config/cyclonedds_config.rviz new file mode 100644 index 0000000..71e9f69 --- /dev/null +++ b/go2_robot_sdk/config/cyclonedds_config.rviz @@ -0,0 +1,351 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 599 + - 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 +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 + - 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: /robot0/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + map: + Alpha: 1 + Show Axes: false + Show Trail: false + odom: + Alpha: 1 + Show Axes: false + Show Trail: false + robot0/FL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FL_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FL_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FR_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FR_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/FR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/Head_lower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/Head_upper: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RL_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RL_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RR_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RR_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/RR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + robot0/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot0/imu: + Alpha: 1 + Show Axes: false + Show Trail: false + robot0/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: 206 + Min Color: 0; 0; 0 + Min Intensity: 56 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot0/point_cloud2 + 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.1908152103424072 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.8516830801963806 + Y: 2.4098002910614014 + Z: 0.6427043676376343 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7103978991508484 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.010410556569695473 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 899 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001de000002e3fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002e3000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000021e000001040000000000000000000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002e3000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000069d0000003efc0100000002fb0000000800540069006d006501000000000000069d0000028e00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a4000002e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1693 + X: 928 + Y: 451 diff --git a/go2_robot_sdk/config/joystick.yaml b/go2_robot_sdk/config/joystick.yaml index a5adb05..aafc9a1 100644 --- a/go2_robot_sdk/config/joystick.yaml +++ b/go2_robot_sdk/config/joystick.yaml @@ -1,29 +1,4 @@ -joy_node: +/joy_node: ros__parameters: device_id: 0 - deadzone: 0.05 - autorepeat_rate: 20.0 - -teleop_node: - ros__parameters: - axis_linear: - x: 1 - y: 0 - scale_linear: - x: 0.4 - y: 0.4 - scale_linear_turbo: - x: 1.0 - y: 1.0 - - axis_angular: - yaw: 2 - scale_angular: - yaw: 1.5 - scale_angular_turbo: - yaw: 1.5 - - - enable_button: 6 - enable_turbo_button: 7 - require_enable_button: true \ No newline at end of file + deadzone: 0.05 \ No newline at end of file diff --git a/go2_robot_sdk/config/conf.rviz b/go2_robot_sdk/config/multi_robot_conf.rviz similarity index 77% rename from go2_robot_sdk/config/conf.rviz rename to go2_robot_sdk/config/multi_robot_conf.rviz index d39fc8e..4984b04 100644 --- a/go2_robot_sdk/config/conf.rviz +++ b/go2_robot_sdk/config/multi_robot_conf.rviz @@ -5,13 +5,8 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /Status1 - - /PointCloud21/Topic1 - - /LaserScan1/Topic1 - - /Image1 - - /Image1/Topic1 Splitter Ratio: 0.5 - Tree Height: 519 + Tree Height: 252 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -61,166 +56,171 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: /robot0/robot_description Enabled: true Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false - FL_calf: + Link Tree Style: Links in Alphabetic Order + map: Alpha: 1 Show Axes: false Show Trail: false - Value: true - FL_calflower: + odom: + Alpha: 1 + Show Axes: false + Show Trail: false + robot0/FL_calf: Alpha: 1 Show Axes: false Show Trail: false Value: true - FL_calflower1: + robot0/FL_calflower: Alpha: 1 Show Axes: false Show Trail: false Value: true - FL_foot: + robot0/FL_calflower1: Alpha: 1 Show Axes: false Show Trail: false Value: true - FL_hip: + robot0/FL_foot: Alpha: 1 Show Axes: false Show Trail: false Value: true - FL_thigh: + robot0/FL_hip: Alpha: 1 Show Axes: false Show Trail: false Value: true - FR_calf: + robot0/FL_thigh: Alpha: 1 Show Axes: false Show Trail: false Value: true - FR_calflower: + robot0/FR_calf: Alpha: 1 Show Axes: false Show Trail: false Value: true - FR_calflower1: + robot0/FR_calflower: Alpha: 1 Show Axes: false Show Trail: false Value: true - FR_foot: + robot0/FR_calflower1: Alpha: 1 Show Axes: false Show Trail: false Value: true - FR_hip: + robot0/FR_foot: Alpha: 1 Show Axes: false Show Trail: false Value: true - FR_thigh: + robot0/FR_hip: Alpha: 1 Show Axes: false Show Trail: false Value: true - Head_lower: + robot0/FR_thigh: Alpha: 1 Show Axes: false Show Trail: false Value: true - Head_upper: + robot0/Head_lower: Alpha: 1 Show Axes: false Show Trail: false Value: true - Link Tree Style: Links in Alphabetic Order - RL_calf: + robot0/Head_upper: Alpha: 1 Show Axes: false Show Trail: false Value: true - RL_calflower: + robot0/RL_calf: Alpha: 1 Show Axes: false Show Trail: false Value: true - RL_calflower1: + robot0/RL_calflower: Alpha: 1 Show Axes: false Show Trail: false Value: true - RL_foot: + robot0/RL_calflower1: Alpha: 1 Show Axes: false Show Trail: false Value: true - RL_hip: + robot0/RL_foot: Alpha: 1 Show Axes: false Show Trail: false Value: true - RL_thigh: + robot0/RL_hip: Alpha: 1 Show Axes: false Show Trail: false Value: true - RR_calf: + robot0/RL_thigh: Alpha: 1 Show Axes: false Show Trail: false Value: true - RR_calflower: + robot0/RR_calf: Alpha: 1 Show Axes: false Show Trail: false Value: true - RR_calflower1: + robot0/RR_calflower: Alpha: 1 Show Axes: false Show Trail: false Value: true - RR_foot: + robot0/RR_calflower1: Alpha: 1 Show Axes: false Show Trail: false Value: true - RR_hip: + robot0/RR_foot: Alpha: 1 Show Axes: false Show Trail: false Value: true - RR_thigh: + robot0/RR_hip: Alpha: 1 Show Axes: false Show Trail: false Value: true - base: + robot0/RR_thigh: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_footprint: + robot0/base_footprint: Alpha: 1 Show Axes: false Show Trail: false - imu: + robot0/base_link: Alpha: 1 Show Axes: false Show Trail: false - map: + Value: true + robot0/front_camera: Alpha: 1 Show Axes: false Show Trail: false - odom: + Value: true + robot0/imu: Alpha: 1 Show Axes: false Show Trail: false - radar: + robot0/radar: Alpha: 1 Show Axes: false Show Trail: false @@ -242,50 +242,16 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 186 - Min Color: 0; 0; 0 - Min Intensity: 36 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /point_cloud2 - Use Fixed Frame: true - Use rainbow: true - Value: false - - 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/LaserScan - Color: 224; 27; 36 - Color Transformer: FlatColor + Color Transformer: "" Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ + Name: PointCloud2 + Position Transformer: "" Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 @@ -295,11 +261,11 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Best Effort - Value: /scan + Reliability Policy: Reliable + Value: /robot0/point_cloud2 Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -383,12 +349,12 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1179 + Height: 899 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000252000003fdfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000292000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d5000001650000002800ffffff000000010000010f000003fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003fd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007b20000003efc0100000002fb0000000800540069006d00650100000000000007b2000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000445000003fd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001de000002e3fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000188000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000021e000001040000000000000000fb0000000a0049006d00610067006501000001cc000001550000002800ffffff000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002e3000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000069d0000003efc0100000002fb0000000800540069006d006501000000000000069d0000028e00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a4000002e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -397,6 +363,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1970 - X: 275 - Y: 118 + Width: 1693 + X: 1637 + Y: 189 diff --git a/go2_robot_sdk/config/twist_mux.yaml b/go2_robot_sdk/config/twist_mux.yaml index 48f6989..9ccad43 100644 --- a/go2_robot_sdk/config/twist_mux.yaml +++ b/go2_robot_sdk/config/twist_mux.yaml @@ -1,11 +1,25 @@ -twist_mux: +/teleop_node: + ros__parameters: + axis_linear: + x: 1 + y: 0 + z: 3 + scale_linear: + x: 0.4 + y: 0.4 + z: 1.0 + + require_enable_button: false + + +/twist_mux: ros__parameters: topics: - navigation: - topic : cmd_vel + joy: + topic : cmd_vel_joy timeout : 0.5 priority: 10 - joystick: - topic : cmd_vel_joy + navigation: + topic : cmd_vel timeout : 0.5 - priority: 90 \ No newline at end of file + priority: 15 \ No newline at end of file diff --git a/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py b/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py index 6c84efb..836774b 100644 --- a/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py +++ b/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py @@ -39,11 +39,12 @@ from rclpy.qos import QoSProfile from tf2_ros import TransformBroadcaster, TransformStamped -from geometry_msgs.msg import Twist, TransformStamped +from geometry_msgs.msg import Twist, TransformStamped, PoseStamped from go2_interfaces.msg import Go2State, IMU +from unitree_go.msg import LowState from sensor_msgs.msg import PointCloud2, PointField, JointState, Joy from sensor_msgs_py import point_cloud2 -from std_msgs.msg import Header, String +from std_msgs.msg import Header from nav_msgs.msg import Odometry @@ -59,259 +60,327 @@ def __init__(self): self.declare_parameter('robot_ip', os.getenv('ROBOT_IP', os.getenv('GO2_IP'))) self.declare_parameter('token', os.getenv('ROBOT_TOKEN', os.getenv('GO2_TOKEN',''))) + self.declare_parameter('conn_type', os.getenv('CONN_TYPE', os.getenv('CONN_TYPE',''))) self.robot_ip = self.get_parameter('robot_ip').get_parameter_value().string_value self.token = self.get_parameter('token').get_parameter_value().string_value + self.robot_ip_lst = self.robot_ip.replace(" ", "").split(",") + self.conn_type = self.get_parameter('conn_type').get_parameter_value().string_value - self.conn = None + self.get_logger().info(f"Received ip list: {self.robot_ip_lst}") + self.get_logger().info(f"Connection type is {self.conn_type}") + + self.conn = {} qos_profile = QoSProfile(depth=10) - self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile) - self.go2_state_pub = self.create_publisher(Go2State, 'go2_states', qos_profile) - self.go2_lidar_pub = self.create_publisher(PointCloud2, 'point_cloud2', qos_profile) - self.go2_odometry_pub = self.create_publisher(Odometry, 'odom', qos_profile) + + self.joint_pub = [] + self.go2_state_pub = [] + self.go2_lidar_pub = [] + self.go2_odometry_pub = [] + self.imu_pub = [] + + for i in range(len(self.robot_ip_lst)): + self.joint_pub.append(self.create_publisher(JointState, f'robot{i}/joint_states', qos_profile)) + self.go2_state_pub.append(self.create_publisher(Go2State, f'robot{i}/go2_states', qos_profile)) + self.go2_lidar_pub.append(self.create_publisher(PointCloud2, f'robot{i}/point_cloud2', qos_profile)) + self.go2_odometry_pub.append(self.create_publisher(Odometry, f'robot{i}/odom', qos_profile)) + self.imu_pub.append(self.create_publisher(IMU, f'robot{i}/imu', qos_profile)) - self.imu_pub = self.create_publisher(IMU, 'imu', qos_profile) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) - self.robot_cmd_vel = None - self.robot_odom = None - self.robot_low_cmd = None - self.robot_sport_state = None - self.robot_lidar = None - self.robot_command_queue = [] - self.joy_state = Joy() - - self.cmd_vel_sub = self.create_subscription( - Twist, - 'cmd_vel', - self.cmd_vel_cb, - qos_profile) + self.robot_cmd_vel = {} + self.robot_odom = {} + self.robot_low_cmd = {} + self.robot_sport_state = {} + self.robot_lidar = {} - self.joy_sub = self.create_subscription( + self.joy_state = Joy() + + for i in range(len(self.robot_ip_lst)): + self.create_subscription( + Twist, + f'robot{str(i)}/cmd_vel', + lambda msg: self.cmd_vel_cb(msg, str(i)), + qos_profile) + + self.create_subscription( Joy, 'joy', self.joy_cb, qos_profile) - self.command_sub = self.create_subscription( - String, - 'command', - self.command_cb, - qos_profile) - + # Support for CycloneDDS (EDU version via ethernet) + if self.conn_type == 'cyclonedds': + self.create_subscription( + LowState, + 'lowstate', + self.publish_joint_state_cyclonedds, + qos_profile) + + self.create_subscription( + PoseStamped, + '/utlidar/robot_pose', + self.publish_body_poss_cyclonedds, + qos_profile) + + self.create_subscription( + PointCloud2, + '/utlidar/cloud', + self.publish_lidar_cyclonedds, + qos_profile) + self.timer = self.create_timer(0.1, self.timer_callback) self.timer_lidar = self.create_timer(0.5, self.timer_callback_lidar) def timer_callback(self): - self.publish_odom() - self.publish_odom_topic() - self.publish_robot_state() - self.publish_joint_state() + if self.conn_type == 'webrtc': + self.publish_odom_webrtc() + self.publish_odom_topic_webrtc() + self.publish_robot_state_webrtc() + self.publish_joint_state_webrtc() def timer_callback_lidar(self): - self.publish_lidar() + if self.conn_type == 'webrtc': + self.publish_lidar_webrtc() - def cmd_vel_cb(self, msg): + def cmd_vel_cb(self, msg, robot_num): x = msg.linear.x y = msg.linear.y - z = msg.angular.z - if x > 0.0 or y > 0.0 or z > 0.0: - self.robot_cmd_vel = gen_mov_command(x, y, z) + z = msg.linear.z + + if x > 0.0 or y > 0.0 or z != 0.0: + self.robot_cmd_vel[robot_num] = gen_mov_command(round(x, 2), round(y, 2), round(z, 2)) def joy_cb(self, msg): self.joy_state = msg - def command_cb(self, msg): - self.get_logger().info(f"Received command: {msg.data}") - self.robot_command_queue.append(msg) - - def joy_cmd(self): - if self.robot_cmd_vel: - self.get_logger().info("Attack!") - self.conn.data_channel.send(self.robot_cmd_vel) - self.robot_cmd_vel = None - - if len(self.robot_command_queue) > 0: - msg = self.robot_command_queue[0] - robot_cmd = gen_command(ROBOT_CMD[msg.data]) - self._logger.info(f"Sending command: {ROBOT_CMD[msg.data]}") - self.conn.data_channel.send(robot_cmd) - self.robot_command_queue.pop(0) - - if self.joy_state.buttons and self.joy_state.buttons[1]: - self.get_logger().info("Stand down") - stand_down_cmd = gen_command(ROBOT_CMD["StandDown"]) - self.conn.data_channel.send(stand_down_cmd) - - if self.joy_state.buttons and self.joy_state.buttons[0]: - self.get_logger().info("Stand up") - stand_up_cmd = gen_command(ROBOT_CMD["StandUp"]) - self.conn.data_channel.send(stand_up_cmd) - balance_stand_cmd = gen_command(ROBOT_CMD['BalanceStand']) - self.conn.data_channel.send(balance_stand_cmd) - - def on_validated(self): - for topic in RTC_TOPIC.values(): - self.conn.data_channel.send(json.dumps({"type": "subscribe", "topic": topic})) + def publish_body_poss_cyclonedds(self, msg): + odom_trans = TransformStamped() + odom_trans.header.stamp = self.get_clock().now().to_msg() + odom_trans.header.frame_id = 'odom' + odom_trans.child_frame_id = f"robot0/base_link" + odom_trans.transform.translation.x = msg.pose.position.x + odom_trans.transform.translation.y = msg.pose.position.y + odom_trans.transform.translation.z = msg.pose.position.z + 0.07 + odom_trans.transform.rotation.x = msg.pose.orientation.x + odom_trans.transform.rotation.y = msg.pose.orientation.y + odom_trans.transform.rotation.z = msg.pose.orientation.z + odom_trans.transform.rotation.w = msg.pose.orientation.w + self.broadcaster.sendTransform(odom_trans) - def on_data_channel_message(self, _, msg): + def publish_joint_state_cyclonedds(self, msg): + joint_state = JointState() + joint_state.header.stamp = self.get_clock().now().to_msg() + joint_state.name = [ + f'robot0/FL_hip_joint', f'robot0/FL_thigh_joint', f'robot0/FL_calf_joint', + f'robot0/FR_hip_joint', f'robot0/FR_thigh_joint', f'robot0/FR_calf_joint', + f'robot0/RL_hip_joint', f'robot0/RL_thigh_joint', f'robot0/RL_calf_joint', + f'robot0/RR_hip_joint', f'robot0/RR_thigh_joint', f'robot0/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, + ] + self.joint_pub[0].publish(joint_state) + + def publish_lidar_cyclonedds(self, msg): + msg.header = Header(frame_id="robot0/radar") + msg.header.stamp = self.get_clock().now().to_msg() + self.go2_lidar_pub[0].publish(msg) + + def joy_cmd(self, robot_num): + + if self.conn_type == 'webrtc': + if robot_num in self.conn and robot_num in self.robot_cmd_vel and self.robot_cmd_vel[robot_num] != None: + self.get_logger().info("Move") + self.conn[robot_num].data_channel.send(self.robot_cmd_vel[robot_num]) + self.robot_cmd_vel[robot_num] = None + + if robot_num in self.conn and self.joy_state.buttons and self.joy_state.buttons[1]: + self.get_logger().info("Stand down") + stand_down_cmd = gen_command(ROBOT_CMD["StandDown"]) + self.conn[robot_num].data_channel.send(stand_down_cmd) + + if robot_num in self.conn and self.joy_state.buttons and self.joy_state.buttons[0]: + self.get_logger().info("Stand up") + stand_up_cmd = gen_command(ROBOT_CMD["StandUp"]) + self.conn[robot_num].data_channel.send(stand_up_cmd) + move_cmd = gen_command(ROBOT_CMD['BalanceStand']) + self.conn[robot_num].data_channel.send(move_cmd) + + def on_validated(self, robot_num): + if robot_num in self.conn: + for topic in RTC_TOPIC.values(): + self.conn[robot_num].data_channel.send(json.dumps({"type": "subscribe", "topic": topic})) + def on_data_channel_message(self, _, msg, robot_num): + if msg.get('topic') == RTC_TOPIC["ULIDAR_ARRAY"]: - self.robot_lidar = msg + self.robot_lidar[robot_num] = msg if msg.get('topic') == RTC_TOPIC['ROBOTODOM']: - self.robot_odom = msg + self.robot_odom[robot_num] = msg if msg.get('topic') == RTC_TOPIC['LF_SPORT_MOD_STATE']: - self.robot_sport_state = msg + self.robot_sport_state[robot_num] = msg if msg.get('topic') == RTC_TOPIC['LOW_STATE']: - self.robot_low_cmd = msg - - def publish_odom(self): - if self.robot_odom: - odom_trans = TransformStamped() - odom_trans.header.stamp = self.get_clock().now().to_msg() - odom_trans.header.frame_id = 'odom' - odom_trans.child_frame_id = 'base_link' - odom_trans.transform.translation.x = self.robot_odom['data']['pose']['position']['x'] - odom_trans.transform.translation.y = self.robot_odom['data']['pose']['position']['y'] - odom_trans.transform.translation.z = self.robot_odom['data']['pose']['position']['z'] + 0.07 - odom_trans.transform.rotation.x = self.robot_odom['data']['pose']['orientation']['x'] - odom_trans.transform.rotation.y = self.robot_odom['data']['pose']['orientation']['y'] - odom_trans.transform.rotation.z = self.robot_odom['data']['pose']['orientation']['z'] - odom_trans.transform.rotation.w = self.robot_odom['data']['pose']['orientation']['w'] - self.broadcaster.sendTransform(odom_trans) + self.robot_low_cmd[robot_num] = msg + + def publish_odom_webrtc(self): + for i in range(len(self.robot_odom)): + if self.robot_odom[str(i)]: + odom_trans = TransformStamped() + odom_trans.header.stamp = self.get_clock().now().to_msg() + odom_trans.header.frame_id = 'odom' + odom_trans.child_frame_id = f"robot{str(i)}/base_link" + odom_trans.transform.translation.x = self.robot_odom[str(i)]['data']['pose']['position']['x'] + odom_trans.transform.translation.y = self.robot_odom[str(i)]['data']['pose']['position']['y'] + odom_trans.transform.translation.z = self.robot_odom[str(i)]['data']['pose']['position']['z'] + 0.07 + odom_trans.transform.rotation.x = self.robot_odom[str(i)]['data']['pose']['orientation']['x'] + odom_trans.transform.rotation.y = self.robot_odom[str(i)]['data']['pose']['orientation']['y'] + odom_trans.transform.rotation.z = self.robot_odom[str(i)]['data']['pose']['orientation']['z'] + odom_trans.transform.rotation.w = self.robot_odom[str(i)]['data']['pose']['orientation']['w'] + self.broadcaster.sendTransform(odom_trans) - def publish_odom_topic(self): - if self.robot_odom: - odom_msg = Odometry() - odom_msg.header.stamp = self.get_clock().now().to_msg() - odom_msg.header.frame_id = 'odom' - odom_msg.child_frame_id = 'base_link' - odom_msg.pose.pose.position.x = self.robot_odom['data']['pose']['position']['x'] - odom_msg.pose.pose.position.y = self.robot_odom['data']['pose']['position']['y'] - odom_msg.pose.pose.position.z = self.robot_odom['data']['pose']['position']['z'] + 0.07 - odom_msg.pose.pose.orientation.x = self.robot_odom['data']['pose']['orientation']['x'] - odom_msg.pose.pose.orientation.y = self.robot_odom['data']['pose']['orientation']['y'] - odom_msg.pose.pose.orientation.z = self.robot_odom['data']['pose']['orientation']['z'] - odom_msg.pose.pose.orientation.w = self.robot_odom['data']['pose']['orientation']['w'] - self.go2_odometry_pub.publish(odom_msg) - - def publish_lidar(self): - if self.robot_lidar: - points = update_meshes_for_cloud2( - self.robot_lidar["decoded_data"]["positions"], - self.robot_lidar["decoded_data"]["uvs"], - self.robot_lidar['data']['resolution'], - self.robot_lidar['data']['origin'], - 0 - ) - point_cloud = PointCloud2() - point_cloud.header = Header(frame_id="odom") - point_cloud.header.stamp = self.get_clock().now().to_msg() - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1), - ] - point_cloud = point_cloud2.create_cloud(point_cloud.header, fields, points) - self.go2_lidar_pub.publish(point_cloud) - - def publish_joint_state(self): - if self.robot_sport_state: - joint_state = JointState() - joint_state.header.stamp = self.get_clock().now().to_msg() - - fl_foot_pos_array = [ - self.robot_sport_state["data"]["foot_position_body"][3], - self.robot_sport_state["data"]["foot_position_body"][4], - self.robot_sport_state["data"]["foot_position_body"][5] - ] - - FL_hip_joint, FL_thigh_joint, FL_calf_joint = get_robot_joints( - fl_foot_pos_array, - 0 - ) - - fr_foot_pos_array = [ - self.robot_sport_state["data"]["foot_position_body"][0], - self.robot_sport_state["data"]["foot_position_body"][1], - self.robot_sport_state["data"]["foot_position_body"][2] - ] - - FR_hip_joint, FR_thigh_joint, FR_calf_joint = get_robot_joints( - fr_foot_pos_array, - 1 - ) - - rl_foot_pos_array = [ - self.robot_sport_state["data"]["foot_position_body"][9], - self.robot_sport_state["data"]["foot_position_body"][10], - self.robot_sport_state["data"]["foot_position_body"][11] - ] + def publish_odom_topic_webrtc(self): + for i in range(len(self.robot_odom)): + if self.robot_odom[str(i)]: + odom_msg = Odometry() + odom_msg.header.stamp = self.get_clock().now().to_msg() + odom_msg.header.frame_id = 'odom' + odom_msg.child_frame_id = f"robot{str(i)}/base_link" + odom_msg.pose.pose.position.x = self.robot_odom[str(i)]['data']['pose']['position']['x'] + odom_msg.pose.pose.position.y = self.robot_odom[str(i)]['data']['pose']['position']['y'] + odom_msg.pose.pose.position.z = self.robot_odom[str(i)]['data']['pose']['position']['z'] + 0.07 + odom_msg.pose.pose.orientation.x = self.robot_odom[str(i)]['data']['pose']['orientation']['x'] + odom_msg.pose.pose.orientation.y = self.robot_odom[str(i)]['data']['pose']['orientation']['y'] + odom_msg.pose.pose.orientation.z = self.robot_odom[str(i)]['data']['pose']['orientation']['z'] + odom_msg.pose.pose.orientation.w = self.robot_odom[str(i)]['data']['pose']['orientation']['w'] + self.go2_odometry_pub[i].publish(odom_msg) + + def publish_lidar_webrtc(self): + for i in range(len(self.robot_lidar)): + if self.robot_lidar[str(i)]: + points = update_meshes_for_cloud2( + self.robot_lidar[str(i)]["decoded_data"]["positions"], + self.robot_lidar[str(i)]["decoded_data"]["uvs"], + self.robot_lidar[str(i)]['data']['resolution'], + self.robot_lidar[str(i)]['data']['origin'], + 0 + ) + point_cloud = PointCloud2() + point_cloud.header = Header(frame_id="odom") + point_cloud.header.stamp = self.get_clock().now().to_msg() + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1), + ] + point_cloud = point_cloud2.create_cloud(point_cloud.header, fields, points) + self.go2_lidar_pub[i].publish(point_cloud) + + def publish_joint_state_webrtc(self): + for i in range(len(self.robot_sport_state)): + if self.robot_sport_state[str(i)]: + joint_state = JointState() + joint_state.header.stamp = self.get_clock().now().to_msg() + + fl_foot_pos_array = [ + self.robot_sport_state[str(i)]["data"]["foot_position_body"][3], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][4], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][5] + ] - RL_hip_joint, RL_thigh_joint, RL_calf_joint = get_robot_joints( - rl_foot_pos_array, - 2 - ) - - rr_foot_pos_array = [ - self.robot_sport_state["data"]["foot_position_body"][6], - self.robot_sport_state["data"]["foot_position_body"][7], - self.robot_sport_state["data"]["foot_position_body"][8] - ] + FL_hip_joint, FL_thigh_joint, FL_calf_joint = get_robot_joints( + fl_foot_pos_array, + 0 + ) + + fr_foot_pos_array = [ + self.robot_sport_state[str(i)]["data"]["foot_position_body"][0], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][1], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][2] + ] - RR_hip_joint, RR_thigh_joint, RR_calf_joint = get_robot_joints( - rr_foot_pos_array, - 3 - ) - - 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', + FR_hip_joint, FR_thigh_joint, FR_calf_joint = get_robot_joints( + fr_foot_pos_array, + 1 + ) + + rl_foot_pos_array = [ + self.robot_sport_state[str(i)]["data"]["foot_position_body"][9], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][10], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][11] ] - joint_state.position = [ - 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, + + RL_hip_joint, RL_thigh_joint, RL_calf_joint = get_robot_joints( + rl_foot_pos_array, + 2 + ) + + rr_foot_pos_array = [ + self.robot_sport_state[str(i)]["data"]["foot_position_body"][6], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][7], + self.robot_sport_state[str(i)]["data"]["foot_position_body"][8] ] - self.joint_pub.publish(joint_state) - - def publish_robot_state(self): - if self.robot_sport_state: - go2_state = Go2State() - go2_state.mode = self.robot_sport_state["data"]["mode"] - go2_state.progress = self.robot_sport_state["data"]["progress"] - go2_state.gait_type = self.robot_sport_state["data"]["gait_type"] - go2_state.position = list(map(float,self.robot_sport_state["data"]["position"])) - go2_state.body_height = float(self.robot_sport_state["data"]["body_height"]) - go2_state.velocity = self.robot_sport_state["data"]["velocity"] - go2_state.range_obstacle = list(map(float, self.robot_sport_state["data"]["range_obstacle"])) - go2_state.foot_force = self.robot_sport_state["data"]["foot_force"] - go2_state.foot_position_body = list(map(float,self.robot_sport_state["data"]["foot_position_body"])) - go2_state.foot_speed_body = list(map(float, self.robot_sport_state["data"]["foot_speed_body"])) - self.go2_state_pub.publish(go2_state) - - imu = IMU() - imu.quaternion = list(map(float,self.robot_sport_state["data"]["imu_state"]["quaternion"])) - imu.accelerometer = list(map(float,self.robot_sport_state["data"]["imu_state"]["accelerometer"])) - imu.gyroscope = list(map(float,self.robot_sport_state["data"]["imu_state"]["gyroscope"])) - imu.rpy = list(map(float,self.robot_sport_state["data"]["imu_state"]["rpy"])) - imu.temperature = self.robot_sport_state["data"]["imu_state"]["temperature"] - self.imu_pub.publish(imu) - - async def run(self, conn): - self.conn = conn - await self.conn.connect() - self.get_logger().info(f"Connected to {os.environ.get('ROBOT_IP')}") + + RR_hip_joint, RR_thigh_joint, RR_calf_joint = get_robot_joints( + rr_foot_pos_array, + 3 + ) + + joint_state.name = [ + f'robot{str(i)}/FL_hip_joint', f'robot{str(i)}/FL_thigh_joint', f'robot{str(i)}/FL_calf_joint', + f'robot{str(i)}/FR_hip_joint', f'robot{str(i)}/FR_thigh_joint', f'robot{str(i)}/FR_calf_joint', + f'robot{str(i)}/RL_hip_joint', f'robot{str(i)}/RL_thigh_joint', f'robot{str(i)}/RL_calf_joint', + f'robot{str(i)}/RR_hip_joint', f'robot{str(i)}/RR_thigh_joint', f'robot{str(i)}/RR_calf_joint', + ] + joint_state.position = [ + 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, + ] + self.joint_pub[i].publish(joint_state) + + + + def publish_robot_state_webrtc(self): + for i in range(len(self.robot_sport_state)): + if self.robot_sport_state[str(i)]: + go2_state = Go2State() + go2_state.mode = self.robot_sport_state[str(i)]["data"]["mode"] + go2_state.progress = self.robot_sport_state[str(i)]["data"]["progress"] + go2_state.gait_type = self.robot_sport_state[str(i)]["data"]["gait_type"] + go2_state.position = list(map(float,self.robot_sport_state[str(i)]["data"]["position"])) + go2_state.body_height = float(self.robot_sport_state[str(i)]["data"]["body_height"]) + go2_state.velocity = self.robot_sport_state[str(i)]["data"]["velocity"] + go2_state.range_obstacle = list(map(float, self.robot_sport_state[str(i)]["data"]["range_obstacle"])) + go2_state.foot_force = self.robot_sport_state[str(i)]["data"]["foot_force"] + go2_state.foot_position_body = list(map(float,self.robot_sport_state[str(i)]["data"]["foot_position_body"])) + go2_state.foot_speed_body = list(map(float, self.robot_sport_state[str(i)]["data"]["foot_speed_body"])) + self.go2_state_pub[i].publish(go2_state) + + imu = IMU() + imu.quaternion = list(map(float,self.robot_sport_state[str(i)]["data"]["imu_state"]["quaternion"])) + imu.accelerometer = list(map(float,self.robot_sport_state[str(i)]["data"]["imu_state"]["accelerometer"])) + imu.gyroscope = list(map(float,self.robot_sport_state[str(i)]["data"]["imu_state"]["gyroscope"])) + imu.rpy = list(map(float,self.robot_sport_state[str(i)]["data"]["imu_state"]["rpy"])) + imu.temperature = self.robot_sport_state[str(i)]["data"]["imu_state"]["temperature"] + self.imu_pub[i].publish(imu) + + async def run(self, conn, robot_num): + self.conn[robot_num] = conn + + if self.conn_type == 'webrtc': + await self.conn[robot_num].connect() while True: - self.joy_cmd() + self.joy_cmd(robot_num) await asyncio.sleep(0.1) @@ -338,16 +407,22 @@ def _spin(node: Node, async def start_node(): base_node = RobotBaseNode() - conn = Go2Connection( - robot_ip=base_node.robot_ip, + spin_task = asyncio.get_event_loop().create_task(spin(base_node)) + sleep_task_lst = [] + + for i in range(len(base_node.robot_ip_lst)): + + conn = Go2Connection( + robot_ip=base_node.robot_ip_lst[i], + robot_num=str(i), token=base_node.token, on_validated=base_node.on_validated, on_message=base_node.on_data_channel_message, + ) + + sleep_task_lst.append(asyncio.get_event_loop().create_task(base_node.run(conn, str(i)))) - ) - spin_task = asyncio.get_event_loop().create_task(spin(base_node)) - sleep_task = asyncio.get_event_loop().create_task(base_node.run(conn)) - await asyncio.wait([spin_task, sleep_task], return_when=asyncio.FIRST_COMPLETED) + await asyncio.wait([spin_task, *sleep_task_lst], return_when=asyncio.FIRST_COMPLETED) def main(): rclpy.init() diff --git a/go2_robot_sdk/launch/robot.launch.py b/go2_robot_sdk/launch/robot.launch.py index ac6fff2..70bccf7 100644 --- a/go2_robot_sdk/launch/robot.launch.py +++ b/go2_robot_sdk/launch/robot.launch.py @@ -25,23 +25,29 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.conditions import UnlessCondition -from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource +from launch.launch_description_sources import FrontendLaunchDescriptionSource def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') no_rviz2 = LaunchConfiguration('no_rviz2', default='false') - robot_ip = LaunchConfiguration('robot_ip', default=os.getenv('ROBOT_IP')) - robot_token = LaunchConfiguration('robot_token', default=os.getenv('ROBOT_TOKEN','')) + + robot_token = os.getenv('ROBOT_TOKEN','') + robot_ip = os.getenv('ROBOT_IP', '') + robot_ip_lst = robot_ip.replace(" ", "").split(",") + conn_type = os.getenv('CONN_TYPE', 'webrtc') + rviz_config = "multi_robot_conf.rviz" + + if conn_type == 'cyclonedds': + rviz_config = "cyclonedds_config.rviz" - urdf_file_name = 'go2.urdf' + urdf_file_name = 'multi_go2.urdf' urdf = os.path.join( get_package_share_directory('go2_robot_sdk'), "urdf", @@ -49,138 +55,140 @@ def generate_launch_description(): with open(urdf, 'r') as infp: robot_desc = infp.read() - - foxglove_launch = os.path.join( - get_package_share_directory('foxglove_bridge'), - 'launch', - 'foxglove_bridge_launch.xml', - ) + robot_desc_modified_lst = [] - if not os.path.exists(foxglove_launch): - foxglove_launch = os.path.join( - get_package_share_directory('foxglove_bridge'), - 'foxglove_bridge_launch.xml' - ) + for i in range(len(robot_ip_lst)): + robot_desc_modified_lst.append(robot_desc.format(robot_num=f"robot{i}")) + + urdf_launch_nodes = [] joy_params = os.path.join( get_package_share_directory('go2_robot_sdk'), 'config', 'joystick.yaml' ) - default_config_topics = os.path.join(get_package_share_directory('go2_robot_sdk'), - 'config', 'twist_mux.yaml') - - slam_toolbox_config = os.path.join( + default_config_topics = os.path.join( get_package_share_directory('go2_robot_sdk'), - 'config', - 'mapper_params_online_async.yaml' - ) - - nav2_config = os.path.join( - get_package_share_directory('go2_robot_sdk'), - 'config', - 'nav2_params.yaml' - ) + 'config', 'twist_mux.yaml') - return LaunchDescription([ - - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'config_topics', - default_value=default_config_topics, - description='Default topics config file'), - - DeclareLaunchArgument( - 'cmd_vel_out', - default_value='cmd_vel', - description='cmd vel output topic'), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource(foxglove_launch) - ), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - os.path.join(get_package_share_directory('slam_toolbox'), 'launch', 'online_async_launch.py') - ]), - launch_arguments={ - 'params_file': slam_toolbox_config, - 'use_sim_time': use_sim_time, - }.items(), - ), + foxglove_launch = os.path.join( + get_package_share_directory('foxglove_bridge'), + 'launch', + 'foxglove_bridge_launch.xml', + ) - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - os.path.join(get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py') - ]), - launch_arguments={ - 'params_file': nav2_config, - 'use_sim_time': use_sim_time, - }.items(), - ), + # TODO Need to fix Nav2 + # slam_toolbox_config = os.path.join( + # get_package_share_directory('go2_robot_sdk'), + # 'config', + # 'mapper_params_online_async.yaml' + # ) + + # nav2_config = os.path.join( + # get_package_share_directory('go2_robot_sdk'), + # 'config', + # 'nav2_params.yaml' + # ) + + for i in range(len(robot_ip_lst)): + urdf_launch_nodes.append( + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + namespace=f"robot{i}", + parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc_modified_lst[i]}], + arguments=[urdf] + ), + ) + urdf_launch_nodes.append( + Node( + package='ros2_go2_video', + executable='ros2_go2_video', + parameters=[{'robot_ip': robot_ip_lst[i], 'robot_token': robot_token}], + ), + ) - Node( + urdf_launch_nodes.append( + Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', name='pointcloud_to_laserscan', remappings=[ - ('cloud_in', '/point_cloud2'), + ('cloud_in', f'robot{i}/point_cloud2'), ], parameters=[{ - 'target_frame': 'base_link', + 'target_frame': f'robot{i}/base_link', }] ), + ) + + return LaunchDescription([ + *urdf_launch_nodes, 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_desc}], - arguments=[urdf]), + package='go2_robot_sdk', + executable='go2_driver_node', + parameters=[{'robot_ip': robot_ip, 'token': robot_token, "conn_type": conn_type}], + ), + Node( + package='rviz2', + namespace='', + executable='rviz2', + condition=UnlessCondition(no_rviz2), + name='rviz2', + arguments=['-d' + os.path.join(get_package_share_directory('go2_robot_sdk'), 'config', rviz_config)] + ), + Node( package='joy', executable='joy_node', - parameters=[joy_params]), + parameters=[joy_params] + ), + Node( package='teleop_twist_joy', executable='teleop_node', name='teleop_node', - parameters=[joy_params], - remappings=[('/cmd_vel', '/cmd_vel_joy')],), + parameters=[default_config_topics], + remappings=[('/cmd_vel', '/cmd_vel_joy')] + ), + Node( package='twist_mux', executable='twist_mux', output='screen', - remappings={('/cmd_vel_out', LaunchConfiguration('cmd_vel_out'))}, parameters=[ - {'use_sim_time': LaunchConfiguration('use_sim_time')}, - LaunchConfiguration('config_topics')] + {'use_sim_time': use_sim_time}, + default_config_topics + ], + remappings=[('/cmd_vel_out', 'robot0/cmd_vel')] ), - Node( - package='go2_robot_sdk', - executable='go2_driver_node', - parameters=[{'robot_ip': robot_ip, 'token': robot_token}], - ), - Node( - package='go2_robot_sdk', - executable='go2_proc_text', - ), - Node( - package='ros2_go2_video', - executable='ros2_go2_video', - parameters=[{'robot_ip': robot_ip, 'robot_token': robot_token}], - ), - Node( - package='rviz2', - namespace='', - executable='rviz2', - condition=UnlessCondition(no_rviz2), - name='rviz2', - arguments=['-d' + os.path.join(get_package_share_directory('go2_robot_sdk'), 'config', 'conf.rviz')] - ) + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource(foxglove_launch) + ), + + + # TODO Need to fix Nav2 + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ + # os.path.join(get_package_share_directory('slam_toolbox'), 'launch', 'online_async_launch.py') + # ]), + # launch_arguments={ + # 'params_file': slam_toolbox_config, + # 'use_sim_time': use_sim_time, + # }.items(), + # ), + + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ + # os.path.join(get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py') + # ]), + # launch_arguments={ + # 'params_file': nav2_config, + # 'use_sim_time': use_sim_time, + # }.items(), + # ), ]) \ No newline at end of file diff --git a/go2_robot_sdk/launch/robot_on_steroids.launch.py b/go2_robot_sdk/launch/robot_on_steroids.launch.py deleted file mode 100644 index a67fb60..0000000 --- a/go2_robot_sdk/launch/robot_on_steroids.launch.py +++ /dev/null @@ -1,95 +0,0 @@ -# Copyright (c) 2024, RoboVerse community -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 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.conditions import UnlessCondition -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - - use_sim_time = LaunchConfiguration('use_sim_time', default='false') - no_rviz2 = LaunchConfiguration('no_rviz2', default='false') - robot_ip = LaunchConfiguration('robot_ip', default=os.getenv('ROBOT_IP')) - robot_token = LaunchConfiguration('robot_token', default=os.getenv('ROBOT_TOEKN','')) - - - urdf_file_name = 'go2_on_steroids.urdf' - urdf = os.path.join( - get_package_share_directory('go2_robot_sdk'), - "urdf", - urdf_file_name) - with open(urdf, 'r') as infp: - robot_desc = infp.read() - - - joy_params = os.path.join( - get_package_share_directory('go2_robot_sdk'), - 'config', 'joystick.yaml' - ) - - return LaunchDescription([ - - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true'), - 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_desc}], - arguments=[urdf]), - Node( - package='joy', - executable='joy_node', - parameters=[joy_params]), - Node( - package='teleop_twist_joy', - executable='teleop_node', - name='teleop_node', - parameters=[joy_params]), - Node( - package='go2_robot_sdk', - executable='go2_driver_node', - parameters=[{'robot_ip': robot_ip, 'token': robot_token}], - ), - Node( - package='ros2_go2_video', - executable='ros2_go2_video', - parameters=[{'robot_ip': robot_ip, 'robot_token': robot_token}], - ), - Node( - package='rviz2', - namespace='', - executable='rviz2', - condition=UnlessCondition(no_rviz2), - name='rviz2', - arguments=['-d' + os.path.join(get_package_share_directory('go2_robot_sdk'), 'config', 'conf.rviz')] - ) - ]) \ No newline at end of file diff --git a/go2_robot_sdk/launch/urdf.launch.py b/go2_robot_sdk/launch/urdf.launch.py deleted file mode 100644 index 982c393..0000000 --- a/go2_robot_sdk/launch/urdf.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - urdf_file = os.path.join( - get_package_share_directory("go2_robot_sdk"), "urdf", "go2.urdf" - ) - - return LaunchDescription( - [ - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[{"robot_description": open(urdf_file, "r").read()}], - ) - ] - ) diff --git a/go2_robot_sdk/scripts/webrtc_driver.py b/go2_robot_sdk/scripts/webrtc_driver.py index b6e6005..31bfc36 100644 --- a/go2_robot_sdk/scripts/webrtc_driver.py +++ b/go2_robot_sdk/scripts/webrtc_driver.py @@ -24,6 +24,7 @@ # WEB_RTC_COMM WAS ORIGINALY FORKED from https://github.com/tfoldi/go2-webrtc/tree/master # Big thanks for your passion! @tfoldi (Földi Tamás) +import time import aiohttp import base64 import hashlib @@ -49,16 +50,16 @@ class Go2Connection(): def __init__( self, robot_ip=None, + robot_num=None, token="", on_validated=None, on_message=None, on_open=None ): - - self.pc = RTCPeerConnection() self.robot_ip = robot_ip + self.robot_num = str(robot_num) self.token = token self.robot_validation = "PENDING" self.on_validated = on_validated @@ -117,35 +118,41 @@ def on_data_channel_message(self, msg): msgobj = Go2Connection.deal_array_buffer(msg) if self.on_message: - self.on_message(msg, msgobj) + self.on_message(msg, msgobj, self.robot_num) except json.JSONDecodeError: pass - + async def connect(self): offer = await self.generate_offer() - async with aiohttp.ClientSession() as session: - url = f"http://{self.robot_ip}:8081/offer" - headers = {"content-type": "application/json"} - data = { - "sdp": offer, - "id": "STA_localNetwork", - "type": "offer", - "token": self.token, - } - async with session.post(url, json=data, headers=headers) as resp: - if resp.status == 200: - answer_data = await resp.json() - answer_sdp = answer_data.get("sdp") - await self.set_answer(answer_sdp) - else: - logger.info("Failed to get answer from server") + url = f"http://{self.robot_ip}:8081/offer" + headers = {"Content-Type": "application/json"} + data = { + "sdp": offer, + "id": "STA_localNetwork", + "type": "offer", + "token": "", + } + + connected = False + while not connected: + async with aiohttp.ClientSession() as session: + async with session.post(url, json=data, headers=headers) as resp: + if resp.status == 200: + answer_data = await resp.json() + answer_sdp = answer_data.get("sdp") + await self.set_answer(answer_sdp) + connected = True + else: + logger.info(f"Failed to get answer from server: Reason: {resp}") + logger.info("Try to reconnect...") + time.sleep(1) def validate_robot_conn(self, message): if message.get("data") == "Validation Ok.": self.validation_result = "SUCCESS" if self.on_validated: - self.on_validated() + self.on_validated(self.robot_num) else: self.publish( "", diff --git a/go2_robot_sdk/urdf/multi_go2.urdf b/go2_robot_sdk/urdf/multi_go2.urdf new file mode 100644 index 0000000..f547c9a --- /dev/null +++ b/go2_robot_sdk/urdf/multi_go2.urdf @@ -0,0 +1,1277 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/unitree_go/CMakeLists.txt b/unitree_go/CMakeLists.txt new file mode 100644 index 0000000..9b55bdd --- /dev/null +++ b/unitree_go/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.8) +project(unitree_go) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. + + +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 # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg +) + + +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/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 100644 index 0000000..c2f73f7 --- /dev/null +++ b/unitree_go/package.xml @@ -0,0 +1,23 @@ + + + + unitree_go + 0.0.0 + TODO: Package description + Unitree + BSD 3-Clause License + + ament_cmake + geometry_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_lint_auto + ament_lint_common + + + ament_cmake + +