１．はじめに
前回記事に引き続き、 ROS2のPlanSys2の試行と、それに関連する気づきをまとめました。
ROS2 Planning System — ROS2 Planning System 2 1.0.0 documentation
- １．はじめに
- ２．plansys2_terminalの使い方
- ３．まとめ
２．plansys2_terminalの使い方
チュートリアルチャレンジ（１）で準備したソースコードを、そのまま使います。
２つのターミナルでそれぞれ起動しておきます。
ターミナル１
$ cd ~/gitwork/p2tuto $ source /opt/ros/humble/setup.bash $ source install/local_setup.bash # 実行 $ ros2 launch plansys2_simple_example plansys2_simple_example_launch.py
ターミナル２
$ 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のプロンプトから、helpコマンドを実行すると下記表示されます。
> help The list of available commands is the following: ? help|? [ set | get | remove | run | check | source | quit | help | ? ] check run action <action> run num_actions <num> run plan-file <planfile> get get model types | predicates | functions | actions | predicate <predicate_name> | function <function_name> | action <action_name> get problem instances | predicates | functions | goal get domain get plan help help|? [ set | get | remove | run | check | source | quit | help | ? ] quit remove remove instance <object> remove predicate <predicate> remove function <function> remove goal run run action <action> run num_actions <num> run plan-file <planfile> set set instance <object> <type> set predicate <predicate> set function (= <function> <value>) set goal <goal_formula> source source <file_name> [0|1]
（２）plansys2_terminalの実行例・PDDLに基づいた静的モデル
「Getting Started」チュートリアル を例に、plansys2_terminalを実行した例をまとめました。
get domain
現在のPDDLの内容を表示します。 サンプルでは、こちらのファイルと同じ内容が出てきます。
src/plansys2_simple_example/pddl/simple_example.pddl
> get domain domain: (define (domain simple) (:requirements :adl :durative-actions :fluents :strips :typing ) (:types robot room ) ・・・ :effect (and (at end(not(battery_low ?r))) (at end(battery_full ?r)) ) ) )
launchファイルとの関係
サンプルでは、こちらを起動しています。
src/plansys2_simple_example/launch/plansys2_simple_example_launch.py
このファイルの、下記の部分に、PDDLファイルを読み込んでいます。
・・・ def generate_launch_description(): ・・・ plansys2_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('plansys2_bringup'), 'launch', 'plansys2_bringup_launch_monolithic.py')), launch_arguments={ 'model_file': example_dir + '/pddl/simple_example.pddl', 'namespace': namespace }.items()) ・・・
get model types
モデルに含まれるインスタンスのタイプを確認
> get model types Types: 2 robot room
PDDLファイルの下記に該当します。
(:types robot room)
get model actions
> get model actions Actions: 0 move (durative action) askcharge (durative action) charge (durative action) >
PDDLファイルの下記に該当します。
(:durative-action move :parameters (?r - robot ?r1 ?r2 - room) :duration ( = ?duration 5) :condition (and (at start(connected ?r1 ?r2)) (at start(robot_at ?r ?r1)) (over all(battery_full ?r)) ) :effect (and (at start(not(robot_at ?r ?r1))) (at end(robot_at ?r ?r2)) ) ) (:durative-action askcharge・・・) (:durative-action charge・・・)
さらに個別の詳細（下記例はmove）を取得することもできます。
> get model action move
[ERROR] [1715070648.260690641] [domain_expert_client]: /domain_expert/get_domain_action_details: Action not found
Type: durative-action
Parameters: 3
?0 - robot
?1 - room
?2 - room
AtStart requirements: (and (connected ?1 ?2)(robot_at ?0 ?1))
OverAll requirements: (and (battery_full ?0))
AtEnd requirements:
AtStart effect: (and (not (robot_at ?0 ?1)))
AtEnd effect: (and (robot_at ?0 ?2))
get model predicates
> get model predicates
Predicates: 5
battery_full
battery_low
charging_point_at
connected
robot_at
PDDLファイルの下記に該当します。
(:predicates (robot_at ?r - robot ?ro - room) (connected ?ro1 ?ro2 - room) (battery_full ?r - robot) (battery_low ?r - robot) (charging_point_at ?ro - room) )
さらに個別の詳細（下記例はrobot_at）を取得することもできます。
> get model predicate robot_at
Parameters: 2
robot - ?robot0
room - ?room1
（３）plansys2_terminalの実行例・動的モデル
これまで、PlanSys2 の実行中に変更されないモデルを検査する方法を見てきました。それは計画要素の静的な部分であると言えます。もう 1 つの要素は問題であり、これにはインスタンス、根拠のある (ドメインのような汎用的なものではなく、すでにインスタンスを備えた) 述語、および目標が含まれます。
get problem
まずは、それぞれのproblemが、空であることを確認してみます。
plansys2_terminalで下記を実行します。
> get problem instances Instances: 0 > get problem predicates Predicates: 0 > get problem goal Goal:
plansys2_terminalで下記を実行します。
> source src/plansys2_simple_example/launch/commands
plansys2_terminalで下記を実行すると、先ほど読み込んだcommandsファイルの内容に従い、それぞれのproblem がセットされていることがわかります。
> get problem instances Instances: 7 leia robot entrance room kitchen room bedroom room dinning room bathroom room chargingroom room > get problem predicates Predicates: 13 (connected entrance dinning) (connected dinning entrance) (connected dinning kitchen) (connected kitchen dinning) (connected dinning bedroom) (connected bedroom dinning) (connected bathroom bedroom) (connected bedroom bathroom) (connected chargingroom kitchen) (connected kitchen chargingroom) (charging_point_at chargingroom) (battery_low leia) (robot_at leia entrance) > get problem goal Goal: (and (robot_at leia bathroom))
get plan
goalまで設定できていると、それを達成するための計画を演算できます。
> 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]
これを実行したことにより、/tmpディレクトリに２つのpddlファイルが生成されます。
$ ls /tmp/*.pddl
/tmp/domain.pddl /tmp/problem.pddl
/tmp/domain.pddlファイルは、src/plansys2_simple_example/pddl/simple_example.pddlと同じ内容です。
もう一つの/tmp/problem.pddlファイルは、下記の内容になっています。
$ cat /tmp/problem.pddl ( define ( problem problem_1 ) ( :domain simple ) ( :objects leia - robot entrance kitchen bedroom dinning bathroom chargingroom - room ) ( :init ( connected entrance dinning ) ( connected dinning entrance ) ( connected dinning kitchen ) ( connected kitchen dinning ) ( connected dinning bedroom ) ( connected bedroom dinning ) ( connected bathroom bedroom ) ( connected bedroom bathroom ) ( connected chargingroom kitchen ) ( connected kitchen chargingroom ) ( charging_point_at chargingroom ) ( battery_low leia ) ( robot_at leia entrance ) ) ( :goal ( and ( robot_at leia bathroom ) ) ) )
plansys2_terminalを使わず、get planを直接実行する場合
別のターミナル３を開いて、下記を実行します。
$ source /opt/ros/humble/setup.bash $ source install/local_setup.bash $ ros2 run popf popf /tmp/domain.pddl /tmp/problem.pddl Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] [110%] [120%] [130%] [140%] [150%] [160%] [170%] Post filtering unreachable actions: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] [110%] [120%] [130%] [140%] [150%] [160%] [170%] All the ground actions in this problem are compression-safe b (4.000 | 10.001)b (3.000 | 15.002)b (2.000 | 20.003)b (1.000 | 25.004);;;; Solution Found ; Time 0.00 0.000: (askcharge leia entrance chargingroom) [5.000] 5.001: (charge leia chargingroom) [5.000] 10.002: (move leia chargingroom kitchen) [5.000] 15.003: (move leia kitchen dinning) [5.000] 20.004: (move leia dinning bedroom) [5.000] 25.005: (move leia bedroom bathroom) [5.000]
実行が終わると、/tmpに二つのファイルが生成されます。
$ ls /tmp/*.pddl
/tmp/check_domain.pddl /tmp/domain.pddl
この時点での、rqtの状態はこのようになります。
実行します。
> run
実行途中
実行完了
… > run [INFO] [1715073858.182791378] [executor_client]: Plan Succeeded Successful finished
（４）plansys2_terminalの実行例・goal／predicateの再定義
チュートリアルの範囲を超えて、 goalの再定義からプラン演算、実行まで実行してみます。
現在地から最初の場所entranceに戻る
いったん、goalを削除します。
> remove goal
goalを設定します。現在地がbathroomなので、最初にいたentranceに戻ります。
> set goal (and(robot_at leia entrance))
プランの再演算をしてから、実行します。
> get plan plan: 0: (move leia bathroom bedroom) [5] 5.001: (move leia bedroom dinning) [5] 10.002: (move leia dinning entrance) [5] > run [INFO] [1715074011.465900834] [executor_client]: Plan Succeeded Successful finished
entranceに戻りました。
バッテリー残量を低下した状態にして、kitchenに向かわせる
# バッテリー残量を低下にする > set predicate (battery_low leia) # 行き先をキッチンにする > set goal (and(robot_at leia kitchen)) # プランの演算 > get plan plan: 0: (move leia entrance dinning) [5] 5.001: (move leia dinning kitchen) [5]
ここで、chargingroomへの経路がないことに気づく。
rqtのKnowledgeを見ると、
predicate (battery_full leia)とredicate (battery_low leia)が二重に定義されている。
# battery_full のpredicate を削除 > remove predicate (battery_full leia)
rqtのKnowledgeから、predicate (battery_full leia)が消えた。
# 改めてプランを演算 > get plan plan: 0: (askcharge leia entrance chargingroom) [5] 5.001: (charge leia chargingroom) [5] 10.002: (move leia chargingroom kitchen) [5] # 今度は、chargingroomが現れた！ # 実行 > run
predicate の指定が不足している状態でgetplanする
predicate (battery_*がない状態にします。
> remove predicate (battery_full leia) > remove predicate (battery_low leia)
プランの演算に失敗しました。
> get plan [ERROR] [1715132672.285605721] [planner_client]: /planner/get_plan: Plan not found Plan not found
namespace指定（プラン実行時のPDDL生成の出力先を/tmp以外にする）
■注意：namespaceを使うと、rqtでの表示ができなくなります
デフォルトでは、/tmpディレクトリにPDDLファイルが作られます。
複数のプランを並行で作るなど干渉を避けたい場合は、namespaceを使うことで、/tmpディレクトリよりも下のディレクトリに、PDDLファイルが作成されます。
$ ros2 run popf popf /tmp/${namespace}/domain.pddl /tmp/${namespace}/problem.pddl
plansys2_simple_exampleをnamespace指定する場合
各コマンドの引数にnamespace（ここではmy_namespace）を指定します。
$ ros2 launch plansys2_simple_example plansys2_simple_example_launch.py namespace:=my_namespace [INFO] [launch]: Default logging verbosity is set to INFO ・・・ [plansys2_node-1] [INFO] [1715215844.974130342] [my_namespace.problem_expert]: [problem_expert] Activated [plansys2_node-1] [INFO] [1715215844.974273432] [my_namespace.problem_expert_lc_mngr]: Transition 3 successfully triggered. [plansys2_node-1] [INFO] [1715215844.974476313] [my_namespace.planner]: [planner] Activating... [plansys2_node-1] [INFO] [1715215844.974495632] [my_namespace.planner]: [planner] Activated
$ ros2 run plansys2_terminal plansys2_terminal --ros-args -r __ns:=/my_namespace [INFO] [1715215850.414612093] [my_namespace.terminal]: No problem file specified. ROS2 Planning System console. Type "quit" to finish > source src/plansys2_simple_example/launch/commands > get plan
それぞれのログを見ると、my_namespace.*となっています。
/tmpディレクトリの下に、新たにmy_namespaceができています。
$ ls /tmp/my_namespace/
check.out check_domain.pddl check_problem.pddl domain.pddl plan problem.pddl
３．まとめ
plansys2_terminalの使い方を少し深堀りしてみました。
次回は、PlanSys2のパッケージの構造と、それを使ったロボットの制御の方法を探っていきます。
