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!
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/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
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/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)
-
+
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
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