こんにちは。Preferred Roboticsの寺田です。
初めてのqiita投稿です。本記事ではkachakaのAPIを公開した記念にROS2のAPIでオープンソースのnavigation2を動かすことにチャレンジしてみます。
kachakaとは？
kachakaは、ロボットと家具が一体となって機能する、スマートファニチャープラットフォームです。 kachakaは専用のアプリや音声コマンドで操作可能で、家具を目的の場所まで自動で運ぶことができます。
先日公開されたAPIによりkachakaを、ユーザーが書いたコードから自由に動かすことができます。
棚をA地点からB地点へ運ぶようなハイレベルなインターフェイスでの利用例は、
等に記載しているので試してみて下さい。
今回は速度やlidarの情報を使って、基礎的な自律移動機能を動かしてみることを行います。
Kachakaには謹製のNavigationのスタックが動いています。本記事を参照すると、謹製のNavigationスタックを上書きすることができます。自分で作ったNavigationのスタックを動かしたり、ROS２のNavigationをカスタマイズする助けになると思います。
kachakaをROS2から動かす
kachaka体内ではgRPCのインターフェイスを公開しており、様々な言語からgRPCのAPIを叩くことで色々な動作をさせることができます。
このgRPCのインターフェイスをROS２に変換するブリッジを公開しているので、ROS２での開発も可能になっています。
下図の様にkachaka_grpc_ros2_bridgeをお手元のPCで動作させることでROS2からロボットを動かす仕組みになっています。
事前準備
本記事の内容を試すには
- kachaka本体
- 外部PC (ubuntu 22.04 推奨）
が必要です。
本記事はkachka本体のソフトが2.1以降になっていることを想定しています。
スマホから、アプリー＞設定ー＞アプリ情報ー＞kachakaー＞バージョン
が2.1以上になっていない場合は最新版へのバージョンアップをお願いいたします。
PCにDockerとROS2 Humbleの環境を入れます。インストール方法はこちらになります。
https://docs.ros.org/en/humble/Installation.html
環境が用意するのが大変な場合、今回の記事の内容はdockerでも動作確認していますのでdocker上にROS2環境を構築しても、動かすことができると思います。
rviz等も含めたdockerの環境の構築には下記の記事が参考になるります。
ROS２が用意できたら、gRPCとROS2を繋ぐ必要があります。
まずkachaka apiのコードを手元にcloneしてきます。
cd ~
git clone https://github.com/pf-robotics/kachaka-api.git
以下のコマンドでros2とgPRCのブリッジがスタートします。これでkachakaのROS2の機能が使えるようになります。
cd ~/kachaka-api/tools/ros2_bridge
./start_bridge.sh <kachakaのIPアドレス>
ROS2のtopic
利用可能なtopicを以下にまとめます。
| topic名 | type | 意味 |
|---|---|---|
| /goal_pose | geometry_msgs/msg/PoseStamped | 自律移動のゴール位置 |
| /kachaka/front_camera/camera_info | sensor_msgs/msg/CameraInfo | フロントカメラのパラメータ |
| /kachaka/front_camera/image_raw | sensor_msgs/msg/Image | フロントカメラの画像（非圧縮） |
| /kachaka/front_camera/image_raw/compressed | sensor_msgs/msg/Image | フロントカメラの画像（圧縮） |
| /kachaka/imu/imu | sensor_msgs/msg/Imu | Imuのデータ |
| /kachaka/layout/locations/list | kachaka_interfaces/msg/LocationList | 目標位置の名前のリスト |
| /kachaka/layout/shelves/list | kachaka_interfaces/msg/ShelfList | kachakaの棚の名前リスト |
| /kachaka/lidar/scan | sensor_msgs/msg/LaserScan | lidarが取得したscanデータ |
| /kachaka/manual_control/cmd_vel | geometry_msgs/msg/Twist | 指令速度、角速度 |
| /kachaka/mapping/map | nav_msgs/msg/OccupancyGrid | 現在読み込まれている地図 |
| /kachaka/object_detection/result | kachaka_interfaces/msg/ObjectDetectionListStamped | 人、棚等の物体認識結果 |
| /kachaka/odometry/odometry | nav_msgs/msg/Odometry | オドメトリ情報（タイヤの回転数から取得した位置、速度） |
| /kachaka_description/joint_states | /kachaka_description/joint_states | 関節角度 |
| /kachaka_description/robot_description | std_msgs/String | URDFで記述したロボットモデル |
| /tf | tf2_msgs/msg/TFMessage | 動的tf |
| /tf_static | tf2_msgs/msg/TFMessage | 静的tf |
ROS2でpub/subを試してみる
コマンドラインツールでtopicのpub/subを試してみます。
以下は適切なターミナルから実行してください。
source /opt/ros/humble/setup.bash
でhumble環境に入り、topicをsubしてみます。
ros2 topic echo /kachaka/odometry/odometry
これで
---
header:
stamp:
sec: 1694066377
nanosec: 971004486
frame_id: odom
child_frame_id: ''
pose:
pose:
position:
x: 0.06328201509224213
y: 0.08089029119230853
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.34910823770306504
w: 0.937082407458309
covariance:
- 223639829233756.9
- 231354824301255.6
...
の様にオドメトリの値が取得できます。
次にPCからtopicをpubして、ロボットを動かしてみます。
以下はその場で0.3[rad/s]で回転するコマンドです。
ros2 topic pub /kachaka/manual_control/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.0}, angular: {z: 0.3}}'
その場でkachakaが回転を始めたと思います。
次に、いよいよこれらのtopicを利用してnavigation2を動かしてみます。
Navigation2
navigation2はROSのnavigationスタックの後継版でROS2に対応しています。
navigation2は自律移動に関わるパッケージの集合で、
- mapの管理
- ロボットのマップ上の位置の推定
- A地点からB地点への障害物を避けたパスの生成
- 生成されたパスをフォローするコントローラ
- 与えられた格子専有地図とセンサ情報からコストマップを作る
- ロボットの振る舞いをBehavior Treeで管理する
- ロボットが停止してしまう状況になった場合の復帰
- 与えた複数の経由点をつないで動作させる
- ノードのライフサイクルの管理
- これらの機能をプラグインとして管理する
- センサの生の値から直帰の衝突を検出する
- pythonインターフェースによる簡単な指令
- 速度をスムーズにして動作させる機能
を含みます。
これらの機能を組み合わせてnavigation2は動きます。
この記事では、マップの管理と位置の推定はkachakaが持っているものを利用したほうが簡単なのでそのまま使い、それ以外のコストマップを作る部分やパスプラン、制御の部分をnavigation2から使っています。
ビルド
以下の手順でワークスペースを作ってビルドします。
- githubからkachakaでnavigation2を動かすサンプルの設定を持ってくる
- 必要なパッケージをワークスペースにコピー
- 依存関係を解決
- ビルド
の順番で実施します。以下のコマンドで環境を作成します。
git clone https://github.com/pf-robotics/kachaka-api
source /opt/ros/humble/setup.bash
mkdir -p ws/src
cp -r kachaka-api/ros2/kachaka_description/ ws/src
cp -r kachaka-api/ros2/demos/kachaka_nav2_bringup/ ws/src
cd ws/src
sudo rosdep init
rosdep update
sudo apt update
rosdep install -i --from-paths .
cd ..
colcon build
設定
以下のnodeを立ち上げます。
-
nav2_controller
pathのfollowerを実装しています。plugin方式になっておりいくつかのアルゴリズムを選択的に選んだり、または自分で実装することができます。
現在MPPI, DWB, Regulated Pure Pursuite, Rotate shimあたりが実装されているようです。- MPPI Model Predictive Controlを軌道追従に応用したものです。MPCとは違いサンプリングベースで確率的に最適化を行うところが特徴です。
- DWB ROS1のnavigationスタックで広く使われていたDWAの拡張で動的なWindowを使うことでより複雑な環境に対応できるようになっています。
- Regulate Pure Pursuite パスの少し前方のポイントと現在値の差分を利用してコントロールするpure pursuiteのアルゴリズムにより良く追従するなどの変更を加えたものです。
- Rotate Shim 開始方向を合わせるcontrollerで単独では使われずに他のコントローラーを動かす前に方向を合わせるために使われます。
ここではデフォルトであるDWBを利用してみます。
-
nav2_smoother
smootherはplannerにより計画されたパスをfollowerでなめらかに追従できる様にギザギザや、不連続性を低減させます。controllerと同じようにpluginのシステムになっておりいくつかのオプションを選択可能です。
- Simple Smoother 基本的なsmootherです。ロボットの運動的な拘束を考慮していないのでplannerレベルでノンホロノミックな拘束を考慮した計画をした場合は使うべきでは無いです。
- [Savitzky-Golay Smoother] (https://navigation.ros.org/configuration/packages/configuring-savitzky-golay-smoother.html) Savitzky-Golay filterに基づくsmootherです。ノンパラメトリックで高速ですが、大きな問題を修正することはできません。
-
nav2_planner
plannerはcostmapとstatic mapからゴールを結ぶ軌道を生成します。こちらもプラグインで以下のような選択肢が容易されています。
- NavFn NavFnはWavefront DijkstraもしくはA*を用いた基本的なplannerです。ロボットの形状は考慮されません。
- Smac Planner これは、Smac Planner 2D, Smac Hybrid-A* Planner, Smac Lattice Plannerの３種類のplanner含みます。 この中でも、Hybrid-A*, Smac Lattice Plannerを用いるロボットの角度を考慮した計画ができます。
- Theta Star Planner A*を任意の角度の直線がでるように改良したアルゴリズムです。
この記事ではここではNavFnを利用してみます。
これ以外にBehavior Treeの設定をするとより詳細にロボットの挙動を設定できます。
詳しくは nav2のドキュメントを参照してください。
設定は nav2_params.yaml にまとめています。ROS1と違ってnode毎にlocal なパラメータになります。
起動
以下のコマンドでnavigationを起動します。上述のros2grpcブリッジが起動している必要があります。
source install/setup.bash
ros2 launch kachaka_nav2_bringup navigation_launch.py
可視化
rvizで可視化します。表示されているのはPreferred Roboticsのオフィスになります。
source install/setup.bash
rviz2 -d install/kachaka_bringup/share/kachaka_nav2_bringup/rviz/kachaka-nav.rviz
rvizのウインドウ上の2D Goal Poseから移動させたいgoalを引くとパスが計画されてロボットが動き出します。
動作確認
rviz2の 2D Goal Poseで行きたい目的地を指定します。この状態で緑のパスが出て無事にkachakaをnavigationさせることができました。この際にcostmapも実際の障害物に合わせて変化していることを確認できます。
まとめ
kachakaのnavigationを上書きして、ロボットが動作することが確かめられました。リモートブレインですが、ある程度安定してnavigationができています。
発展として以下のようなことを進められます。
- SLAMも差し替えてみる
- 画像もつかってみる
- 物体認識結果と合わせてみる
- 自作のControllerやpathプランをpluginで書いて試す
興味のある方は2023/9/15に開催される、カチャカAPIワークショップ へのご参加をおすすめします。
読んでくださりありがとうございました。
本記事を読んでいただき、ありがとうございます。