１．はじめに
ROS2のPlanSys2を触る機会があったので、「Getting Started」チュートリアルを試した時の 気づきをまとめました。
ROS2 Planning System — ROS2 Planning System 2 1.0.0 documentation
ざっくりPlansys2とは
ロボットをはじめとした自動運転する機械は、 決まった動作を決まった順番に実行、あるいは繰り返す、すなわちシーケンス制御により、動きが決定されます。
現場の環境に応じて「決まった順番」を変えられる仕組みを考えたときに、その順番の計画を立て直す「プランニング問題」に直面します。
前述したプランニング問題を解決することを目的とした技術の一つが、自動計画手法です。
計画を立てることが出来る手法のうち、主な3つの方法として以下を挙げます。
- PDDLベースによる計画
- タイムラインとスケジューリングによる計画
- 制約ベースのスケジューリングによる計画
Plansys2はROS2における「プランニング問題」の解決ツールの一つで、 PDDLベースによる計画システムです。
PDDLとは「Planning Domain Descrition Language」の略で、 直訳すると「計画ドメイン定義言語」です。
Plansys2は主に4つのノードから構成されます。
- Domain Expert
- PDDLモデル情報（タイプ、述語、関数、およびアクション）
- Problem Export
- モデルを構成する現在のインスタンス、Predicate、関数、および目標
- Planner
- ドメインおよびProblem Expertsに含まれる情報を使用して、計画（一連のアクション）を生成
Executor
- プランを取得し、アクションパフォーマー（各アクションを実装するROS2ノード）をアクティブ化し実行
- プラン→ビヘイビアツリーに変換して実行
参考資料
- 「ロボット配備計画」の最新技術とは 未来の介護施設ではロボットは普及している？ | AIケアラボ
２．インストール
実行環境
$ uname -a Linux 6.5.0-28-generic #29~22.04.1-Ubuntu SMP PREEMPT_DYNAMIC Thu Apr 4 14:39:20 UTC 2 x86_64 x86_64 x86_64 GNU/Linux $ lsb_release -a No LSB modules are available. Distributor ID: Ubuntu Description: Ubuntu 22.04.4 LTS Release: 22.04 Codename: jammy
インストール
ROS2 humbleの基本パッケージがインストール済みの状態とします。
# plansys2関連パッケージを追加でインストール。 $ sudo apt install ros-humble-plansys2-* -y # あとでビルドに必要なパッケージもインストール $ sudo apt install ros-humble-navigation2 ros-humble-test-msgs -y
３．「Getting Started」チュートリアル
（１）準備
- こちらの手順に従って進める。
# 環境変数 $ source /opt/ros/humble/setup.bash # 作業ディレクトリを作って入る $ mkdir -p ~/gitwork/p2tuto $ cd !$ # チュートリアルのソースコードをダウンロード $ git clone -b humble https://github.com/IntelligentRoboticsLabs/ros2_planning_system_examples.git src # ここからは、チュートリアルの手順に従う # ビルド $ colcon build --symlink-install Starting >>> plansys2_bt_example Starting >>> plansys2_cascade_example Starting >>> plansys2_multidomain_example Starting >>> plansys2_patrol_navigation_example Starting >>> plansys2_simple_example Finished <<< plansys2_simple_example [0.41s] Finished <<< plansys2_cascade_example [0.42s] Finished <<< plansys2_multidomain_example [0.44s] Finished <<< plansys2_patrol_navigation_example [12.2s] [Processing: plansys2_bt_example] --- stderr: plansys2_bt_example In file included from /home/xxxxx/gitwork/plansys2/src/plansys2_bt_example/src/nav2_sim_node.cpp:20: /opt/ros/humble/include/tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.h:35:2: warning: #warning This header is obsolete, please include tf2_geometry_msgs/tf2_geometry_msgs.hpp instead [-Wcpp] 35 | #warning This header is obsolete, please include tf2_geometry_msgs/tf2_geometry_msgs.hpp instead | ^~~~~~~ In file included from /home/xxxxx/gitwork/plansys2/src/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp:23: /opt/ros/humble/include/tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.h:35:2: warning: #warning This header is obsolete, please include tf2_geometry_msgs/tf2_geometry_msgs.hpp instead [-Wcpp] 35 | #warning This header is obsolete, please include tf2_geometry_msgs/tf2_geometry_msgs.hpp instead | ^~~~~~~ --- Finished <<< plansys2_bt_example [1min 12s] Summary: 5 packages finished [1min 12s] 1 package had stderr output: plansys2_bt_example # 若干数のワーニングを確認 # 引き続き、チュートリアルの手順に従う $ rosdep install --from-paths src --ignore-src -r -y ERROR: your rosdep installation has not been initialized yet. Please run: sudo rosdep init rosdep update # エラー発生したので、上記の誘導の通りにコマンドを入力 $ sudo rosdep init Wrote /etc/ros/rosdep/sources.list.d/20-default.list $ rosdep update reading in sources list data from /etc/ros/rosdep/sources.list.d Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/osx-homebrew.yaml Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/base.yaml Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/python.yaml Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/ruby.yaml Hit https://raw.githubusercontent.com/ros/rosdistro/master/releases/fuerte.yaml Query rosdistro index https://raw.githubusercontent.com/ros/rosdistro/master/index-v4.yaml Skip end-of-life distro "ardent" Skip end-of-life distro "bouncy" Skip end-of-life distro "crystal" Skip end-of-life distro "dashing" Skip end-of-life distro "eloquent" Skip end-of-life distro "foxy" Skip end-of-life distro "galactic" Skip end-of-life distro "groovy" Add distro "humble" Skip end-of-life distro "hydro" # 改めて、チュートリアルの手順に戻る $ rosdep install --from-paths src --ignore-src -r -y $ colcon build --symlink-install # 試し実行 $ ros2 launch plansys2_simple_example plansys2_simple_example_launch.py Package 'plansys2_simple_example' not found: "package 'plansys2_simple_example' not found, searching: ['/opt/ros/humble']" # パッケージが見つからないエラーが出るので、作業ディレクトリの環境変数を読み込み $ source install/local_setup.bash # 改めて、試し実行 $ ros2 launch plansys2_simple_example plansys2_simple_example_launch.py [INFO] [launch]: All log files can be found below /home/xxxxx/.ros/log/2024-05-01-11-32-34-050794-devros-28084 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [plansys2_node-1]: process started with pid [28092] [INFO] [move_action_node-2]: process started with pid [28094] [INFO] [charge_action_node-3]: process started with pid [28096] [INFO] [ask_charge_action_node-4]: process started with pid [28098] [plansys2_node-1] [INFO] [1714530754.267456796] [domain_expert_lc_mngr]: Creating client for service [domain_expert/get_state] [plansys2_node-1] [INFO] [1714530754.267567987] [domain_expert_lc_mngr]: Creating client for service [domain_expert/change_state] ・・・ [plansys2_node-1] [INFO] [1714531700.068686477] [domain_expert_lc_mngr]: Node domain_expert_lc_mngr has current state active. [plansys2_node-1] [INFO] [1714531700.069034223] [problem_expert_lc_mngr]: Node problem_expert_lc_mngr has current state active. [plansys2_node-1] [INFO] [1714531700.069290746] [planner_lc_mngr]: Node planner_lc_mngr has current state active. [plansys2_node-1] [INFO] [1714531700.069516872] [executor_lc_mngr]: Node executor_lc_mngr has current state active. [Ctrl-C] $
（補足）ビルド時のエラー例：
下記エラーの時は、ROS環境変数の読み込みsource /opt/ros/humble/setup.bashが必要です。
--- stderr: plansys2_bt_example
CMake Error at CMakeLists.txt:5 (find_package): By not providing "Findament_cmake.cmake" in CMAKE_MODULE_PATH this project has asked CMake to find a package configuration file provided by "ament_cmake", but CMake did not find one.
下記エラーの時は、ros-humble-navigation2のインストールが必要です。
--- stderr: plansys2_patrol_navigation_example
CMake Error at CMakeLists.txt:10 (find_package): By not providing "Findnav2_msgs.cmake" in CMAKE_MODULE_PATH this project has asked CMake to find a package configuration file provided by "nav2_msgs", but CMake did not find one.
下記エラーの時は、ros-humble-test-msgsのインストールが必要です。
--- stderr: plansys2_bt_example
CMake Error at /opt/ros/humble/share/plansys2_bt_actions/cmake/ament_cmake_export_dependencies-extras.cmake:21 (find_package): By not providing "Findtest_msgs.cmake" in CMAKE_MODULE_PATH this project has asked CMake to find a package configuration file provided by "test_msgs", but CMake did not find one.
（２）実行
サンプルの立ち上げまで
２つのターミナルから、２つのサンプルをそれぞれ実行。
ターミナル１
# 環境変数 $ cd ~/gitwork/p2tuto $ source /opt/ros/humble/setup.bash $ source install/local_setup.bash # 実行 $ ros2 launch plansys2_simple_example plansys2_simple_example_launch.py [INFO] [launch]: All log files can be found below /home/xxxxx/.ros/log/2024-05-01-11-48-19-655061-devros-34912 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [plansys2_node-1]: process started with pid [34913] [INFO] [move_action_node-2]: process started with pid [34915] [INFO] [charge_action_node-3]: process started with pid [34917] [INFO] [ask_charge_action_node-4]: process started with pid [34919] [plansys2_node-1] [INFO] [1714531699.850273612] [domain_expert_lc_mngr]: Creating client for service [domain_expert/get_state] [plansys2_node-1] [INFO] [1714531699.850370691] [domain_expert_lc_mngr]: Creating client for service [domain_expert/change_state] [plansys2_node-1] [INFO] [1714531699.851227870] [executor_lc_mngr]: Creating client for service [executor/get_state] [plansys2_node-1] [INFO] [1714531699.851248310] [executor_lc_mngr]: Creating client for service [executor/change_state] [plansys2_node-1] [INFO] [1714531699.852079740] [planner_lc_mngr]: Creating client for service [planner/get_state] [plansys2_node-1] [INFO] [1714531699.852098688] [planner_lc_mngr]: Creating client for service [planner/change_state] [plansys2_node-1] [INFO] [1714531699.852944208] [problem_expert_lc_mngr]: Creating client for service [problem_expert/get_state] [plansys2_node-1] [INFO] [1714531699.852964935] [problem_expert_lc_mngr]: Creating client for service [problem_expert/change_state] [plansys2_node-1] [INFO] [1714531699.854163319] [domain_expert]: [domain_expert] Configuring... [plansys2_node-1] [INFO] [1714531700.024586510] [domain_expert]: [domain_expert] Configured [plansys2_node-1] [INFO] [1714531700.025312276] [domain_expert_lc_mngr]: Transition 1 successfully triggered. [plansys2_node-1] [INFO] [1714531700.026112788] [domain_expert_lc_mngr]: Node domain_expert_lc_mngr has current state inactive. [plansys2_node-1] [INFO] [1714531700.026354520] [problem_expert]: [problem_expert] Configuring... [plansys2_node-1] [INFO] [1714531700.026822989] [problem_expert]: [problem_expert] Configured [plansys2_node-1] [INFO] [1714531700.027275348] [problem_expert_lc_mngr]: Transition 1 successfully triggered. [plansys2_node-1] [INFO] [1714531700.027961476] [problem_expert_lc_mngr]: Node problem_expert_lc_mngr has current state inactive. [plansys2_node-1] [INFO] [1714531700.028050663] [planner]: [planner] Configuring... [plansys2_node-1] [INFO] [1714531700.028496441] [planner]: Created solver : POPF of type plansys2/POPFPlanSolver [plansys2_node-1] [INFO] [1714531700.028519249] [planner]: [planner] Configured [plansys2_node-1] [INFO] [1714531700.028675092] [planner_lc_mngr]: Transition 1 successfully triggered. [plansys2_node-1] [INFO] [1714531700.028967789] [planner_lc_mngr]: Node planner_lc_mngr has current state inactive. [plansys2_node-1] [INFO] [1714531700.029132070] [executor]: [executor] Configuring... [plansys2_node-1] [INFO] [1714531700.066183058] [executor]: [executor] Configured [plansys2_node-1] [INFO] [1714531700.066517471] [executor_lc_mngr]: Transition 1 successfully triggered. [plansys2_node-1] [INFO] [1714531700.066907041] [executor_lc_mngr]: Node executor_lc_mngr has current state inactive. [plansys2_node-1] [INFO] [1714531700.067164475] [domain_expert]: [domain_expert] Activating... [plansys2_node-1] [INFO] [1714531700.067185801] [domain_expert]: [domain_expert] Activated [plansys2_node-1] [INFO] [1714531700.067353202] [domain_expert_lc_mngr]: Transition 3 successfully triggered. [plansys2_node-1] [INFO] [1714531700.067494519] [problem_expert]: [problem_expert] Activating... [plansys2_node-1] [INFO] [1714531700.067512942] [problem_expert]: [problem_expert] Activated [plansys2_node-1] [INFO] [1714531700.067691890] [problem_expert_lc_mngr]: Transition 3 successfully triggered. [plansys2_node-1] [INFO] [1714531700.067826424] [planner]: [planner] Activating... [plansys2_node-1] [INFO] [1714531700.067842219] [planner]: [planner] Activated [plansys2_node-1] [INFO] [1714531700.068009696] [planner_lc_mngr]: Transition 3 successfully triggered. [plansys2_node-1] [INFO] [1714531700.068138337] [executor]: [executor] Activating... [plansys2_node-1] [INFO] [1714531700.068154710] [executor]: [executor] Activated [plansys2_node-1] [INFO] [1714531700.068317602] [executor_lc_mngr]: Transition 3 successfully triggered. [plansys2_node-1] [INFO] [1714531700.068686477] [domain_expert_lc_mngr]: Node domain_expert_lc_mngr has current state active. [plansys2_node-1] [INFO] [1714531700.069034223] [problem_expert_lc_mngr]: Node problem_expert_lc_mngr has current state active. [plansys2_node-1] [INFO] [1714531700.069290746] [planner_lc_mngr]: Node planner_lc_mngr has current state active. [plansys2_node-1] [INFO] [1714531700.069516872] [executor_lc_mngr]: Node executor_lc_mngr has current state active. （ここで待機）
ターミナル２
$ cd ~/gitwork/p2tuto $ source /opt/ros/humble/setup.bash $ source install/local_setup.bash $ ros2 run plansys2_terminal plansys2_terminal [INFO] [1714531774.867610478] [terminal]: No problem file specified. ROS2 Planning System console. Type "quit" to finish > （ここで待機）
plansys2_terminalからコマンドを実行
待機状態のターミナル２から、plansys2_terminalのプロンプトに、下記を順番に入力します。
# プラン対象の環境を初期化 > set instance leia robot > set instance entrance room > set instance kitchen room > set instance bedroom room > set instance dinning room > set instance bathroom room > set instance bathroom room > set instance chargingroom room > set predicate (connected entrance dinning) > set predicate (connected dinning entrance) > set predicate (connected dinning kitchen) > set predicate (connected kitchen dinning) > set predicate (connected bedroom dinning) > set predicate (connected dinning bedroom) > set predicate (connected bedroom dinning) > set predicate (connected bathroom bedroom) > set predicate (connected bedroom bathroom) > set predicate (connected chargingroom kitchen) > set predicate (connected kitchen chargingroom) > set predicate (charging_point_at chargingroom) > set predicate (battery_low leia) > set predicate (robot_at leia entrance) # 目標を設定 > set goal (and(robot_at leia bathroom))
set instanceまでを一通り入力した段階で、
instance の括りは以下のイメージになっています。
graph TD
subgraph room
entrance
kitchen
bedroom
dinning
bathroom
bathroom
chargingroom
end
subgraph robot
leia
end
set predicateまでを一通り入力し終えた段階で、
各roomの位置関係は下記のようなイメージになってます。
graph LR
A === B
B === C
C === D
B === E
E === F
A(entrance</br>（初めにロボットがいるところ）)
B(dining)
C(kitchen)
D(chargingroom</br>（充電場所）)
E(bedroom)
F(bathroom</br>（ロボットに向かわせる先）)
後で出てきますが、predicateに添えてある単語は、PDDLファイルsrc/plansys2_simple_example/pddl/simple_example.pddlにて定義されていて、下記のイメージになってます。
graph TD
subgraph model_predicate
robot_at
connected
battery_full
battery_low
charging_point_at
end
subgraph model_actions
move
askcharge
charge
end
改めて、ここからプランニングを実行します。
# プランの状況を確認 > get plan plan: 0: (askcharge leia entrance chargingroom) [5] 5.001: (charge leia chargingroom) [5] 10.002: (move leia chargingroom kitchen) [5] 15.003: (move leia kitchen dinning) [5] 20.004: (move leia dinning bedroom) [5] 25.005: (move leia bedroom bathroom) [5] # プランを実行 > run [INFO] [1714532505.082763012] [executor_client]: Plan Succeeded Successful finished
移動順序を図示すると、下記のようになります。
■矢印中の表記： 1(askcharge/5sec) → 順番(アクション/所要時間)
graph LR
A === B
B === C
C === D
B === E
E === F
A --1(askcharge/5sec)--> D
D --(charge/5sec)--> D
D --2(move/5sec)--> C
C --3(move/5sec)--> B
B --4(move/5sec)--> E
E --5(move/5sec)--> F
A(entrance</br>（初めにロボットがいるところ）)
B(dining)
C(kitchen)
D(chargingroom</br>（充電場所）)
E(bedroom)
F(bathroom</br>（ロボットに向かわせる先）)
■疑問■：最初に
entrance→chargingroomにワープしてるのはなぜだろう？バッテリーがなくてロボットが動けないので、人がロボットを持って運んだのかな？
このときのターミナル１の状態を見ると、下記の内容が追加で出力しています。
・・・ [plansys2_node-1] [INFO] [1714532418.695388970] [executor]: Action askcharge timeout percentage -1.000000 [plansys2_node-1] [INFO] [1714532418.697038670] [executor]: Action charge timeout percentage -1.000000 [plansys2_node-1] [INFO] [1714532418.698644771] [executor]: Action move timeout percentage -1.000000 [plansys2_node-1] [INFO] [1714532418.701316685] [executor]: Action move timeout percentage -1.000000 [plansys2_node-1] [INFO] [1714532418.704650534] [executor]: Action move timeout percentage -1.000000 [plansys2_node-1] [INFO] [1714532418.706749934] [executor]: Action move timeout percentage -1.000000 [plansys2_node-1] [WARN] [1714532418.710294283] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. [plansys2_node-1] [WARN] [1714532418.727634952] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. Requesting for charging ... [100%] Charging ... [100%] Moving ... [100%] Moving ... [100%] Moving ... [100%] Moving ... [100%] [plansys2_node-1] [INFO] [1714532502.966406356] [executor]: Plan Succeeded
（３）もう一歩先の手順
knowledge情報を別のファイルから読み込み
plansys2_terminalのプロンプトに、毎回打ち込むのは大変ですが、あらかじめ記述したファイルを、まとめて読み込むコマンドがあります。
ここでチュートリアルしているplansys2_simple_exampleには、先ほどplansys2_terminalに入力した内容が、一式格納されたファイルがあるので、それを読み込みます。
$ nano src/plansys2_simple_example/launch/commands
ファイルの中身
set instance leia robot set instance entrance room set instance kitchen room set instance bedroom room set instance dinning room set instance bathroom room set instance chargingroom room set predicate (connected entrance dinning) set predicate (connected dinning entrance) set predicate (connected dinning kitchen) set predicate (connected kitchen dinning) set predicate (connected dinning bedroom) set predicate (connected bedroom dinning) set predicate (connected bathroom bedroom) set predicate (connected bedroom bathroom) set predicate (connected chargingroom kitchen) set predicate (connected kitchen chargingroom) set predicate (charging_point_at chargingroom) set predicate (battery_low leia) set predicate (robot_at leia entrance) set goal (and(robot_at leia bathroom))
set goal節は、書いても読み込んでくれないようなので、省略。
plansys_terminalから続きを実行
# Knoulegeの内容を指定のファイルから読み込み > source src/plansys2_simple_example/launch/commands > get plan plan: 0: (askcharge leia entrance chargingroom) [5] 5.001: (charge leia chargingroom) [5] 10.002: (move leia chargingroom kitchen) [5] 15.003: (move leia kitchen dinning) [5] 20.004: (move leia dinning bedroom) [5] 25.005: (move leia bedroom bathroom) [5] > run
GUIツール
現在のplansysの状態を観測できます。
起動のために３つ目のターミナルを起動して、最小限の環境変数を読み込んでおきます。
$ source /opt/ros/humble/setup.bash
rqtを使う
$ rqt
メニューを開いて、PlanSys2の３つのプラグインを有効にします。
source src/plansys2_simple_example/launch/commandsを読み込んだ状態で開くと、このように表示されます。
runすると、Planのダイアログに進捗が出ます。
完了までたどり着くと、下記になります。
■懸念■：plansys2_terminalから、sourceでファイルを読み込ませたときに、低い頻度でrqtが下記のエラーで止まります。その時はrqtを起動しなおしてください。
terminate called after throwing an instance of 'std::length_error' what(): basic_string::_M_create
rqt_graphを使う
launchで起動した、それぞれのノード・トピックの状態を観測します。
$ rqt_graph
一度、更新ボタンを押す必要があります。
runすると、表示が若干変わります。
複数のユーザが同時にROS2を使うときの混信対策
ターミナルを開くたびに、環境変数ROS_DOMAIN_IDを指定します。
$ source /opt/ros/humble/setup.bash $ source install/local_setup.bash $ export ROS_DOMAIN_ID=231
４．まとめ
まずは「Getting Started」チュートリアルを一通り試してみました。 手順の一部が抜けている箇所もありましたが、出てきたエラーから読み解いて、不足を補うことができました。
次回は、plansys2_terminalの機能を深堀りしていきます。
