Merge pull request #84 from abizovnuralem/camera_for_v111

feat: Camera images from v1.1.1 firmware

.gitmodules

@@ -1,3 +1,4 @@
-[submodule "go2_video"]
-	path = go2_video
-	url = https://github.com/tfoldi/ros2_go2_video
+[submodule "go2_robot_sdk/external_lib/aioice"]
+	path = go2_robot_sdk/external_lib/aioice
+	url = https://github.com/legion1581/aioice.git
+	branch = go2

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/__init__.py

@@ -0,0 +1,15 @@
+import sys
+import os
+import logging
+from ament_index_python import get_package_share_directory
+
+logger = logging.getLogger(__name__)
+
+libs_path =  os.path.join(
+        get_package_share_directory('go2_robot_sdk'),
+        'external_lib'
+    )
+sys.path.insert(0, os.path.join(libs_path, 'aioice'))
+sys.path.insert(0, os.path.join(libs_path))
+
+logger.info('Patched aioice added to sys.path: {}'.format(sys.path))

go2_robot_sdk/go2_robot_sdk/go2_driver_node.py

@@ -28,6 +28,10 @@
 import threading
 import asyncio
 
+from aiortc import MediaStreamTrack
+from cv_bridge import CvBridge
+
+
 from scripts.go2_constants import ROBOT_CMD, RTC_TOPIC
 from scripts.go2_func import gen_command, gen_mov_command
 from scripts.go2_lidar_decoder import update_meshes_for_cloud2
@@ -46,6 +50,7 @@
 from sensor_msgs_py import point_cloud2
 from std_msgs.msg import Header
 from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Image
 
 
 logging.basicConfig(level=logging.WARN)
@@ -88,6 +93,7 @@ def __init__(self):
         self.go2_lidar_pub = []
         self.go2_odometry_pub = []
         self.imu_pub = []
+        self.img_pub = []
 
         if self.conn_mode == 'single':
             self.joint_pub.append(self.create_publisher(
@@ -99,6 +105,7 @@ def __init__(self):
             self.go2_odometry_pub.append(
                 self.create_publisher(Odometry, 'odom', qos_profile))
             self.imu_pub.append(self.create_publisher(IMU, 'imu', qos_profile))
+            self.img_pub.append(self.create_publisher(Image, 'camera/image_raw', qos_profile))
 
         else:
             for i in range(len(self.robot_ip_lst)):
@@ -112,9 +119,13 @@ def __init__(self):
                     Odometry, f'robot{i}/odom', qos_profile))
                 self.imu_pub.append(self.create_publisher(
                     IMU, f'robot{i}/imu', qos_profile))
+                self.img_pub.append(self.create_publisher(
+                    Image, f'robot{i}/camera/image_raw', qos_profile))
 
         self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
 
+        self.bridge = CvBridge()
+
         self.robot_cmd_vel = {}
         self.robot_odom = {}
         self.robot_low_cmd = {}
@@ -254,6 +265,21 @@ def on_validated(self, robot_num):
                 self.conn[robot_num].data_channel.send(
                     json.dumps({"type": "subscribe", "topic": topic}))
 
+    async def on_video_frame(self, track: MediaStreamTrack, robot_num):
+        logger.info(f"Video frame received for robot {robot_num}")
+
+        while True:
+            frame = await track.recv()
+            img = frame.to_ndarray(format="bgr24")
+
+            logger.debug(f"Shape: {img.shape}, Dimensions: {img.ndim}, Type: {img.dtype}, Size: {img.size}")
+
+            # Convert the OpenCV image to ROS Image message
+            ros_image = self.bridge.cv2_to_imgmsg(img, encoding="bgr8")
+
+            # Publish the image
+            self.img_pub[robot_num].publish(ros_image)
+
     def on_data_channel_message(self, _, msg, robot_num):
 
         if msg.get('topic') == RTC_TOPIC["ULIDAR_ARRAY"]:
@@ -524,6 +550,7 @@ async def start_node():
             token=base_node.token,
             on_validated=base_node.on_validated,
             on_message=base_node.on_data_channel_message,
+            on_video_frame=base_node.on_video_frame,
         )
 
         sleep_task_lst.append(asyncio.get_event_loop(

go2_robot_sdk/launch/robot.launch.py

@@ -24,7 +24,7 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.conditions import UnlessCondition
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch.actions import IncludeLaunchDescription
@@ -33,7 +33,12 @@
 def generate_launch_description():
 
     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
-    no_rviz2 = LaunchConfiguration('no_rviz2', default='false')
+    with_rviz2 = LaunchConfiguration('rviz2', default='true')
+    with_nav2 = LaunchConfiguration('nav2', default='true')
+    with_slam = LaunchConfiguration('slam', default='true')
+    with_foxglove = LaunchConfiguration('foxglove', default='true')
+    with_joystick = LaunchConfiguration('joystick', default='true')
+    with_teleop = LaunchConfiguration('teleop', default='true')
 
     robot_token = os.getenv('ROBOT_TOKEN', '') # how does this work for multiple robots?
     robot_ip = os.getenv('ROBOT_IP', '')
@@ -119,14 +124,6 @@ def generate_launch_description():
                 arguments=[urdf]
             ),
         )
-        # urdf_launch_nodes.append(
-        #     Node(
-        #         package='ros2_go2_video',
-        #         executable='ros2_go2_video',
-        #         parameters=[{'robot_ip': robot_ip_lst[0],
-        #                      'robot_token': robot_token}],
-        #     ),
-        # )
         urdf_launch_nodes.append(
             Node(
                 package='pointcloud_to_laserscan',
@@ -159,14 +156,6 @@ def generate_launch_description():
                     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}],
-            #     ),
-            # )
             urdf_launch_nodes.append(
                 Node(
                     package='pointcloud_to_laserscan',
@@ -201,40 +190,45 @@ def generate_launch_description():
             package='rviz2',
             namespace='',
             executable='rviz2',
-            condition=UnlessCondition(no_rviz2),
+            condition=IfCondition(with_rviz2),
             name='rviz2',
             arguments=['-d' + os.path.join(get_package_share_directory('go2_robot_sdk'), 'config', rviz_config)]
         ),
         Node(
             package='joy',
             executable='joy_node',
+            condition=IfCondition(with_joystick),
             parameters=[joy_params]
         ),
         Node(
             package='teleop_twist_joy',
             executable='teleop_node',
             name='teleop_node',
+            condition=IfCondition(with_joystick),
             parameters=[default_config_topics],
         ),
         Node(
             package='twist_mux',
             executable='twist_mux',
             output='screen',
+            condition=IfCondition(with_teleop),
             parameters=[
                 {'use_sim_time': use_sim_time},
                 default_config_topics
             ],
         ),
 
         IncludeLaunchDescription(
-            FrontendLaunchDescriptionSource(foxglove_launch)
+            FrontendLaunchDescriptionSource(foxglove_launch),
+            condition=IfCondition(with_foxglove),
         ),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
                 os.path.join(get_package_share_directory(
                     'slam_toolbox'), 'launch', 'online_async_launch.py')
             ]),
+            condition=IfCondition(with_slam),
             launch_arguments={
                 'slam_params_file': slam_toolbox_config,
                 'use_sim_time': use_sim_time,
@@ -246,6 +240,7 @@ def generate_launch_description():
                 os.path.join(get_package_share_directory(
                     'nav2_bringup'), 'launch', 'navigation_launch.py')
             ]),
+            condition=IfCondition(with_nav2),
             launch_arguments={
                 'params_file': nav2_config,
                 'use_sim_time': use_sim_time,

go2_robot_sdk/package.xml

@@ -9,6 +9,7 @@
 
   <depend>rclpy</depend>
   <depend>go2_interfaces</depend>
+  <depend>unitree_go</depend>
 
   <exec_depend>twist_mux</exec_depend>
   <exec_depend>joy</exec_depend>
@@ -24,6 +25,8 @@
 
   <exec_depend>foxglove_bridge</exec_depend>
 
+  <exec_depend>cv_bridge</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

go2_robot_sdk/scripts/webrtc_driver.py

@@ -26,9 +26,7 @@
 
 
 import binascii
-import time
 import uuid
-import aiohttp
 import base64
 import hashlib
 import json
@@ -39,7 +37,6 @@
 from Crypto.Cipher import PKCS1_v1_5
 import requests
 from aiortc import RTCPeerConnection, RTCSessionDescription
-from aiortc.contrib.media import MediaBlackhole
 
 
 from scripts.go2_lidar_decoder import LidarDecoder
@@ -193,7 +190,8 @@ def __init__(
             token="",
             on_validated=None,
             on_message=None,
-            on_open=None
+            on_open=None,
+            on_video_frame=None,
     ):
 
         self.pc = RTCPeerConnection()
@@ -205,8 +203,7 @@ def __init__(
         self.on_message = on_message
         self.on_open = on_open
 
-        self.audio_track = MediaBlackhole()
-        self.video_track = MediaBlackhole()
+        self.on_video_frame = on_video_frame
 
         self.data_channel = self.pc.createDataChannel("data", id=0)
         self.data_channel.on("open", self.on_data_channel_open)
@@ -215,19 +212,22 @@ def __init__(
         self.pc.on("track", self.on_track)
         self.pc.on("connectionstatechange", self.on_connection_state_change)
 
+        self.pc.addTransceiver("video", direction="recvonly")
+
     def on_connection_state_change(self):
         logger.info(f"Connection state is {self.pc.connectionState}")
 
-    def on_track(self, track):
+    async def on_track(self, track):
         logger.info(f"Receiving {track.kind}")
         if track.kind == "audio":
             pass
         elif track.kind == "video":
-            pass
+            frame = await track.recv()
+            logger.info(f"Received frame {frame}")
+            if self.on_video_frame:
+                await self.on_video_frame(track, int(self.robot_num))
 
     async def generate_offer(self):
-        await self.audio_track.start()
-        await self.video_track.start()
         offer = await self.pc.createOffer()
         await self.pc.setLocalDescription(offer)
         return offer.sdp
@@ -336,6 +336,14 @@ async def connect(self):
 
     def validate_robot_conn(self, message):
         if message.get("data") == "Validation Ok.":
+
+            # turn on video
+            self.publish(
+                "",
+                "on",
+                "vid",
+            )
+
             self.validation_result = "SUCCESS"
             if self.on_validated:
                 self.on_validated(self.robot_num)

go2_robot_sdk/setup.py

@@ -43,6 +43,7 @@
         (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']),
+        (os.path.join('share', package_name, 'external_lib/aioice'), glob(os.path.join('external_lib/aioice/src/aioice', '*'))),
 
 
     ],