こんにちは、倉澤ズズくんです。
本記事では、Raspberry Pi Mouseのraspimouse_slam_navigation_ros2パッケージに新たに追加された、Nav2を使用したWaypointによるナビゲーションのサンプルを紹介します。
また、前回の記事では、衝突監視機能のCollision Monitorを紹介しています。よろしければ、こちらもご覧ください。
Raspberry Pi Mouseの紹介
Raspberry Pi Mouse（ラズパイマウス）はメインボードにRaspberry Piを使用した左右独立二輪方式の小型移動プラットフォームロボットです。全長13cm、重さ700gとコンパクトであるため、持ち運びや開発スペースの確保も簡単です。LiDARやカメラを搭載可能で、学習・研究用途の幅広い開発が可能です。
ROS 2のサンプルパッケージを使うと、基本的な制御やSLAMによる地図作成も簡単に始められます。
Raspberry Pi Mouseの詳細な特徴については、下記の製品ページをご覧ください。
Raspberry Pi Mouseは下記の販売ページから購入できます。ROS 2パッケージは無償で公開しているため、購入前の参考にご利用ください。
Waypointサンプルの紹介
今回は、raspimouse_slam_navigation_ros2パッケージに新たに追加されたWaypointによるナビゲーションサンプルを紹介します。
ナビゲーションパッケージに関する説明やインストール方法の詳細は、以下のブログをご参照ください。
（本記事で紹介するサンプルはROS 2 Jazzy対応ですが、下記の記事はROS 2 Humble向けであり、実行するコマンドに差異がある点にご注意ください）
Waypointとは
ナビゲーションにおけるWaypointとは、ロボットの移動先を示す目標地点のことです。
複数のWaypointを指定することで、ロボットは各目標地点を順にたどって移動できます。
Nav2 Simple Commanderによるナビゲーションの実行
RVizからWaypointを指定することもできますが、本サンプルではNav2 Simple Commanderパッケージを使ってWaypointを指定し、ナビゲーションを実行します。
Nav2 Simple Commanderは、Nav2によるナビゲーションをPythonのスクリプトから簡単に行うためのライブラリです。ユーザーは、ROS 2のノード管理やアクション通信の詳細を意識せずに、Nav2のナビゲーション機能を自分のアプリケーションから呼び出せます。
プログラムの説明
Waypointサンプルの動作は、raspimouse_navigation_examplesパッケージ内の waypoint.py に記述されています。
以下では、項目ごとの処理を簡単に説明します。
ロボットの位置姿勢の定義
generate_pose()は、Waypointに必要なロボットの位置姿勢（PoseStamped型）を定義する関数です。平面上の位置(x, y)とロボットの向きを示すヨー角を引数として受け取り、姿勢をクォータニオンに変換します。
def generate_pose(navigator, x: float, y: float, deg: float) -> PoseStamped:
wp = PoseStamped()
wp.header.frame_id = 'map'
wp.header.stamp = navigator.get_clock().now().to_msg()
wp.pose.position.x = x
wp.pose.position.y = y
wp.pose.position.z = 0.0
rad = math.radians(deg)
wp.pose.orientation.x = 0.0
wp.pose.orientation.y = 0.0
wp.pose.orientation.z = math.sin(rad / 2.0)
wp.pose.orientation.w = math.cos(rad / 2.0)
return wp
初期位置姿勢とWaypointの指定
先程定義したgenerate_pose()で、初期位置姿勢と4つのWaypointを指定しています。これらの座標は実際の環境に合わせて調整してください。この後の実験では、指定するWaypointを変更した上で実施します。
# Initial pose
initial_pose = generate_pose(navigator=nav, x=0.0, y=0.0, deg=0.0)
nav.setInitialPose(initial_pose)
# Wait for the navigation stack to come up
nav.waitUntilNav2Active()
# Set goal_1
goal_poses = []
goal_pose1 = generate_pose(navigator=nav, x=0.0, y=0.5, deg=90.0)
goal_poses.append(goal_pose1)
# Set goal_2
goal_pose2 = generate_pose(navigator=nav, x=1.0, y=0.5, deg=0.0)
goal_poses.append(goal_pose2)
# Set goal_3
goal_pose3 = generate_pose(navigator=nav, x=1.0, y=-0.5, deg=-90.0)
goal_poses.append(goal_pose3)
# Set goal_4
goal_pose4 = generate_pose(navigator=nav, x=0.0, y=-0.5, deg=180.0)
goal_poses.append(goal_pose4)
上記サンプルプログラムのWaypointは、シミュレーション環境のLake Houseを想定したものです。Raspberry Pi Mouseのシミュレーションに関しては、raspimouse_simパッケージを参照してください。
ナビゲーションの開始
Commander APIのfollowWaypoint()メソッドを使ってナビゲーションを開始します。本メソッドは、与えられたWaypointを順番にたどるように移動ロボットをナビゲーションします。
nav.followWaypoints(goal_poses)
followWaypoint()の他にも、単一の目標位置姿勢に向かって移動するgoToPose()や、一括で全ての目標位置姿勢を考慮した経路を計画して移動するgoThroughPoses()といったメソッドがあります（Commander API）。
フィードバック処理
followWaypoints()は非ブロッキングな処理なので、完了までwhile文で待機します。ナビゲーション実行中はgetFeedback()によって今何番目のWaypointに向かっているか取得することができます。
また、本サンプルではwhile文中で実行時間を計測することで、90秒経過時に新しいWaypointを登録したり、180秒経過時にナビゲーション動作を終了させたりしています。
i = 0
root_changed = False
while not nav.isTaskComplete():
# Display feedback every 5 cycles
i = i + 1
feedback = nav.getFeedback()
if feedback and i % 5 == 0:
print(
'Executing current waypoint: '
+ str(feedback.current_waypoint + 1)
+ '/'
+ str(len(goal_poses)),
flush=True,
)
# Update timestamp
now = nav.get_clock().now()
# Some navigation timeout to demo cancellation
if now - nav_start > Duration(seconds=180.0):
nav.cancelTask()
# Some follow waypoints request change to demo preemption
if now - nav_start > Duration(seconds=90.0):
if not root_changed:
goal_pose = generate_pose(
navigator=nav, x=0.0, y=0.0, deg=0.0
)
goal_poses = [goal_pose]
nav_start = now
nav.followWaypoints(goal_poses)
print('Changed goal!!')
root_changed = True
結果の表示
ナビゲーションの終了後、結果に応じたメッセージを出力して終了します。
# Do something depending on the return code
result = nav.getResult()
if result == TaskResult.SUCCEEDED:
print('Goal succeeded!')
elif result == TaskResult.CANCELED:
print('Goal was canceled!')
elif result == TaskResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
nav.lifecycleShutdown()
exit(0)
Raspberry Pi Mouseで動作確認
今回は、RPLIDAR A1を使用してRaspberry Pi Mouseで動作確認を行います。
実験環境
以下の画像のような迷路を作成しました。
画像右下のワイプは、RViz上の表示です。黄色い矢印は4つのWaypointを示しています。
ソースコード中のWaypointは、以下のように編集しました。
# Set goal_1
goal_poses = []
goal_pose1 = generate_pose(navigator=nav, x=0.25, y=-0.3, deg=-40.0)
goal_poses.append(goal_pose1)
# Set goal_2
goal_pose2 = generate_pose(navigator=nav, x=0.8, y=-0.36, deg=75.0)
goal_poses.append(goal_pose2)
# Set goal_3
goal_pose3 = generate_pose(navigator=nav, x=1.2, y=-0.3, deg=-80.0)
goal_poses.append(goal_pose3)
# Set goal_4
goal_pose4 = generate_pose(navigator=nav, x=1.25, y=-1.0, deg=-90.0)
goal_poses.append(goal_pose4)
実行方法
以下のコマンドを実行します。Waypointスクリプトを起動するlaunchファイルも用意しています。
ros2 launch raspimouse_navigation robot_navigation.launch.py lidar:=rplidar
# ターミナル１ ros2 launch raspimouse_navigation pc_navigation.launch.py map:=$HOME/MAP_NAME.yaml # ターミナル２ ros2 launch raspimouse_navigation_examples example.launch.py example:=waypoint
シミュレーション環境下で動作させるには、以下のコマンドを実行します。
# ターミナル1 ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=rplidar # ターミナル2 ros2 launch raspimouse_navigation pc_navigation.launch.py use_sim_time:=true map:=$HOME/MAP_NAME.yaml # ターミナル3 ros2 launch raspimouse_navigation_examples example.launch.py example:=waypoint
動作の様子
4つのWaypointに対して、逐次ナビゲーションが実行されていることが確認できます。
おわりに
今回は、新たに追加されたWaypointによるナビゲーションのサンプルを紹介しました。本サンプルは、スクリプトからWaypointを設定してナビゲーションを実行する構成になっており、自動巡回などの処理も手軽に構築できます。
ナビゲーションに使用しているNav2 Simple Commanderには、様々な機能が用意されているので、是非試してみてください。