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

Co-authored-by: Shota Aoki <s.aoki@rt-net.jp>

Author: YusukeKato

README.md

@@ -51,6 +51,15 @@ Similarly, display a RGB Camera mounted robot model with the following command:
 ```sh
 ros2 launch raspimouse_description display.launch.py use_rgb_camera:=true
 ```
+![](https://rt-net.github.io/images/raspberry-pi-mouse/mouse_with_rgb_camera.png)
+
+RGB Camera can be pointed down with the following command:
+
+```sh
+ros2 launch raspimouse_description display.launch.py use_rgb_camera:=true camera_downward:=true
+```
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/mouse_with_rgb_camera_downward.png)
 
 ## LICENSE
 

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 point the camera downwards.')
 
     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,

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
                 ])

test/test_robot_description_loader.py

@@ -22,6 +22,7 @@
 from raspimouse_description.robot_description_loader import RobotDescriptionLoader
 from launch.launch_context import LaunchContext
 import pytest
+import math
 
 
 def exec_load(loader):
@@ -108,3 +109,15 @@ def test_camera_link():
     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'
+    camera_angle = math.radians(30)
+    assert '<pose>0 0 0 0 ' + str(camera_angle) + ' 0</pose>' in exec_load(rdl)

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

urdf/sensors/rgb_camera.xacro

@@ -3,20 +3,35 @@
 
   <xacro:include filename="$(find raspimouse_description)/urdf/common.xacro" />
 
+  <xacro:property name="DOWNWARD_CAMERA_ANGLE" value="${radians(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 ${DOWNWARD_CAMERA_ANGLE} 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>
+          <origin xyz="0.08 0.0 0.055" rpy="0 0 0"/>
+      </xacro:sensor_d435>
+    </xacro:unless>
   </xacro:if>
 
   <xacro:if value="${use_gazebo}">
@@ -31,7 +46,12 @@
         <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>
+        <xacro:if value="${camera_downward}">
+          <pose>0 0 0 0 ${DOWNWARD_CAMERA_ANGLE} 0</pose>
+        </xacro:if>
+        <xacro:unless value="${camera_downward}">
+          <pose>0 0 0 0 0 0</pose>
+        </xacro:unless>
         <topic>/camera/color/image_raw</topic>
         <camera name="rgb_camera">
           <horizontal_fov>1.20428</horizontal_fov>