camera_downwardがtrueのときRGBカメラが斜め下を向くように変更

rt-net · Nov 20, 2023 · a54fd14 · a54fd14
1 parent 8eb245c
commit a54fd14
Show file tree

Hide file tree

Showing 5 changed files with 88 additions and 25 deletions.
diff --git a/launch/display.launch.py b/launch/display.launch.py
@@ -44,11 +44,16 @@ def generate_launch_description():
         'use_rgb_camera',
         default_value='false',
         description='Set "true" to mount rgb camera.')
+    declare_arg_camera_downward = DeclareLaunchArgument(
+        'camera_downward',
+        default_value='false',
+        description='Set "true" to camera down.')
 
     description_loader = RobotDescriptionLoader()
     description_loader.lidar = LaunchConfiguration('lidar')
     description_loader.lidar_frame = LaunchConfiguration('lidar_frame')
     description_loader.use_rgb_camera = LaunchConfiguration('use_rgb_camera')
+    description_loader.camera_downward = LaunchConfiguration('camera_downward')
 
     push_ns = PushRosNamespace([LaunchConfiguration('namespace')])
 
@@ -78,6 +83,7 @@ def generate_launch_description():
         declare_arg_namespace,
         declare_arg_use_rviz,
         declare_arg_use_rgb_camera,
+        declare_arg_camera_downward,
         push_ns,
         rsp,
         jsp,

diff --git a/raspimouse_description/robot_description_loader.py b/raspimouse_description/robot_description_loader.py
@@ -36,6 +36,7 @@ def __init__(self):
         self.lidar_frame = 'laser'
         self.use_gazebo = 'false'
         self.use_rgb_camera = 'false'
+        self.camera_downward = 'false'
         self.gz_control_config_package = ''
         self.gz_control_config_file_path = ''
 
@@ -47,6 +48,7 @@ def load(self):
                 ' lidar_frame:=', self.lidar_frame,
                 ' use_gazebo:=', self.use_gazebo,
                 ' use_rgb_camera:=', self.use_rgb_camera,
+                ' camera_downward:=', self.camera_downward,
                 ' gz_control_config_package:=', self.gz_control_config_package,
                 ' gz_control_config_file_path:=', self.gz_control_config_file_path
                 ])
diff --git a/test/test_robot_description_loader.py b/test/test_robot_description_loader.py
@@ -95,6 +95,7 @@ def test_use_rgb_camera():
     # use_rgb_cameraが変更され、xacroにRGB Cameraがセットされることを期待
     rdl = RobotDescriptionLoader()
     rdl.use_rgb_camera = 'true'
+    rdl.camera_downward = 'false'
     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)
@@ -105,6 +106,19 @@ def test_camera_link():
     rdl = RobotDescriptionLoader()
     rdl.use_gazebo = 'true'
     rdl.use_rgb_camera = 'true'
+    rdl.camera_downward = 'false'
     rdl.gz_control_config_package = 'raspimouse_description'
     rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
     assert 'camera_link' in exec_load(rdl)
+
+
+def test_camera_downward():
+    # camera_downwardが変更され、カメラが斜め30度下を向くことを期待
+    rdl = RobotDescriptionLoader()
+    rdl.use_gazebo = 'true'
+    rdl.use_rgb_camera = 'true'
+    rdl.camera_downward = 'true'
+    rdl.gz_control_config_package = 'raspimouse_description'
+    rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml'
+    print(exec_load(rdl))
+    assert '<pose>0 0 0 0 0.523 0</pose>' in exec_load(rdl)
diff --git a/urdf/raspimouse.urdf.xacro b/urdf/raspimouse.urdf.xacro
@@ -14,6 +14,7 @@
   <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="camera_downward" default="false" />
   <xacro:arg name="gz_control_config_package" default="" />
   <xacro:arg name="gz_control_config_file_path" default="" />
 
@@ -102,7 +103,8 @@
   <!-- =============== Gazebo =============== -->
   <xacro:rgb_camera_settings
     use_gazebo="$(arg use_gazebo)"
-    use_rgb_camera="$(arg use_rgb_camera)" />
+    use_rgb_camera="$(arg use_rgb_camera)"
+    camera_downward="$(arg camera_downward)" />
 
   <xacro:if value="$(arg use_gazebo)">
     <xacro:gazebo_diffdrive_settings

diff --git a/urdf/sensors/rgb_camera.xacro b/urdf/sensors/rgb_camera.xacro
@@ -6,17 +6,30 @@
   <xacro:macro name="rgb_camera_settings"
     params="use_gazebo
             use_rgb_camera
+            camera_downward
             ">
 
   <xacro:if value="$(arg use_rgb_camera)">
     <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
-    <xacro:sensor_d435
+    <xacro:if value="$(arg camera_downward)">
+      <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>
+          <origin xyz="0.08 0.0 0.055" rpy="0 0.523 0"/>
+      </xacro:sensor_d435>
+    </xacro:if>
+
+    <xacro:unless value="$(arg camera_downward)">
+      <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:unless>
   </xacro:if>
 
   <xacro:if value="${use_gazebo}">
@@ -26,27 +39,53 @@
       </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 value="${camera_downward}">
+      <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.523 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:unless value="${camera_downward}">
+      <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:unless>
   </xacro:if>
 </xacro:macro>