FollowPath / FollowPathAction

役割

FollowPath は simple_pure_pursuit の follow_path action server を呼ぶ BehaviorTree action node です。XML では path_index input port に経路番号を渡します。

Interface

種別	名前	型 / 内容
input port	`path_index`	実行する経路 index
action client	`follow_path`	`rogi_msgs/action/FollowPath`

状態遷移

内部状態を、

\[q\in\{\mathrm{START},\mathrm{WAIT\_SERVER},\mathrm{WAIT\_GOAL},\mathrm{WAIT\_RESULT},\mathrm{DONE}\}\]

とみなせます。

onStart() では input port を読みます。

\[i_p = B[\mathrm{path\_index}]\]

読み取りに失敗した場合は FAILURE です。成功した場合は action goal の送信準備だけ行い、RUNNING を返します。

Goal 送信

server が未準備なら、

\[\operatorname{ready}(\mathrm{follow\_path})=\mathrm{false}\]

なので RUNNING を返して待ちます。server が準備できたら goal を送ります。

\[g=\{ \mathrm{path\_index}=i_p \}\]

goal handle が null の場合、server に reject されたため FAILURE です。

Result 判定

result code を \(c\)、result の success flag を \(b\) とします。成功条件は次だけです。

\[c=\mathrm{SUCCEEDED}\land b=\mathrm{true}\]

それ以外は FAILURE です。

\[c\in\{\mathrm{ABORTED},\mathrm{CANCELED}\}\lor b=\mathrm{false} \Rightarrow \mathrm{FAILURE}\]

Halt

上位 node から halt された場合、goal handle が存在すれば cancel を送ります。

\[h\ne\emptyset \Rightarrow \operatorname{cancel}(h)\]

これにより BehaviorTree 側の中断と pure pursuit 側の走行停止要求を対応させます。

設定ファイル

この node は example/sample/config/tree/main.xml 内の Action ID="FollowPath" または Action ID="FollowPathAction" で使います。

<Action ID="FollowPath" path_index="0"/>

path_index は simple_pure_pursuit の input.csv.follow_path_files の index に対応します。

\[\mathrm{path\_index}=i \Rightarrow P_i=\mathrm{follow\_path\_files}[i]\]

sample launch は path/trajectory/{0..11}.csv を 12 本登録します。index の範囲は 0〜11 ですが、sample が収録する CSV は 0・1 のみです。存在しない index や空 CSV を指定すると、pure pursuit 側で action goal が reject され、この BT node は FAILURE になります。

TreeNodesModel には Groot 表示用に input port も定義します。

<Action ID="FollowPath">
    <input_port name="path_index"/>
</Action>

現行 sample の tree は 0〜11 を順に要求しますが、収録済み CSV は 0・1 だけです。そのままでは経路2の要求時に失敗します。使用する経路を生成するか、tree の FollowPath を収録済み index に合わせてください。FollowPath.action の goal は int32 path_index、result は bool success、feedback は空です。実行中の追加 goal は reject されます。