１．はじめに
以前の記事（ROS2・PlanSys2 チュートリアルチャレンジ（２） - hotch-potch, Note to self）の応用として、ロボットに変化する条件を与えて、プランの結果が変わることを確認してみます。
Terminal Usage — ROS2 Planning System 2 1.0.0 documentation
２．概要
（１）ロボットの動きの復習
チュートリアルplansys2_simple_exampleでは、
移動ロボットのバッテリーの残量は以下２つの状態を持ちます。
- battery_low
- battery_full
チュートリアルでは、以下の順番でプランが進みます。
- 初期状態のKnowlegeでは、
predicate (battery_low leia)となっています - プラン実行と同時に、
askchargeアクションが実行されます - battery_lowなので、充電エリア
chargingroomにワープ（？）し、chargeアクションが実行されます - 充電が終わると、
predicate (battery_low leia)を消して、predicate (battery_full leia)に書き換えます - 地点間の移動
moveアクションをはじめます
（２）改修後のロボットの動きの概要
バッテリーの残量を数値で管理し、移動ごとにバッテリーの消耗させて、一定以下に消耗したら再度充電するようにします。
下記にバッテリー残量の値を保持します。
battery_full
初期状態のKnowlegeでは、
predicate (battery_level leia)とします。この時点では、battery_level = 0です。- プラン実行と同時に、
askchargeアクションが実行されます battery_level = 0なので、充電エリアchargingroomにワープ（？）し、chargeアクションが実行されます- 充電が終わると、
battery_level= battery_level + 5に書き換えます - 地点間の移動
moveアクションをはじめます。移動のたびに電力を消費し、battery_level= battery_level - 1にします。 - 移動中に
battery_level < 2になったら、askchargeアクションが実行され充電します - 引き続き、地点間を移動します
これを、をPDDLで管理するよう改造してみました。
３．PDDLの読み解きと改修
（１）オリジナルのPDDLの読み解き
src/plansys2_simple_example/pddl/simple_example.pddl
こちらは、クローンしてきたソースコードの中で、simple_exampleを実行するときに使うPDDLファイルです。
PDDLの読み解きは、こちらの資料を参考にしました。
（以下、長くなりますので、ご勘弁を…）
(define (domain simple) (:requirements :strips :typing :adl :fluents :durative-actions) ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (:types robot room ) ;; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; (:predicates (robot_at ?r - robot ?ro - room) (connected ?ro1 ?ro2 - room) (battery_full ?r - robot) (battery_low ?r - robot) (charging_point_at ?ro - room) ) ;; end Predicates ;;;;;;;;;;;;;;;;;;;; ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; (:functions ) ;; end Functions ;;;;;;;;;;;;;;;;;;;; ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;durative-action ;持続アクション ;（完了までに時間がかかるアクション） ;移動 (:durative-action move ;このアクションに与えるパラメーターの定義 :parameters (?r - robot ?r1 ?r2 - room) ;アクションが完了するまでの所要時間、計画時に使う :duration ( = ?duration 5) ;このアクションをアクティブ状態に維持する条件 :condition (and ;最初からtrueの必要な条件 ;出発元と到着先が繋がっていること (at start(connected ?r1 ?r2)) ;ロボットが出発元にいること (at start(robot_at ?r ?r1)) ;最初と最後を含む、アクション全体を通じてtrueが必要な条件 (over all(battery_full ?r)) ) ;このアクションがアクティブになったことにより変化させる対象 :effect (and ;アクティブ状態になった直後に変化させる対象 ;ロボットが以前にいた場所を削除 (at start(not(robot_at ?r ?r1))) ;アクティブ状態が終わる直前に変化させる対象 ;ロボットが到着した先をセット (at end(robot_at ?r ?r2)) ) ) ;充電残量の問い合わせ (:durative-action askcharge ;このアクションに与えるパラメーターの定義 :parameters (?r - robot ?r1 ?r2 - room) :duration ( = ?duration 5) ;このアクションをアクティブ状態に維持する条件 :condition (and ;最初からtrueの必要な条件 ;ロボットが出発元にいること (at start(robot_at ?r ?r1)) ;充電場所が設定されていること (at start(charging_point_at ?r2)) ) ;このアクションがアクティブになったことにより変化させる対象 :effect (and ;アクティブ状態になった直後に変化させる対象 ;ロボットが以前にいた場所を削除 (at start(not(robot_at ?r ?r1))) ;アクティブ状態が終わる直前に変化させる対象 ;ロボットが到着した先をセット (at end(robot_at ?r ?r2)) ) ) (:durative-action charge ;このアクションに与えるパラメーターの定義 :parameters (?r - robot ?ro - room) ;アクションが完了するまでの所要時間、計画時に使う :duration ( = ?duration 5) ;このアクションをアクティブ状態に維持する条件 :condition (and ;最初からtrueの必要な条件 ;ロボットが出発元にいること (at start(robot_at ?r ?ro)) ;充電場所が設定されていること (at start(charging_point_at ?ro)) ) ;このアクションがアクティブになったことにより変化させる対象 :effect (and ;アクティブ状態が終わる直前に変化させる対象 (at end(not(battery_low ?r))) (at end(battery_full ?r)) ) ) ;; end Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; ) ;; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
（２）PDDLの改修
改修に入る前に、オリジナルをバックアップしておきましょう。
$ cd ~/gitwork/p2tuto/ $ cp -p src/plansys2_simple_example/pddl/simple_example.pddl src/plansys2_simple_example/pddl/simple_example.org.pddl
前章の「改修後のロボットの動きの概要」に合わせて、PDDLを書き換えます。
(define (domain simple) (:requirements :strips :typing :adl :fluents :durative-actions) ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (:types robot room ) ;; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; (:predicates (robot_at ?r - robot ?ro - room) (connected ?ro1 ?ro2 - room) (battery_full ?r - robot) (battery_low ?r - robot) (charging_point_at ?ro - room) ) ;; end Predicates ;;;;;;;;;;;;;;;;;;;; ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; (:functions ;（★追加）バッテリー残量 (battery_level ?r - robot) ) ;; end Functions ;;;;;;;;;;;;;;;;;;;; ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; (: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)) ;（★追加）バッテリー残量確認、2以上でmoveできる (at start(>= (battery_level ?r) 2)) ;（★コメントアウト）最初と最後を含む、アクション全体を通じてtrueの必要な条件 ;(over all(battery_full ?r)) ) :effect (and (at start(not(robot_at ?r ?r1))) (at end(robot_at ?r ?r2)) ;（★追加）バッテリーを-1消費させる (at end(decrease (battery_level ?r) 1)) ) ) (:durative-action askcharge :parameters (?r - robot ?r1 ?r2 - room) :duration ( = ?duration 5) :condition (and (at start(robot_at ?r ?r1)) (at start(charging_point_at ?r2)) ) :effect (and (at start(not(robot_at ?r ?r1))) (at end(robot_at ?r ?r2)) ) ) (:durative-action charge :parameters (?r - robot ?ro - room) :duration ( = ?duration 5) :condition (and (at start(robot_at ?r ?ro)) (at start(charging_point_at ?ro)) ) :effect (and ;（★コメントアウト）アクティブ状態が終わる直前に変化させる対象 ;(at end(not(battery_low ?r))) ;(at end(battery_full ?r)) ;（★追加）バッテリーを+5充電する (at end(increase (battery_level ?r) 5)) ) ) ;; end Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; ) ;; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
計算に使う場合はfunctionsに書くこと
先ほどのbattery_levelを、試しに、:functionsではなく:predicatesに書いてみると・・・
…
;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;;
(:predicates
(robot_at ?r - robot ?ro - room)
(connected ?ro1 ?ro2 - room)
(battery_full ?r - robot)
(battery_low ?r - robot)
(charging_point_at ?ro - room)
;（★追加）バッテリー残量
(battery_level ?r - robot)
)
;; end Predicates ;;;;;;;;;;;;;;;;;;;;
;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;;
(:functions
;（★追加）バッテリー残量
;(battery_level ?r - robot)
)
;; end Functions ;;;;;;;;;;;;;;;;;;;;
…
VSCodeがエラーを出してきました。
VSCoceのこちらの拡張機能がおすすめです。
（３） Knowledge(commandsファイル)の改修
続いて、Knowledgeが書かれたcommandsファイルを改修します。
改修に入る前に、オリジナルをバックアップしておきましょう。
$ cd ~/gitwork/p2tuto/ $ cp -p src/plansys2_simple_example/launch/commands src/plansys2_simple_example/launch/commands.org
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 function (battery_level leia) ; ←ここを追加 set goal (and(robot_at leia showerroom))
■注意：上記ソースの「 ; ←ここを追加」は、実際にはファイルに書かないでください
４．プランの実行と確認
以前のチュートリアルplansys2_simple_exampleの手順に倣って、プランの作成と実行を試行します。
ターミナル１
simple_example_launchを、以前作成したシェルスクリプト経由で起動します。
$ cd ~/gitwork/p2tuto
$ ./tsimple_example_launch.sh
ターミナル２
plansys2_terminalを、以前作成したシェルスクリプト経由で起動します。
$ cd ~/gitwork/p2tuto $ ./tpterm.sh [INFO] [1715338910.245917057] [terminal]: No problem file specified. ROS2 Planning System console. Type "quit" to finish >
ターミナル３
rqtを、以前作成したシェルスクリプト経由で起動します。
$ cd ~/gitwork/p2tuto
$ ./trqt.sh
プラン１回目
plansys2_terminalから、以下を入力します。
> source src/plansys2_simple_example/launch/commands done
この時、rqtは下記の内容になります。
今回からKnowledgeに、function (battery_level leia) = 0が表示されます。
> 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]
これを図示するとこうなります。（チュートリアルと同じです）
graph LR
A === B
B === C
C === D
B === E
E === F
A --1(askcharge/bat=0)--> D
D --(charge/bat=5)--> D
D --2(move/bat=4)--> C
C --3(move/bat=3)--> B
B --4(move/bat=2)--> E
E --5(move/bat=1)--> F
A(entrance</br>（初めにロボットがいるところ）)
B(dining)
C(kitchen)
D(chargingroom</br>（充電場所）)
E(bedroom)
F(bathroom</br>（ロボットに向かわせる先）)
プランを実行します。
> run [INFO] [1715339372.462591623] [executor_client]: Plan Succeeded Successful finished
２つ目のプラン、(charge leia chargingroom)が終わった時点で、
バッテリー残量function (battery_level leia) = 5に充電されました。
最後のプランを終了した時点で、
バッテリー残量function (battery_level leia) = 1となります。
bathroomに到着後
プラン２回目
現在ロボットはbathroomにいるので、
今度はentranceに向かわせます。
> set goal (and(robot_at leia entrance)) > get plan plan: 0: (askcharge leia bathroom 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 entrance) [5]
２回目はこのようなプランになりました。 これを図示するとこうなります。
graph LR
A === B
B === C
C === D
B === E
E === F
F --1(askcharge/bat=1)--> D
D --(charge/bat=6)--> D
D --2(move/bat=5)--> C
C --3(move/bat=4)--> B
B --4(move/bat=3)--> A
A(entrance</br>（ロボットに向かわせる先）)
B(dining)
C(kitchen)
D(chargingroom</br>（充電場所）)
E(bedroom)
F(bathroom</br>（初めにロボットがいるところ）)
ここで使っているPDDLでは、 moveアクションの開始条件の指定が下記のようになっています。
(:durative-action movemove … :condition (and … ;（★追加）バッテリー残量確認、2以上でmoveできる (at start(>= (battery_level ?r) 2)) …
そのため、現時点のバッテリー残量function (battery_level leia) = 1なので、
２回目のプランでも最初に充電に向かうプランが組まれています。
> run [INFO] [1715339600.416725752] [executor_client]: Plan Succeeded Successful finished >
entranceに到着後
最後のmoveを終了した時点で、
バッテリー残量function (battery_level leia) = 3となります。
プラン３回目
現在ロボットはentranceにいるので、
今度は再びbathroomに向かわせます。
> set goal (and(robot_at leia bathroom)) > get plan plan: 0: (move leia entrance dinning) [5] 5.001: (move leia dinning bedroom) [5] 10.002: (askcharge leia bedroom chargingroom) [5] 15.003: (charge leia chargingroom) [5] 20.004: (move leia chargingroom kitchen) [5] 25.005: (move leia kitchen dinning) [5] 30.006: (move leia dinning bedroom) [5] 35.007: (move leia bedroom bathroom) [5] > run
３回目はやや複雑なプランになりました。 これを図示するとこうなります。
graph LR
A === B
B === C
C === D
B === E
E === F
A --1(move/bat=2)--> B
B --2(move/bat=1)--> C
C --3(askcharge/bat=1)--> D
D --(charge/bat=6)--> D
D --4(move/bat=5)--> C
C --5(move/bat=4)--> B
B --6(move/bat=3)--> E
E --7(move/bat=2)--> F
A(entrance</br>（初めにロボットがいるところ）)
B(dining)
C(kitchen)
D(chargingroom</br>（充電場所）)
E(bedroom)
F(bathroom</br>（ロボットに向かわせる先）)
entranceからdinningにたどり着いた時点で、function (battery_level leia) = 3 → 2になります- moveし始める前の、バッテリー残量の条件
(at start(>= (battery_level ?r) 2))を下回るので、ロボットは充電に行きます - 充電を終えると、バッテリー残量
function (battery_level leia) = 3となります - 引き続き、
bathroomに向かいます - 最後のmoveを終了した時点で、バッテリー残量
function (battery_level leia) = 2となります。
満充電になった直後
bathroomに到着後
プラン４回目
現在ロボットはbathroomにいるので、
最後にdinningに向かわせます。
> set goal (and(robot_at leia dinning)) > get plan plan: 0: (move leia bathroom bedroom) [5] 5.001: (askcharge leia bedroom chargingroom) [5] 10.002: (charge leia chargingroom) [5] 15.003: (move leia chargingroom kitchen) [5] 20.004: (move leia kitchen dinning) [5] > run
これを図示するとこうなります。
graph LR
A === B
B === C
C === D
B === E
E === F
F --1(charge/bat=1)--> E
E --2(askcharge/bat=1)--> D
D --(charge/bat=6)--> D
D --3(move/bat=5)--> C
C --4(move/bat=4)--> B
A(entrance)
B(dining</br>（ロボットに向かわせる先）)
C(kitchen)
D(chargingroom</br>（充電場所）)
E(bedroom)
F(bathroom</br>（初めにロボットがいるところ）)
最後はこのような状態で終わりました。
dinningに到着後
５．まとめ
ロボットに変化する条件を与えて、プランの結果が変わることを確認しました。
