シミュレータ環境でSLAMとNavigationを実行 (#77)

* LakeHouseのモデルをワールドに配置

* bridgeにscanトピックを追加

* センサのプラグインを追加

* READMEにカメラライントレースとSLAM&Navigationの実行方法を追記

* lakehouseワールドに30cmの黒いキューブを複数配置

* READMEに画像を追加

* 不足していた依存関係を追加

* コピペできるようにコマンドを修正

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

* 引数mapの説明を修正

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

* Copyrightの文面を修正

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

* 英語版READMEのコマンドもコピペできるように修正

---------

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

README.en.md

@@ -93,6 +93,79 @@ ros2 launch raspimouse_ros2_examples object_tracking.launch.py mouse:=false use_
 
 ![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_object_tracking.gif)
 
+### camera_line_follower 
+
+Terminal 1:
+
+```sh
+ros2 launch raspimouse_gazebo raspimouse_with_line_follower_field.launch.py use_rgb_camera:=true camera_downward:=true
+```
+
+Terminal 2:
+
+```sh
+ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py mouse:=false use_camera_node:=false
+```
+
+Terminal 3: Start
+
+```sh
+ros2 topic pub --once /switches raspimouse_msgs/msg/Switches "{switch0: false, switch1: false, switch2: true}"
+```
+
+Terminal 3: Stop
+```sh
+ros2 topic pub --once /switches raspimouse_msgs/msg/Switches "{switch0: true, switch1: false, switch2: false}"
+```
+
+For information on parameters in camera line follower, click [here](https://github.com/rt-net/raspimouse_ros2_examples/blob/master/README.en.md#parameters).
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_camerafollower_short.gif)
+
+### SLAM & Navigation
+
+#### SLAM
+
+Terminal 1:
+```sh
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
+```
+The lidar option supports `urg`, `lds`, and `rplidar`.
+
+Terminal 2:
+```sh
+ros2 launch raspimouse_ros2_examples teleop_joy.launch.py joydev:="/dev/input/js0" joyconfig:=f710 mouse:=false
+```
+
+Terminal 3:
+```sh
+ros2 launch raspimouse_slam pc_slam.launch.py
+```
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_slam.png)
+
+Terminal 4:
+```sh
+ros2 run nav2_map_server map_saver_cli -f ~/MAP_NAME
+```
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_slam_short.gif)
+
+#### Navigation
+
+Terminal 1:
+```sh
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
+```
+The lidar option supports `urg`, `lds`, and `rplidar`.
+
+Terminal 2:
+```sh
+ros2 launch raspimouse_navigation pc_navigation.launch.py map:=$HOME/MAP_NAME.yaml
+```
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_navigation_short.gif)
+
 ## Model data list
 
 ### course_curve_50x50cm
@@ -108,12 +181,12 @@ Panel size is 50 cm x 50 cm and line width is 4 cm.
 ![](./raspimouse_gazebo/models/course_straight_50x50cm/meshes/course_straight.jpg)
 
 ### cube_*cm_color-name
-Each cube is 5 cm, 7.5 cm, 10 cm, and 15 cm on a side.
-The cube colors are red, yellow, blue, and green.
+Each cube is 5 cm, 7.5 cm, 10 cm, and 15 cm, 30 cm on a side.
+The cube colors are red, yellow, blue, green and black.
 
 ![](https://rt-net.github.io/images/raspberry-pi-mouse/color_objects.png)
 
-### daeファイルについて
+### about dae files
 The dae file is edited in Blender 4.0.
 
 ## License

README.md

@@ -39,6 +39,7 @@ git clone -b ros2 https://github.com/rt-net/raspimouse_sim.git
 ```sh
 cd ~/ros2_ws/src
 git clone https://github.com/rt-net/raspimouse_ros2_examples.git
+git clone https://github.com/rt-net/raspimouse_slam_navigation_ros2.git
 git clone -b ros2 https://github.com/rt-net/raspimouse_description.git
 rosdep install -r -y -i --from-paths raspimouse*
 ```
@@ -93,6 +94,78 @@ ros2 launch raspimouse_ros2_examples object_tracking.launch.py mouse:=false use_
 
 ![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_object_tracking.gif)
 
+### RGBカメラを用いたライントレースサンプル
+
+端末1で次のコマンドを実行すると、ライントレースのサンプルコースが配置されたワールドが表示されます。
+```sh
+ros2 launch raspimouse_gazebo raspimouse_with_line_follower_field.launch.py use_rgb_camera:=true camera_downward:=true
+```
+
+端末2で次のコマンドを実行すると、カメラライントレースのノードが起動します。
+```sh
+ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py mouse:=false use_camera_node:=false
+```
+
+端末3で次のコマンドを実行すると、Raspberry Pi Mouseが走行を開始します。
+```sh
+ros2 topic pub --once /switches raspimouse_msgs/msg/Switches "{switch0: false, switch1: false, switch2: true}"
+```
+
+次のコマンドを実行すると、Raspberry Pi Mouseが停止します。
+```sh
+ros2 topic pub --once /switches raspimouse_msgs/msg/Switches "{switch0: true, switch1: false, switch2: false}"
+```
+
+カメラライントレースにおけるパラメータは[こちら](https://github.com/rt-net/raspimouse_ros2_examples?tab=readme-ov-file#parameters)を参照してください。
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_camerafollower_short.gif)
+
+### LiDARを用いたSLAMとNavigationのサンプル
+
+#### SLAM
+
+端末1で次のコマンドを実行すると、`Lake House`のモデルが配置されたワールドが表示されます。
+```sh
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
+```
+`lidar`は`urg`、`lds`、`rplidar`のいずれかを指定してください。
+
+端末2で次のコマンドを実行すると、Raspberry Pi Mouseをジョイスティックコントローラで操作できます。
+```sh
+ros2 launch raspimouse_ros2_examples teleop_joy.launch.py joydev:="/dev/input/js0" joyconfig:=f710 mouse:=false
+```
+
+端末3で次のコマンドを実行すると、SLAMが実行されます。
+```sh
+ros2 launch raspimouse_slam pc_slam.launch.py
+```
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_slam.png)
+
+端末4で次のコマンドを実行すると、作成した地図を保存できます。
+```sh
+ros2 run nav2_map_server map_saver_cli -f ~/MAP_NAME
+```
+`MAP_NAME`は任意の名前を指定できます。
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_slam_short.gif)
+
+#### Navigation
+
+端末1で次のコマンドを実行すると、`Lake House`のモデルが配置されたワールドが表示されます。
+```sh
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
+```
+`lidar`は`urg`、`lds`、`rplidar`のいずれかを指定してください。
+
+端末2で次のコマンドを実行すると、Navigationが実行されます。
+```sh
+ros2 launch raspimouse_navigation pc_navigation.launch.py map:=$HOME/MAP_NAME.yaml
+```
+引数`map`にはSLAMで作成した地図ファイルのパスを指定してください。
+
+![](https://rt-net.github.io/images/raspberry-pi-mouse/raspimouse_sim_navigation_short.gif)
+
 ## モデルデータ一覧
 
 ### course_curve_50x50cm
@@ -110,8 +183,8 @@ ros2 launch raspimouse_ros2_examples object_tracking.launch.py mouse:=false use_
 ![](./raspimouse_gazebo/models/course_straight_50x50cm/meshes/course_straight.jpg)
 
 ### cube_*cm_color-name
-それぞれ一辺5cm、7.5cm、10cm、15cmの立方体です。
-色は赤、黄、青、緑です。
+それぞれ一辺5cm、7.5cm、10cm、15cm、30cmの立方体です。
+色は赤、黄、青、緑、黒です。
 
 ![](https://rt-net.github.io/images/raspberry-pi-mouse/color_objects.png)
 

raspimouse_gazebo/launch/raspimouse_with_emptyworld.launch.py

@@ -135,6 +135,7 @@ def generate_launch_description():
         package='ros_gz_bridge',
         executable='parameter_bridge',
         arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
+                   '/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
                    '/camera/color/image_raw@sensor_msgs/msg/Image@gz.msgs.Image',
                    '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo'],
         output='screen'

raspimouse_gazebo/launch/raspimouse_with_lakehouse.launch.py

@@ -0,0 +1,48 @@
+# The MIT License (MIT)
+#
+# Copyright 2024 RT Corporation <support@rt-net.jp>
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy of
+# this software and associated documentation files (the "Software"), to deal in
+# the Software without restriction, including without limitation the rights to
+# use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+# the Software, and to permit persons to whom the Software is furnished to do so,
+# subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+# FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+# COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+# IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+# CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch_ros.actions import SetParameter
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    world_file = os.path.join(
+        get_package_share_directory('raspimouse_gazebo'),
+        'worlds',
+        'lakehouse.sdf')
+    world_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            get_package_share_directory('raspimouse_gazebo'),
+            '/launch/raspimouse_with_emptyworld.launch.py']),
+        launch_arguments={
+            'world_name': world_file
+            }.items()
+    )
+
+    return LaunchDescription([
+        SetParameter(name='use_sim_time', value=True),
+        world_launch
+    ])

raspimouse_gazebo/models/cube_30cm_black/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+
+<model>
+  <name>Black Cube 30cm</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>YusukeKato</name>
+    <email>yusuke.kato@rt-net.jp</email>
+  </author>
+
+  <description>
+    Cube
+  </description>
+</model>

raspimouse_gazebo/models/cube_30cm_black/model.sdf

@@ -0,0 +1,38 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+    <model name="cube_30cm_black">
+      <link name="cube_black_link">
+        <inertial>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="cube_black_collision">
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.3</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="cube_black_visual">
+          <material>
+            <ambient>0 0 0 1</ambient>
+            <diffuse>0 0 0 1</diffuse>
+            <specular>0 0 0 0</specular>
+            <emissive>0 0 0 1</emissive>
+          </material>
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.3</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+</sdf>

raspimouse_gazebo/package.xml

@@ -20,6 +20,8 @@
   <depend>raspimouse_fake</depend>
   <depend>raspimouse_description</depend>
   <depend>robot_state_publisher</depend>
+  <depend>diff_drive_controller</depend>
+  <depend>joint_state_broadcaster</depend>
 
   <export>
     <build_type>ament_cmake</build_type>