Merge pull request #56 from husarion/add-sensors-and-fix

Add sensors and launch fix

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",
@@ -59,10 +60,11 @@ def generate_launch_description():
         description="Add LiDAR model to the robot URDF",
         choices=[
             "None",
-            "ouster_os1_32",
             "slamtec_rplidar_a2",
             "slamtec_rplidar_a3",
             "slamtec_rplidar_s1",
+            "slamtec_rplidar_s2",
+            "slamtec_rplidar_s3",
             "velodyne_puck",
         ],
     )

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",
@@ -65,10 +66,11 @@ def generate_launch_description():
         description="Add LiDAR model to the robot URDF",
         choices=[
             "None",
-            "ouster_os1_32",
             "slamtec_rplidar_a2",
             "slamtec_rplidar_a3",
             "slamtec_rplidar_s1",
+            "slamtec_rplidar_s2",
+            "slamtec_rplidar_s3",
             "velodyne_puck",
         ],
     )

rosbot_xl_controller/test/test_xacro.py

@@ -25,14 +25,17 @@ def test_rosbot_description_parsing():
     simulation_engine_values = ["ignition-gazebo", "webots"]  # 'gazebo-classic'
     lidar_model = [
         "None",
-        "slamtec_rplidar_s1",
         "slamtec_rplidar_a2",
         "slamtec_rplidar_a3",
+        "slamtec_rplidar_s1",
+        "slamtec_rplidar_s2",
+        "slamtec_rplidar_s3",
         "velodyne_puck",
     ]
     camera_model = [
         "None",
         "intel_realsense_d435",
+        "orbbec_astra",
         "stereolabs_zed",
         "stereolabs_zedm",
         "stereolabs_zed2",

rosbot_xl_description/urdf/rosbot_xl.urdf.xacro

@@ -45,6 +45,28 @@
       simulation_engine="$(arg simulation_engine)" />
   </xacro:if>
 
+  <xacro:if value="${lidar_model == 'slamtec_rplidar_s2'}">
+    <xacro:include filename="$(find ros_components_description)/urdf/slamtec_rplidar_s2.urdf.xacro"
+      ns="lidar" />
+    <xacro:lidar.slamtec_rplidar_s2
+      parent_link="${lidar_parent_link}"
+      xyz="${lidar_xyz}"
+      rpy="${lidar_rpy}"
+      use_gpu="${lidar_use_gpu}"
+      simulation_engine="$(arg simulation_engine)" />
+  </xacro:if>
+
+  <xacro:if value="${lidar_model == 'slamtec_rplidar_s3'}">
+    <xacro:include filename="$(find ros_components_description)/urdf/slamtec_rplidar_s3.urdf.xacro"
+      ns="lidar" />
+    <xacro:lidar.slamtec_rplidar_s3
+      parent_link="${lidar_parent_link}"
+      xyz="${lidar_xyz}"
+      rpy="${lidar_rpy}"
+      use_gpu="${lidar_use_gpu}"
+      simulation_engine="$(arg simulation_engine)" />
+  </xacro:if>
+
   <xacro:if value="${lidar_model == 'slamtec_rplidar_a2'}">
     <xacro:include filename="$(find ros_components_description)/urdf/slamtec_rplidar_a2.urdf.xacro"
       ns="lidar" />
@@ -96,12 +118,21 @@
       parent_link="${camera_parent_link}"
       xyz="${camera_xyz}"
       rpy="${camera_rpy}"
-      name="camera"
-      topic="camera"
       use_nominal_extrinsics="$(arg use_sim)"
       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}"
+      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" />
@@ -130,9 +161,6 @@
       xyz="${camera_xyz}"
       rpy="${camera_rpy}"
       model="${camera_model_type}"
-      name="camera"
-      topic="camera"
-      use_nominal_extrinsics="$(arg use_sim)"
       simulation_engine="$(arg simulation_engine)" />
   </xacro:if>
 

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",
@@ -160,14 +171,15 @@ def generate_launch_description():
     lidar_model = LaunchConfiguration("lidar_model")
     declare_lidar_model_arg = DeclareLaunchArgument(
         "lidar_model",
-        default_value="slamtec_rplidar_s1",
+        default_value="slamtec_rplidar_s3",
         description="Add LiDAR model to the robot URDF",
         choices=[
             "None",
-            "ouster_os1_32",
             "slamtec_rplidar_a2",
             "slamtec_rplidar_a3",
             "slamtec_rplidar_s1",
+            "slamtec_rplidar_s2",
+            "slamtec_rplidar_s3",
             "velodyne_puck",
         ],
     )