feat: publish camera info

README.md

@@ -162,6 +162,50 @@ sudo snap install foxglove-studio
 <img width="1280" height="640" src="https://github.com/abizovnuralem/go2_ros2_sdk/assets/33475993/59f33599-a54c-4cff-8ac2-6859a05ccb8a" alt='Slam'>
 </p>
 
+## 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!

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.     ]

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()

go2_robot_sdk/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>go2_robot_sdk</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Unitree Go2 ROS2 Unofficial SDK</description>
   <maintainer email="[email protected]">brimo</maintainer>
   <license>Apache-2.0</license>
 

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']),

go2_robot_sdk/urdf/go2.urdf

@@ -134,7 +134,7 @@ Stephen Brawner ([email protected])
     </collision>
   </link>
   <joint name="front_camera_joint" type="fixed">
-    <origin xyz="0.045 0 0.03" rpy="-1.5708 0 -1.5708" />
+    <origin xyz="0.045 0 0.03" rpy="1.5708 0 1.5708" />
     <parent link="Head_upper" />
     <child link="front_camera" />
     <axis xyz="0 0 0" />

requirements.txt

@@ -2,4 +2,5 @@ aiortc
 aiohttp
 paho-mqtt
 python-dotenv
-wasmtime
+wasmtime
+pyyaml