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 Slam

+## 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