RGBカメラのモデルを表示できるように変更

launch/display.launch.py

@@ -40,10 +40,15 @@ def generate_launch_description():
         'use_rviz',
         default_value='true',
         description='Set "true" to launch rviz.')
+    declare_arg_use_rgb_camera = DeclareLaunchArgument(
+        'use_rgb_camera',
+        default_value='false',
+        description='Set "true" to mount rgb camera.')
 
     description_loader = RobotDescriptionLoader()
     description_loader.lidar = LaunchConfiguration('lidar')
     description_loader.lidar_frame = LaunchConfiguration('lidar_frame')
+    description_loader.use_rgb_camera = LaunchConfiguration('use_rgb_camera')
 
     push_ns = PushRosNamespace([LaunchConfiguration('namespace')])
 
@@ -72,6 +77,7 @@ def generate_launch_description():
         declare_arg_lidar_frame,
         declare_arg_namespace,
         declare_arg_use_rviz,
+        declare_arg_use_rgb_camera,
         push_ns,
         rsp,
         jsp,

package.xml

@@ -22,6 +22,7 @@
   <depend>xacro</depend>
   <depend>launch</depend>
   <depend>ign_ros2_control</depend>
+  <depend>realsense2_description</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

raspimouse_description/robot_description_loader.py

@@ -35,6 +35,7 @@ def __init__(self):
         self.lidar = 'none'
         self.lidar_frame = 'laser'
         self.use_gazebo = 'false'
+        self.use_rgb_camera = 'false'
         self.gz_control_config_package = ''
         self.gz_control_config_file_path = ''
 
@@ -45,6 +46,7 @@ def load(self):
                 ' lidar:=', self.lidar,
                 ' lidar_frame:=', self.lidar_frame,
                 ' use_gazebo:=', self.use_gazebo,
+                ' use_rgb_camera:=', self.use_rgb_camera,
                 ' gz_control_config_package:=', self.gz_control_config_package,
                 ' gz_control_config_file_path:=', self.gz_control_config_file_path
                 ])

test/test_robot_description_loader.py

@@ -89,3 +89,12 @@ def test_use_gazebo():
     rdl.gz_control_config_package = 'raspimouse_description'
     rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
     assert 'ign_ros2_control/IgnitionSystem' in exec_load(rdl)
+
+
+def test_use_rgb_camera():
+    # use_rgb_cameraが変更され、xacroにRGB Cameraがセットされることを期待
+    rdl = RobotDescriptionLoader()
+    rdl.use_rgb_camera = 'true'
+    rdl.gz_control_config_package = 'raspimouse_description'
+    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
+    assert 'realsense2_description/meshes/d435.dae' in exec_load(rdl)

urdf/raspimouse.urdf.xacro

@@ -13,6 +13,7 @@
   <xacro:arg name="robot_namespace" default="/" />
   <xacro:arg name="sensor_namespace" default="raspimouse_on_gazebo" />
   <xacro:arg name="use_gazebo" default="false" />
+  <xacro:arg name="use_rgb_camera" default="false" />
   <xacro:arg name="gz_control_config_package" default="" />
   <xacro:arg name="gz_control_config_file_path" default="" />
 
@@ -97,6 +98,18 @@
     </xacro:lidar_rp_sensor>
   </xacro:if>
 
+  <!-- RGB Camera -->
+  <xacro:if value="$(arg use_rgb_camera)">
+    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+    <xacro:sensor_d435
+        parent="base_link"
+        use_nominal_extrinsics="false"
+        add_plug="false"
+        use_mesh="true">
+      <origin xyz="0.08 0.0 0.055" rpy="0 0 0"/>
+    </xacro:sensor_d435>
+  </xacro:if>
+
   <!-- =============== Gazebo =============== -->
 
   <xacro:if value="$(arg use_gazebo)">