Waypointによるナビゲーションサンプルの追加 (#16)

* waypointのサンプルプログラムとlaunchファイルを追加

* 初期位置のwとzを入れ替え

* maintainerを修正

* package.xmlのmaintainerを修正

* コードフォーマットを修正

* README整備

* waypointサンプルの説明や文体を修正

* 見栄えのために改行を追加

* テーブルコンテンツやサンプル項目を修正

* ContributingをTable of Contentsに追加

* waypointの指定を関数からdegreeで指定するように変更

* 新規のルールにしたがってフォーマット

* モジュールのimport順を修正

* コード修正にあたり、READMEを修正

* SLAMのサンプルをraspimouse_slamのREADME内に移動

* raspimouse_navigationパッケージのREADMEを更新

* WaypointSampleのREADMEを追加

* 目次を終始絵

* エスケープシーケンスを追加

* 過剰なリンクを削除して目次も整理

* 不要な目次の項目を削除

* リンクミスを修正

* 画像の改行を修正

* waypointフォロワーに変更してrvizの画角を変更

* waypoint指定のプログラムを整理

* コメントを追加

* メタデータや説明項目を修正

* fixed code format

* 動作変更時の不具合を修正

* 再フォーマット修正

* レビューに沿って文章中の表現やtypoの修正

* コードブロックの指定ミスやtypoを修正

* リポジトリトップのREADMEのtypoなどを修正

* Quick Start 内の表現を修正

* raspimouse_slamの表現を修正

* リンクや表記ゆれを修正

* パッケージ動作の画像と動画を追加

* fixed typo

Co-authored-by: YusukeKato <YusukeKato@users.noreply.github.com>

* エディタの設定ファイルの追跡を削除

* 冗長な説明を削除

* Waypointで指定するPoseStampedにz=0を明示するように修正

Co-authored-by: YusukeKato <YusukeKato@users.noreply.github.com>

* ファイルパスのリンク修正

Co-authored-by: YusukeKato <YusukeKato@users.noreply.github.com>

* navigationのリンクを修正

* Waypointの座標やタイムアウトの設定を修正しました。

* WaypointサンプルのREADME内での説明を改良

* Navigation2の表記ゆれをNav2に統一

Co-authored-by: YusukeKato <YusukeKato@users.noreply.github.com>

* Copyrightの年を修正

Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

* 章の階層偏光

Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

* 回りくどい言い回しを修正

Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

* Waypointを姿勢という言葉で表現するように偏光

Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

* Noteの項目を使用

Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

* 指示語を明確な表現に変更

Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

* ゲームパッドの注釈を追加

* raspimouse_navigation_examplesの概要を修正

* ゲームパッドの冒頭説明を削除

* <br>を削除

* 存在しない見出しの削除

* サンプルの概要を追加

* typoなどを修正

* サンプルプログラムの概要説明を修正

* Usageの削除と、その周辺を整備

* 冗長な文章を修正

* Nav2のリンク追加

* 実機とGazebo両方の手順を追加

* SLAMに実機とGazeboの手順を追加

* 過剰な改行を削除

* Navigationの起動を実行を分けた

* 目次追加

* リンクミス修正

* 不要な太字部分を削除しました。

---------

Co-authored-by: YusukeKato <YusukeKato@users.noreply.github.com>
Co-authored-by: Kuwamai <kuwagata@rt-net.jp>

Author: 3 people

.gitignore

@@ -102,4 +102,5 @@ Temporary Items
 # .nfs files are created when an open file is removed but is still being accessed
 .nfs*
 
-
+# editor's setting files
+.vscode/

README.md

@@ -1,6 +1,122 @@
 # raspimouse_navigation
 
-パラメータについては以下を参照すること。
-- [Navigation2：初めてのロボットセットアップガイド](https://docs.nav2.org/setup_guides/index.html)
-- [Navigation2：パラメータチューニング方法](https://docs.nav2.org/tuning/index.html)
-- [Navigation2：パラメータ一覧](https://docs.nav2.org/configuration/index.html)
+[Nav2](https://docs.nav2.org/)パッケージを使用してRaspberry Pi Mouseを自律移動させるパッケージです。
+
+## Table of Contents
+
+- [raspimouse\_navigation](#raspimouse_navigation)
+  - [Table of Contents](#table-of-contents)
+  - [Navigation](#navigation)
+      - [Setup](#setup)
+        - [Using Raspberry Pi Mouse](#using-raspberry-pi-mouse)
+        - [Using Gazebo](#using-gazebo)
+      - [Launching Navigation](#launching-navigation)
+        - [Using Raspberry Pi Mouse](#using-raspberry-pi-mouse-1)
+        - [Using Gazebo](#using-gazebo-1)
+      - [Running Navigation](#running-navigation)
+  - [Parameters](#parameters)
+
+## Navigation
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/navigation_ros2_with_raspimouse_model.png width=500 />
+
+> [!NOTE]
+>　サンプルの実行には、Raspberry Pi MouseとRemote PCが同じネットワーク上で同じ`ROS_DOMAIN_ID`を指定している必要があります。
+
+### Setup
+
+#### Using Raspberry Pi Mouse
+
+Raspberry Pi Mouse上で、次のコマンドを実行します。Raspberry Pi MouseのモータとLiDARを制御するためのノードを起動します。
+
+```sh
+# RPLIDAR A1の場合
+ros2 launch raspimouse_navigation robot_navigation.launch.py lidar:=rplidar
+# LDS-01の場合
+ros2 launch raspimouse_navigation robot_navigation.launch.py lidar:=lds
+# URG-04LX-UG01の場合
+ros2 launch raspimouse_navigation robot_navigation.launch.py lidar:=urg lidar_port:=/dev/ttyACM0
+```
+
+#### Using Gazebo
+
+以下のコマンドを実行し、Gazebo上でRaspberry Pi Mouseを起動します。Gazebo上での実行には、[raspimouse_sim](https://github.com/rt-net/raspimouse_sim/tree/jazzy)パッケージのインストールが必要です。
+
+```sh
+# RPLIDAR A1の場合
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=rplidar
+# LDS-01の場合
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=lds
+# URG-04LX-UG01の場合
+ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
+``` 
+
+### Launching Navigation
+
+自己位置推定と経路生成用のノードを起動し、RVizを立ち上げます。
+引数のmapパラメータには、SLAMで生成した地図（.yamlファイル）を指定してください。
+
+#### Using Raspberry Pi Mouse
+
+Remote PC上で、次のコマンドを実行します。
+
+```sh
+ros2 launch raspimouse_navigation pc_navigation.launch.py map:=$HOME/MAP_NAME.yaml
+```
+
+#### Using Gazebo
+
+Gazebo上のRaspberry Pi Mouseに対して実行する場合は`use_sim_time:=true`オプションを指定します。
+
+```sh
+ros2 launch raspimouse_navigation pc_navigation.launch.py map:=$HOME/MAP_NAME.yaml use_sim_time:=true 
+```
+
+### Running Navigation
+
+RVizが起動したら、初期位置・姿勢を合わせます。RVizの画面上部の*2D Pose Estimate*をクリックします。
+
+地図上でRaspberry Pi Mouseが存在すべき尤もらしい位置をクリックし、**そのままホールド**します。
+
+ホールドしながらカーソルを動かし、表示されている矢印の向きをRaspberry Pi Mouseの尤もらしい向きに合わせてからボタンを離します。
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/navigation_ros2_setting_initialpose.gif width=500 />
+
+初期位置・姿勢の指示が完了したら、次は目標位置・姿勢を指示します。RVizの画面上部の*Navigation2 Goal*をクリックします。
+
+地図上で、初期位置・姿勢を合わせた時と同様に、地図上をクリックして目標位置を、ホールドしたままカーソルを動かして目標姿勢を指示します。目標姿勢の指示が完了すると、Raspberry Pi Mouseが自律移動を開始します。
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/navigation_ros2_setting_goalpose.gif width=500 />
+
+### Stopping the robot
+
+下記画像のようなナビゲーション用のパネルがRViz左下に表示されます。
+*Cancel*ボタンを押すと自律移動が中断されます。
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/navigation_ros2_rviz_panel.png width=300 />
+
+Raspberry Pi Mouseを停止させる別の方法として、モータへの電源供給を止める方法もあります。
+
+次のコマンドを実行すると、ソフトウェア側からモータ電源をON / OFFできます。
+
+```sh
+# モータ電源をOFFにする
+ros2 service call /motor_power std_srvs/srv/SetBool data:\ false
+# モータ電源をONにする
+ros2 service call /motor_power std_srvs/srv/SetBool data:\ true
+```
+
+安全に気をつけながらRaspberry Pi Mouseに搭載されたスイッチを操作してモータ用電源をOFFにします。
+
+## Parameters
+
+- `use_sim_time`
+  - Type: `bool`
+  - Default: `false`
+  - シミュレーション動作時は`true`、実機動作時は`false`を指定します。このパラメータは内部で起動される`Nav2`関連ノードで使用されます。
+
+[Nav2](https://docs.nav2.org/)のパラメータについては、以下を参照してください。
+
+- [Nav2：初めてのロボットセットアップガイド](https://docs.nav2.org/setup_guides/index.html)
+- [Nav2：パラメータチューニング方法](https://docs.nav2.org/tuning/index.html)
+- [Nav2：パラメータ一覧](https://docs.nav2.org/configuration/index.html)

raspimouse_navigation/README.md

@@ -65,7 +65,9 @@ def generate_launch_description():
 
     # Launch files and Nodes #
     nav2_node = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
+        PythonLaunchDescriptionSource(
+            [nav2_launch_file_dir, '/bringup_launch.py']
+        ),
         launch_arguments={
             'map': map_yaml_file,
             'params_file': params_file,

raspimouse_navigation/launch/pc_navigation.launch.py

@@ -39,7 +39,9 @@ def generate_launch_description():
     )
 
     declare_arg_lidar_frame = DeclareLaunchArgument(
-        'lidar_frame', default_value='laser', description='Set lidar frame name.'
+        'lidar_frame',
+        default_value='laser',
+        description='Set lidar frame name.',
     )
 
     declare_arg_namespace = DeclareLaunchArgument(
@@ -51,7 +53,8 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             [
                 os.path.join(
-                    get_package_share_directory('hls_lfcd_lds_driver'), 'launch'
+                    get_package_share_directory('hls_lfcd_lds_driver'),
+                    'launch',
                 ),
                 '/hlds_laser.launch.py',
             ]
@@ -71,7 +74,9 @@ def generate_launch_description():
     rplidar_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
-                os.path.join(get_package_share_directory('rplidar_ros'), 'launch'),
+                os.path.join(
+                    get_package_share_directory('rplidar_ros'), 'launch'
+                ),
                 '/rplidar.launch.py',
             ]
         ),
@@ -92,7 +97,9 @@ def generate_launch_description():
     robot_description_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
-                os.path.join(get_package_share_directory('raspimouse_slam'), 'launch/'),
+                os.path.join(
+                    get_package_share_directory('raspimouse_slam'), 'launch/'
+                ),
                 'description.launch.py',
             ]
         ),
@@ -107,7 +114,9 @@ def generate_launch_description():
         output='screen',
         parameters=[
             os.path.join(
-                get_package_share_directory('raspimouse_slam'), 'config', 'mouse.yaml'
+                get_package_share_directory('raspimouse_slam'),
+                'config',
+                'mouse.yaml',
             )
         ],
     )

raspimouse_navigation/launch/robot_navigation.launch.py

@@ -165,7 +165,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 8.0
-        inflation_radius: 0.5
+        inflation_radius: 0.1
       always_send_full_costmap: True
 
 global_costmap:
@@ -202,7 +202,7 @@ global_costmap:
 
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 8.0
+        cost_scaling_factor: 16.0
         inflation_radius: 0.5
       always_send_full_costmap: True