From 1737ec21506c930ef4566a534a8ef7b4f8fe8364 Mon Sep 17 00:00:00 2001
From: Tamas Foldi
Date: Mon, 20 May 2024 14:00:15 +0200
Subject: [PATCH 1/6] feat: publish camera info
---
go2_robot_sdk/calibration/front_camera.yaml | 26 +++++++++++++++++++
.../go2_robot_sdk/go2_driver_node.py | 23 +++++++++++++++-
go2_robot_sdk/setup.py | 1 +
requirements.txt | 3 ++-
4 files changed, 51 insertions(+), 2 deletions(-)
create mode 100644 go2_robot_sdk/calibration/front_camera.yaml
diff --git a/go2_robot_sdk/calibration/front_camera.yaml b/go2_robot_sdk/calibration/front_camera.yaml
new file mode 100644
index 0000000..36231fb
--- /dev/null
+++ b/go2_robot_sdk/calibration/front_camera.yaml
@@ -0,0 +1,26 @@
+image_width: 1920
+image_height: 1080
+camera_name: front_camera
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1310.77826, 0. , 1018.71143,
+ 1. , 1320.25059, 637.37672,
+ 1., 0. , 1. ]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.415971, 0.158898, -0.015395, -0.008031, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1., 0., 0.,
+ 0., 1., 0.,
+ 0., 0., 1.]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [992.26007, 0. , 996.08995, 0. ,
+ 1., 1221.72864, 639.55771, 0. ,
+ 1., 0. , 1. , 0. ]
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..2a72d6b 100644
--- a/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py
+++ b/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py
@@ -27,6 +27,7 @@
import os
import threading
import asyncio
+import yaml
from scripts.go2_constants import ROBOT_CMD, RTC_TOPIC
from scripts.go2_func import gen_command, gen_mov_command
@@ -39,9 +40,10 @@
from rclpy.qos import QoSProfile
from tf2_ros import TransformBroadcaster, TransformStamped
+from ament_index_python.packages import get_package_share_directory
from geometry_msgs.msg import Twist, TransformStamped
from go2_interfaces.msg import Go2State, IMU
-from sensor_msgs.msg import PointCloud2, PointField, JointState, Joy
+from sensor_msgs.msg import PointCloud2, PointField, JointState, Joy, CameraInfo
from sensor_msgs_py import point_cloud2
from std_msgs.msg import Header, String
from nav_msgs.msg import Odometry
@@ -69,6 +71,7 @@ def __init__(self):
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.go2_camera_info_pub = self.create_publisher(CameraInfo, 'camera_info', qos_profile)
self.imu_pub = self.create_publisher(IMU, 'imu', qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
@@ -102,6 +105,24 @@ def __init__(self):
self.timer = self.create_timer(0.1, self.timer_callback)
self.timer_lidar = self.create_timer(0.5, self.timer_callback_lidar)
+ self.publish_camera_info()
+
+ def publish_camera_info(self):
+ with open(get_package_share_directory('go2_robot_sdk') + '/calibration/front_camera.yaml', 'r') as stream:
+ calib_data = yaml.safe_load(stream)
+
+ camera_info = CameraInfo()
+ camera_info.header.frame_id = 'front_camera'
+ camera_info.width = calib_data['image_width']
+ camera_info.height = calib_data['image_height']
+ camera_info.k = calib_data['camera_matrix']['data']
+ camera_info.d = calib_data['distortion_coefficients']['data']
+ camera_info.r = calib_data['rectification_matrix']['data']
+ camera_info.p = calib_data['projection_matrix']['data']
+ camera_info.distortion_model = calib_data['distortion_model']
+
+ self.go2_camera_info_pub.publish(camera_info)
+
def timer_callback(self):
self.publish_odom()
self.publish_odom_topic()
diff --git a/go2_robot_sdk/setup.py b/go2_robot_sdk/setup.py
index bd54451..8debd54 100644
--- a/go2_robot_sdk/setup.py
+++ b/go2_robot_sdk/setup.py
@@ -40,6 +40,7 @@
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
(os.path.join('share', package_name, 'urdf'), glob(os.path.join('urdf', '*'))),
(os.path.join('share', package_name, 'dae'), glob(os.path.join('dae', '*'))),
+ (os.path.join('share', package_name, 'calibration'), glob(os.path.join('calibration', '*'))),
(os.path.join('share', package_name, 'meshes'), glob(os.path.join('meshes', '*'))),
(os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
(os.path.join('share', package_name, 'external_lib'), ['external_lib/libvoxel.wasm']),
diff --git a/requirements.txt b/requirements.txt
index b1ad0ec..59a64e0 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -2,4 +2,5 @@ aiortc
aiohttp
paho-mqtt
python-dotenv
-wasmtime
\ No newline at end of file
+wasmtime
+pyyaml
\ No newline at end of file
From 21ade95f5aab859fda813822dcc7d4de529a2357 Mon Sep 17 00:00:00 2001
From: mvrius
Date: Sun, 2 Jun 2024 10:06:34 -0700
Subject: [PATCH 2/6] add wsl2 ros2 joystick setup
---
README.md | 44 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 44 insertions(+)
diff --git a/README.md b/README.md
index be99fc5..9b883ea 100644
--- a/README.md
+++ b/README.md
@@ -162,6 +162,50 @@ sudo snap install foxglove-studio
+## WSL 2
+
+If you are running ROS2 under WSL2 - you may need to configure Joystick\Gamepad to navigate the robot.
+
+1. Step 1 - share device with WSL2
+
+ Follow steps here https://learn.microsoft.com/en-us/windows/wsl/connect-usb to share your console device with WSL2
+
+2. Step 2 - Enable WSL2 joystick drivers
+
+ WSL2 does not come by default with the modules for joysticks. Build WSL2 Kernel with the joystick drivers. Follow the instructions here: https://github.com/dorssel/usbipd-win/wiki/WSL-support#building-your-own-wsl-2-kernel-with-additional-drivers If you're comfortable with WSl2, skip the export steps and start at `Install prerequisites.`
+
+ Before buiding, edit `.config` file and update the CONFIG_ values listed in this GitHub issue: https://github.com/microsoft/WSL/issues/7747#issuecomment-1328217406
+
+2. Step 3 - Give permissions to /dev/input devices
+
+ Once you've finished the guides under Step 3 - you should be able to see your joystick device under /dev/input
+
+ ```bash
+ ls /dev/input
+ by-id by-path event0 js0
+ ```
+
+ By default /dev/input/event* will only have root permissions, so joy node won't have access to the joystick
+
+ Create a file `/etc/udev/rules.d/99-userdev-input.rules` with the following content:
+ `KERNEL=="event*", SUBSYSTEM=="input", RUN+="/usr/bin/setfacl -m u:YOURUSERNAME:rw $env{DEVNAME}"`
+
+ Run as root: `udevadm control --reload-rules && udevadm trigger`
+
+ https://askubuntu.com/a/609678
+
+3. Step 3 - verify that joy node is able to see the device properly.
+
+ 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!
From 5416f3cd6fdf1271ba7f71143a797a2a728c0484 Mon Sep 17 00:00:00 2001
From: Tamas Foldi
Date: Mon, 20 May 2024 14:00:15 +0200
Subject: [PATCH 3/6] feat: publish camera info
---
go2_robot_sdk/calibration/front_camera.yaml | 26 +++++++++++++++++++
.../go2_robot_sdk/go2_driver_node.py | 23 +++++++++++++++-
go2_robot_sdk/setup.py | 1 +
requirements.txt | 3 ++-
4 files changed, 51 insertions(+), 2 deletions(-)
create mode 100644 go2_robot_sdk/calibration/front_camera.yaml
diff --git a/go2_robot_sdk/calibration/front_camera.yaml b/go2_robot_sdk/calibration/front_camera.yaml
new file mode 100644
index 0000000..36231fb
--- /dev/null
+++ b/go2_robot_sdk/calibration/front_camera.yaml
@@ -0,0 +1,26 @@
+image_width: 1920
+image_height: 1080
+camera_name: front_camera
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1310.77826, 0. , 1018.71143,
+ 1. , 1320.25059, 637.37672,
+ 1., 0. , 1. ]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.415971, 0.158898, -0.015395, -0.008031, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1., 0., 0.,
+ 0., 1., 0.,
+ 0., 0., 1.]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [992.26007, 0. , 996.08995, 0. ,
+ 1., 1221.72864, 639.55771, 0. ,
+ 1., 0. , 1. , 0. ]
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..2a72d6b 100644
--- a/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py
+++ b/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py
@@ -27,6 +27,7 @@
import os
import threading
import asyncio
+import yaml
from scripts.go2_constants import ROBOT_CMD, RTC_TOPIC
from scripts.go2_func import gen_command, gen_mov_command
@@ -39,9 +40,10 @@
from rclpy.qos import QoSProfile
from tf2_ros import TransformBroadcaster, TransformStamped
+from ament_index_python.packages import get_package_share_directory
from geometry_msgs.msg import Twist, TransformStamped
from go2_interfaces.msg import Go2State, IMU
-from sensor_msgs.msg import PointCloud2, PointField, JointState, Joy
+from sensor_msgs.msg import PointCloud2, PointField, JointState, Joy, CameraInfo
from sensor_msgs_py import point_cloud2
from std_msgs.msg import Header, String
from nav_msgs.msg import Odometry
@@ -69,6 +71,7 @@ def __init__(self):
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.go2_camera_info_pub = self.create_publisher(CameraInfo, 'camera_info', qos_profile)
self.imu_pub = self.create_publisher(IMU, 'imu', qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
@@ -102,6 +105,24 @@ def __init__(self):
self.timer = self.create_timer(0.1, self.timer_callback)
self.timer_lidar = self.create_timer(0.5, self.timer_callback_lidar)
+ self.publish_camera_info()
+
+ def publish_camera_info(self):
+ with open(get_package_share_directory('go2_robot_sdk') + '/calibration/front_camera.yaml', 'r') as stream:
+ calib_data = yaml.safe_load(stream)
+
+ camera_info = CameraInfo()
+ camera_info.header.frame_id = 'front_camera'
+ camera_info.width = calib_data['image_width']
+ camera_info.height = calib_data['image_height']
+ camera_info.k = calib_data['camera_matrix']['data']
+ camera_info.d = calib_data['distortion_coefficients']['data']
+ camera_info.r = calib_data['rectification_matrix']['data']
+ camera_info.p = calib_data['projection_matrix']['data']
+ camera_info.distortion_model = calib_data['distortion_model']
+
+ self.go2_camera_info_pub.publish(camera_info)
+
def timer_callback(self):
self.publish_odom()
self.publish_odom_topic()
diff --git a/go2_robot_sdk/setup.py b/go2_robot_sdk/setup.py
index bd54451..8debd54 100644
--- a/go2_robot_sdk/setup.py
+++ b/go2_robot_sdk/setup.py
@@ -40,6 +40,7 @@
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
(os.path.join('share', package_name, 'urdf'), glob(os.path.join('urdf', '*'))),
(os.path.join('share', package_name, 'dae'), glob(os.path.join('dae', '*'))),
+ (os.path.join('share', package_name, 'calibration'), glob(os.path.join('calibration', '*'))),
(os.path.join('share', package_name, 'meshes'), glob(os.path.join('meshes', '*'))),
(os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
(os.path.join('share', package_name, 'external_lib'), ['external_lib/libvoxel.wasm']),
diff --git a/requirements.txt b/requirements.txt
index b1ad0ec..59a64e0 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -2,4 +2,5 @@ aiortc
aiohttp
paho-mqtt
python-dotenv
-wasmtime
\ No newline at end of file
+wasmtime
+pyyaml
\ No newline at end of file
From a2af6d6511141cfb7a70d43f67d8089e8f46b06b Mon Sep 17 00:00:00 2001
From: Tamas Foldi
Date: Mon, 3 Jun 2024 09:14:33 +0200
Subject: [PATCH 4/6] chore: add package name
---
go2_robot_sdk/package.xml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/go2_robot_sdk/package.xml b/go2_robot_sdk/package.xml
index b081327..2e726c2 100644
--- a/go2_robot_sdk/package.xml
+++ b/go2_robot_sdk/package.xml
@@ -3,7 +3,7 @@
go2_robot_sdk
0.0.0
- TODO: Package description
+ Unitree Go2 ROS2 Unofficial SDK
brimo
Apache-2.0
From 82fdaedb90297a994367459ac4ebf276f0e4ae4b Mon Sep 17 00:00:00 2001
From: Tamas Foldi
Date: Mon, 3 Jun 2024 09:14:58 +0200
Subject: [PATCH 5/6] fix: rotate optical frame to ROS2 convention
---
go2_robot_sdk/urdf/go2.urdf | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/go2_robot_sdk/urdf/go2.urdf b/go2_robot_sdk/urdf/go2.urdf
index 9e42b14..2d6b3a2 100644
--- a/go2_robot_sdk/urdf/go2.urdf
+++ b/go2_robot_sdk/urdf/go2.urdf
@@ -134,7 +134,7 @@ Stephen Brawner (brawner@gmail.com)
-
+
From 459c4ca11bcd1ff2b64ed3747c645c190b34671b Mon Sep 17 00:00:00 2001
From: Tamas Foldi
Date: Mon, 3 Jun 2024 18:39:41 +0200
Subject: [PATCH 6/6] chore: add new camera calibration
---
go2_video | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/go2_video b/go2_video
index 794d70b..7191797 160000
--- a/go2_video
+++ b/go2_video
@@ -1 +1 @@
-Subproject commit 794d70b97b447f951b8c3cde8a5a76996c98a39e
+Subproject commit 7191797885706a186069455c615268e91f9ca639