Merge branch 'master' into chore/add_go2_realsense_urdf

Author: tfoldi

go2_robot_sdk/go2_robot_sdk/go2_driver_node.py

@@ -89,7 +89,7 @@ def __init__(self):
         self.publish_raw_voxel = self.get_parameter(
             'publish_raw_voxel').get_parameter_value().bool_value
 
-        self.conn_mode = "single" if len(self.robot_ip_lst) == 1 else "multi"
+        self.conn_mode = "single" if (len(self.robot_ip_lst) == 1 and self.conn_type != "cyclonedds") else "multi"
 
         self.get_logger().info(f"Received ip list: {self.robot_ip_lst}")
         self.get_logger().info(f"Connection type is {self.conn_type}")
@@ -368,7 +368,7 @@ async def on_video_frame(self, track: MediaStreamTrack, robot_num):
             # Publish image and camera info
             self.img_pub[robot_num].publish(ros_image)
             self.camera_info_pub[robot_num].publish(camera_info)
-            asyncio.sleep(0)
+            await asyncio.sleep(0)
 
     def on_data_channel_message(self, _, msg, robot_num):
 
@@ -472,7 +472,7 @@ def publish_voxel_webrtc(self):
         for i in range(len(self.robot_lidar)):
             if self.robot_lidar[str(i)]:
                 voxel_msg = VoxelMapCompressed()
-                voxel_msg.stamp = self.robot_lidar[str(i)]['data']['stamp']
+                voxel_msg.stamp = float(self.robot_lidar[str(i)]['data']['stamp'])
                 voxel_msg.frame_id = 'odom'
 
                 # Example data: {"type":"msg","topic":"rt/utlidar/voxel_map_compressed",

go2_robot_sdk/launch/robot.launch.py

@@ -45,14 +45,15 @@ def generate_launch_description():
     robot_ip_lst = robot_ip.replace(" ", "").split(",")
     print("IP list:", robot_ip_lst)
 
-    conn_mode = "single" if len(robot_ip_lst) == 1 else "multi"
 
     # these are debug only
     map_name = os.getenv('MAP_NAME', '3d_map')
     save_map = os.getenv('MAP_SAVE', 'true')
 
     conn_type = os.getenv('CONN_TYPE', 'webrtc')
 
+    conn_mode = "single" if len(robot_ip_lst) == 1 and conn_type != "cyclonedds" else "multi"
+
     if conn_mode == 'single':
         rviz_config = "single_robot_conf.rviz"
     else:

go2_robot_sdk/scripts/go2_func.py

@@ -33,7 +33,7 @@ def generate_id() -> int:
     return int(datetime.datetime.now().timestamp() * 1000 % 2147483648) + random.randint(0, 999)
 
 
-def create_command_structure(api_id: int, parameter: Any, topic: str = SPORT_MODE_TOPIC) -> Dict:
+def create_command_structure(api_id: int, parameter: str, topic: str = SPORT_MODE_TOPIC) -> Dict:
     return {
         "type": "msg",
         "topic": topic,
@@ -44,12 +44,12 @@ def create_command_structure(api_id: int, parameter: Any, topic: str = SPORT_MOD
                     "api_id": api_id
                 }
             },
-            "parameter": json.dumps(parameter)
+            "parameter": parameter
         }
     }
 
 
-def gen_command(cmd: int, parameters: Optional[Any] = None, topic: Optional[str] = None) -> str:
+def gen_command(cmd: int, parameters: Optional[str] = None, topic: Optional[str] = None) -> str:
     parameter = parameters if parameters is not None else cmd
     command = create_command_structure(
         api_id=cmd,
@@ -63,7 +63,7 @@ def gen_mov_command(x: float, y: float, z: float) -> str:
     parameters = {"x": x, "y": y, "z": z}
     command = create_command_structure(
         api_id=1008,
-        parameter=parameters,
+        parameter=json.dumps(parameters),
         topic=SPORT_MODE_TOPIC
     )
     return json.dumps(command)