From 89a13c09691b09ba4c33c2602db9d3369e83a8fb Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 24 Nov 2023 14:56:39 +0100
Subject: [PATCH] Add sensors, and fix camera_model:=None

---
 rosbot_xl_bringup/launch/bringup.launch.py    |  1 +
 .../launch/controller.launch.py               |  1 +
 rosbot_xl_controller/test/test_xacro.py       |  4 +-
 .../urdf/rosbot_xl.urdf.xacro                 | 27 +++++++++-
 rosbot_xl_gazebo/launch/simulation.launch.py  | 53 +++++++++++--------
 5 files changed, 62 insertions(+), 24 deletions(-)

diff --git a/rosbot_xl_bringup/launch/bringup.launch.py b/rosbot_xl_bringup/launch/bringup.launch.py
index b3c4a006..6f79ecb9 100644
--- a/rosbot_xl_bringup/launch/bringup.launch.py
+++ b/rosbot_xl_bringup/launch/bringup.launch.py
@@ -43,6 +43,7 @@ def generate_launch_description():
         choices=[
             "None",
             "intel_realsense_d435",
+            "orbbec_astra",
             "stereolabs_zed",
             "stereolabs_zedm",
             "stereolabs_zed2",
diff --git a/rosbot_xl_controller/launch/controller.launch.py b/rosbot_xl_controller/launch/controller.launch.py
index f06df1e5..0ca2e42b 100644
--- a/rosbot_xl_controller/launch/controller.launch.py
+++ b/rosbot_xl_controller/launch/controller.launch.py
@@ -49,6 +49,7 @@ def generate_launch_description():
         choices=[
             "None",
             "intel_realsense_d435",
+            "orbbec_astra",
             "stereolabs_zed",
             "stereolabs_zedm",
             "stereolabs_zed2",
diff --git a/rosbot_xl_controller/test/test_xacro.py b/rosbot_xl_controller/test/test_xacro.py
index a417a335..089bad59 100644
--- a/rosbot_xl_controller/test/test_xacro.py
+++ b/rosbot_xl_controller/test/test_xacro.py
@@ -25,14 +25,16 @@ def test_rosbot_description_parsing():
     simulation_engine_values = ["ignition-gazebo", "webots"]  # 'gazebo-classic'
     lidar_model = [
         "None",
-        "slamtec_rplidar_s1",
+        "ouster_os1_32",
         "slamtec_rplidar_a2",
         "slamtec_rplidar_a3",
+        "slamtec_rplidar_s1",
         "velodyne_puck",
     ]
     camera_model = [
         "None",
         "intel_realsense_d435",
+        "orbbec_astra",
         "stereolabs_zed",
         "stereolabs_zedm",
         "stereolabs_zed2",
diff --git a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
index 44a0dc5e..b28dfab5 100644
--- a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
+++ b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
@@ -34,6 +34,18 @@
   <!-- use_gpu has to be set to true, CPU lidar doesn't work in ignition -
     https://github.com/gazebosim/gz-sensors/issues/26 -->
 
+  <xacro:if value="${lidar_model == 'ouster_os1_32'}">
+    <xacro:include filename="$(find ros_components_description)/urdf/ouster_os1_32.urdf.xacro"
+      ns="lidar" />
+    <xacro:lidar.ouster_os1_32
+      parent_link="${lidar_parent_link}"
+      xyz="${lidar_xyz}"
+      rpy="${lidar_rpy}"
+      use_gpu="${lidar_use_gpu}"
+      simulation_engine="$(arg simulation_engine)"
+      topic="velodyne_points" />
+  </xacro:if>
+
   <xacro:if value="${lidar_model == 'slamtec_rplidar_s1'}">
     <xacro:include filename="$(find ros_components_description)/urdf/slamtec_rplidar_s1.urdf.xacro"
       ns="lidar" />
@@ -102,6 +114,18 @@
       simulation_engine="$(arg simulation_engine)" />
   </xacro:if>
 
+  <xacro:if value="${camera_model == 'orbbec_astra'}">
+    <xacro:include
+      filename="$(find ros_components_description)/urdf/orbbec_astra.urdf.xacro"
+      ns="camera" />
+    <xacro:camera.orbbec_astra
+      parent_link="${camera_parent_link}"
+      xyz="${camera_xyz}"
+      rpy="${camera_rpy}"
+      name="camera"
+      simulation_engine="$(arg simulation_engine)" />
+  </xacro:if>
+
   <xacro:if value="${camera_model.startswith('stereolabs')}">
     <xacro:if value="${camera_model == 'stereolabs_zed'}">
       <xacro:property name="camera_model_type" value="zed" />
@@ -131,8 +155,7 @@
       rpy="${camera_rpy}"
       model="${camera_model_type}"
       name="camera"
-      topic="camera"
-      use_nominal_extrinsics="$(arg use_sim)"
+      topic="${camera_model_type}"
       simulation_engine="$(arg simulation_engine)" />
   </xacro:if>
 
diff --git a/rosbot_xl_gazebo/launch/simulation.launch.py b/rosbot_xl_gazebo/launch/simulation.launch.py
index a84812f4..fe540936 100644
--- a/rosbot_xl_gazebo/launch/simulation.launch.py
+++ b/rosbot_xl_gazebo/launch/simulation.launch.py
@@ -55,6 +55,17 @@ def launch_gz_bridge(context: LaunchContext, *args, **kwargs):
 
         depth_camera_parent_tf = "camera_depth_frame"
 
+    if camera_model == "orbbec_astra":
+        gz_args.append("/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo")
+        gz_args.append("/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image")
+        gz_args.append("/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image")
+        gz_args.append("/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked")
+
+        gz_remapping.append(("/camera/camera_info", "/camera/depth/camera_info"))
+        gz_remapping.append(("/camera/depth", "/camera/depth/image_raw"))
+        gz_remapping.append(("/camera/depth_image", "/camera/depth/image_raw"))
+        gz_remapping.append(("/camera/points", "/camera/depth/points"))
+
     elif camera_model.startswith("stereolabs_zed"):
         zed = camera_model[len("stereolabs_") :]
 
@@ -93,8 +104,12 @@ def launch_gz_bridge(context: LaunchContext, *args, **kwargs):
         )
 
         gz_remapping.append(("/velodyne_points/points", "/velodyne_points"))
-    else:  # FIXME: Checkout ouster
-        pass
+    elif lidar_model.startswith("ouster"):
+        gz_args.append(
+            "/velodyne_points/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked"
+        )
+
+        gz_remapping.append(("/velodyne_points/points", "/points"))
 
     gz_bridge_node = Node(
         package="ros_gz_bridge",
@@ -105,26 +120,21 @@ def launch_gz_bridge(context: LaunchContext, *args, **kwargs):
         output="screen",
     )
 
-    # The frame of the point cloud from ignition gazebo 6 isn't provided by <frame_id>.
-    # See https://github.com/gazebosim/gz-sensors/issues/239
-    point_cloud_tf = Node(
-        package="tf2_ros",
-        executable="static_transform_publisher",
-        name="point_cloud_tf",
-        output="log",
-        arguments=[
-            "0.0",
-            "0.0",
-            "0.0",
-            "0.0",
-            "0.0",
-            "0.0",
-            depth_camera_parent_tf,
-            "rosbot_xl/base_link/camera_depth",
-        ],
-    )
-
     if depth_camera_parent_tf:
+        # The frame of the point cloud from ignition gazebo 6 isn't provided by <frame_id>.
+        # See https://github.com/gazebosim/gz-sensors/issues/239
+        point_cloud_tf = Node(
+            package="tf2_ros",
+            executable="static_transform_publisher",
+            name="point_cloud_tf",
+            output="log",
+            arguments=[
+                "--child-frame-id",
+                "rosbot_xl/base_link/camera_depth",
+                "--frame-id",
+                depth_camera_parent_tf,
+            ],
+        )
         return [gz_bridge_node, point_cloud_tf]
     else:
         return [gz_bridge_node]
@@ -148,6 +158,7 @@ def generate_launch_description():
         choices=[
             "None",
             "intel_realsense_d435",
+            "orbbec_astra",
             "stereolabs_zed",
             "stereolabs_zedm",
             "stereolabs_zed2",