Gazebo上で画像トピックを配信できるように変更 (#46)

rt-net · Oct 27, 2023 · 7a7ff7d · 7a7ff7d
1 parent 576a35b
commit 7a7ff7d
Show file tree

Hide file tree

Showing 4 changed files with 68 additions and 12 deletions.
diff --git a/test/test_robot_description_loader.py b/test/test_robot_description_loader.py
@@ -98,3 +98,13 @@ def test_use_rgb_camera():
     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)
+
+
+def test_camera_link():
+    # use_gazeboとuse_rgb_cameraが変更され、xacroにcamera linkがセットされることを期待
+    rdl = RobotDescriptionLoader()
+    rdl.use_gazebo = 'true'
+    rdl.use_rgb_camera = 'true'
+    rdl.gz_control_config_package = 'raspimouse_description'
+    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
+    assert 'camera_link' in exec_load(rdl)
diff --git a/urdf/raspimouse.urdf.xacro b/urdf/raspimouse.urdf.xacro
@@ -29,6 +29,7 @@
   <xacro:include filename="$(find raspimouse_description)/urdf/wheel/diffdrive.gazebo_ros2_control.xacro" />
   <xacro:include filename="$(find raspimouse_description)/urdf/sensors/lightsens.urdf.xacro" />
   <xacro:include filename="$(find raspimouse_description)/urdf/sensors/lidar.urdf.xacro" />
+  <xacro:include filename="$(find raspimouse_description)/urdf/sensors/rgb_camera.xacro" />
 
   <!-- =============== Link & Joint =============== -->
   <!-- Base -->
@@ -98,19 +99,10 @@
     </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:rgb_camera_settings
+    use_gazebo="$(arg use_gazebo)"
+    use_rgb_camera="$(arg use_rgb_camera)" />
 
   <xacro:if value="$(arg use_gazebo)">
     <xacro:gazebo_diffdrive_settings

diff --git a/urdf/sensors/rgb_camera.xacro b/urdf/sensors/rgb_camera.xacro
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<robot name="rgb_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find raspimouse_description)/urdf/common.xacro" />
+
+  <xacro:macro name="rgb_camera_settings"
+    params="use_gazebo
+            use_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>
+
+  <xacro:if value="${use_gazebo}">
+    <gazebo>
+      <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+        <render_engine>ogre2</render_engine>
+      </plugin>
+    </gazebo>
+
+    <gazebo reference="camera_link">
+      <sensor name="camera_link" type="camera">
+        <update_rate>30.0</update_rate>
+        <always_on>true</always_on>
+        <ignition_frame_id>camera_link</ignition_frame_id>
+        <pose>0 0 0 0 0 0</pose>
+        <topic>/camera/color/image_raw</topic>
+        <camera name="rgb_camera">
+          <horizontal_fov>1.20428</horizontal_fov>
+          <image>
+            <width>1920</width>
+            <height>1080</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+        </camera>
+      </sensor>
+    </gazebo>
+  </xacro:if>
+</xacro:macro>
+
+</robot>
diff --git a/urdf/wheel/diffdrive.gazebo.xacro b/urdf/wheel/diffdrive.gazebo.xacro
@@ -20,6 +20,7 @@
         </plugin>
       </xacro:if>
     </gazebo>
+
   </xacro:macro>
 
 </robot>