Merge pull request #54 from rt-net/ros2

1.2.0リリースのためにros2ブランチの変更差分をhumble-develブランチへマージ
rt-net · Mar 6, 2024 · 259e567 · 259e567
2 parents 833a5e2 + 6db4f04
commit 259e567
Show file tree

Hide file tree

Showing 9 changed files with 123 additions and 50 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -2,6 +2,22 @@
 Changelog for package raspimouse_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.2.0 (2024-03-05)
+------------------
+* シミュレータ環境でscanトピックをpublish (`#52 <https://github.com/rt-net/raspimouse_description/issues/52>`_)
+* camera_downwardがtrueのときRGBカメラが斜め下を向くように変更 (`#50 <https://github.com/rt-net/raspimouse_description/issues/50>`_)
+  Co-authored-by: Shota Aoki <[email protected]>
+* READMEにRGBカメラを表示するコマンドを追記 (`#47 <https://github.com/rt-net/raspimouse_description/issues/47>`_)
+* Gazebo上で画像トピックを配信できるように変更 (`#46 <https://github.com/rt-net/raspimouse_description/issues/46>`_)
+* RGBカメラのモデルを表示できるように変更 (`#45 <https://github.com/rt-net/raspimouse_description/issues/45>`_)
+  Co-authored-by: Shota Aoki <[email protected]>
+* cmd_velとodomのトピック名をremapping (`#44 <https://github.com/rt-net/raspimouse_description/issues/44>`_)
+* controller managerが起動するように変更 (`#43 <https://github.com/rt-net/raspimouse_description/issues/43>`_)
+  Co-authored-by: Shota Aoki <[email protected]>
+* robot_description_loaderを実装 (`#42 <https://github.com/rt-net/raspimouse_description/issues/42>`_)
+  Co-authored-by: Daisuke Sato <[email protected]>
+* Contributors: YusukeKato
+
 1.1.0 (2023-11-07)
 ------------------
 * READMEにRGBカメラを表示するコマンドを追記 (`#47 <https://github.com/rt-net/raspimouse_description/issues/47>`_)

diff --git a/README.md b/README.md
@@ -51,6 +51,16 @@ 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
 

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 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,

diff --git a/package.xml b/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>raspimouse_description</name>
-  <version>1.1.0</version>
+  <version>1.2.0</version>
   <description>The raspimouse_description package</description>
   <maintainer email="[email protected]">RT Corporation</maintainer>
 

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
@@ -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)
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
@@ -141,6 +143,10 @@
       robot_namespace="$(arg sensor_namespace)"
       robotdescription_namespace="$(arg robot_namespace)" />
 
+  </xacro:if>
+
+  <xacro:if value="$(arg use_gazebo)">
+
     <xacro:if value="${'$(arg lidar)' == 'urg'}">
       <xacro:lidar_gazebo sensor_link_name="$(arg lidar_frame)" base_rad="0" rad_range="4.71" min_range="0.10" max_range="5.6" />
     </xacro:if>

diff --git a/urdf/sensors/lidar.gazebo.xacro b/urdf/sensors/lidar.gazebo.xacro
@@ -3,8 +3,12 @@
 
   <xacro:macro name="lidar_gazebo" params="sensor_link_name base_rad rad_range min_range max_range">
     <gazebo reference="${sensor_link_name}">
-      <sensor type="ray" name="${sensor_link_name}_sensor">
-        <pose>0 0 0 0 0 0</pose>
+      <sensor type="gpu_lidar" name="${sensor_link_name}_sensor">
+        <pose relative_to='${sensor_link_name}'>0 0 0 0 0 0</pose>
+        <ignition_frame_id>${sensor_link_name}</ignition_frame_id>
+        <frame_id>${sensor_link_name}</frame_id>
+        <topic>scan</topic>
+        <always_on>1</always_on>
         <visualize>$(arg laser_visual)</visualize>
         <update_rate>10</update_rate>
         <ray>
@@ -27,10 +31,6 @@
             <stddev>0.03</stddev>
           </noise>
         </ray>
-        <plugin name="gazebo_ros_${sensor_link_name}_controller" filename="libgazebo_ros_laser.so">
-          <topicName>scan</topicName>
-          <frameName>${sensor_link_name}</frameName>
-        </plugin>
       </sensor>
     </gazebo>
   </xacro:macro>

diff --git a/urdf/sensors/rgb_camera.xacro b/urdf/sensors/rgb_camera.xacro
@@ -3,51 +3,71 @@
 
   <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
-        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>
+    <xacro:if value="$(arg use_rgb_camera)">
+      <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+      <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>
+      </xacro:unless>
+
+      <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>
+            <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>
+              <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:if>
+  </xacro:macro>
 
-</robot>
+</robot>