こんにちは。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を叩くことで色々な動作をさせることができます。

GitHub

この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_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_bringup navigation_launch.py

可視化

rvizで可視化します。表示されているのはPreferred Roboticsのオフィスになります。

source install/setup.bash
rviz2 -d install/kachaka_bringup/share/kachaka_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ワークショップへのご参加をおすすめします。
読んでくださりありがとうございました。

本記事を読んでいただき、ありがとうございます。

kachakaをROS2のnavigation2から動かす