waypointが更新されない

[AriYu]

障害物が近くに来て止まった場合に、move_baseがreached_goalという出力を出してゴールに到達したことになり、止まってしまいます。
しかしながら、当然ながらmove_baseで設定したしきい値よりもロボットはゴールから離れており、waypoint_navigatorのしきい値よりも離れているためwaypointの更新が行われません。
move_baseのしきい値　< waypoint_navigatorのしきい値
です。
ただし、90[s]間ロボットが動いていないと判断されればwaypoint_navigatorは次のwaypointを送るため、つくばチャレンジ2016本番では致命的な問題にはなりませんでした。

[MoriKen254]

@AriYu
確認を含め、コメントします。

ゴール判定のロジックを見ると、たしかに停止することがゴール到達条件になっているようですね。

障害物が出現→ロボットが停止→ゴール判定となってしまう

が成り立ってしまいそうです。

move_base
↓
DWAPlannerROS::isGoalReached()
↓
LatchedStopRotateController::isGoalReached

• ここで、速度0になったかを確認した上で、return true; を返すよう。

あくまで予想ですが、下記の類の現象が発生しているように思われます。

• 本来
  • 閾値に達していなくとも、おそらくある程度近づいた段階で徐々に速度を落とし始めるモードに入ってしまう。
  • 停止した段階で閾値に入っているくらいのナビゲーションをしている。
  • このモードに入ると、速度が完全に0となったらゴールと判定する。
  • これだと閾値に入ってすぐにゴール判定されることはないので、結果としてそこそこゴールに近づくこととなる。
• 今回の現象
  • やはり閾値に達していないが、徐々に速度を落とし始めるモードに入ってしまう。
  • まだ減速中なのに急に障害物が登場。
  • ここで速度が0となるのでゴールと判定されてしまう。

あるいは、厳密にはこれと違ったとしても、waypointにある程度接近した状態で突然停止すると、プランナー側がゴールと認識してしまうロジックが入り込んでいるのでしょう。

もしその仕様が「真」であった場合、move_baseあるいはlocal_plannerがその状態をtopicか何かで出力していない限り、それらのコードを編集する以外の手段で解決するのは難しいように思われます…。

どうでしょうか^^;

[forno]

こんばんは。

move_base が reached_goal に達した状態というのは Action が完了した状態では無いかと思い書き込ませていただきます。見当違いでしたら申し訳ございませんがご指摘などいただけますと幸いです。

恐らくご存知のことと思いますが、 Action に送った値に move_base が達した（あるいは到達不可能と判断した）場合にはActionが完了しResultが得られるかと思います。
私は Result が得られたか確認すれば、 move_base の状態がわかるのではないかと考えました。

actionlib Documentation の Communication Protocol にあるようにActionには豊富なステータスがあり、これを用いればResultが得られたかどうか、あるいはそれ以上の有容な情報を得ることができるのではないかと思います。
move_base がどの時にどのStatusをセットするかまでは調べていませんが、 actionlib::SimpleActionClientの getStatus メソッドにて Action のステータスが容易に得られるため、まずはこれを用いて解決策を検討することも十二分にありかと思います。

この件が Actionlib と全く関係のない話題であれば、場違いな発言をお許しください。

[AriYu]

@MoriKen254
いろいろ調べて下さりありがとうございます。
isGoalReached()関数を見てみましたが、確かにロボットが止まっているかどうかが判定内容に含まれているようですが、その一つ上のif文でそもそもゴールとロボットとの位置がxy_tolerance以下になっていないと止まっているかどうかの判定に達しないと読めますが、どうでしょうか…

@forno
actionlibの存在を失念していました。
確かに、goalしたかどうかならactionlibを使うことで判定できそうです。
waypoint_navigator側でactionlibの結果も参照するようにしてみます。

[AriYu]

http://docs.ros.org/api/actionlib_msgs/html/msg/GoalStatus.html
をみるとplanning abortも判定出来そうですねぇ…

[forno]

Communication Protocol にある説明からすると。

• PENDING - The goal has yet to be processed by the action server
• ACTIVE - The goal is currently being processed by the action server
• REJECTED - The goal was rejected by the action server without being processed and without a request from the action client to cancel
• SUCCEEDED - The goal was achieved successfully by the action server
• ABORTED - The goal was aborted by the action server
• PREEMPTING - Processing of the goal was canceled by either another goal, or a cancel request sent to the action server
• PREEMPTED - The goal was preempted by either another goal, or a preempt message being sent to the action server
• RECALLING - The goal has not been processed and a cancel request has been received from the action client, but the action server has not confirmed the goal is canceled
• RECALLED - The goal was canceled by either another goal, or a cancel request before the action server began processing the goal
• LOST - The goal was sent by the ActionClient, but disappeared due to some communication error

LOSTかABORTEDあたりが使えそうな香りがします。

[forno]

けれど、 move_base のコードを流し読みしてもどんなステータスが帰ってくるのかよくわからないので、箇所がわかったら教えていただけますと有り難いです。

[MoriKen254]

@AriYu
おっしゃる通りですね．私の見逃しです．失礼しました^^;

@forno
ありがとうございます．

そうですね，actionlibで状態判定するのが良さそうですね．
move_baseのコードを読んだところ，いくつか状態を制御している部分がありました．

SUCCEEDED

• move_base.cpp#L932
  • "Goal reached."

ABORTED

• move_base.cpp#L689
  • "Aborting on goal because it was sent with an invalid quaternion"
• move_base.cpp#L734
  • "Aborting on goal because it was sent with an invalid quaternion"
• move_base.cpp#L828
  • "Aborting on the goal because the node has been killed"
• move_base.cpp#L897
  • "Failed to pass global plan to the controller."
• move_base.cpp#L1015
  • "Failed to find a valid control. Even after executing recovery behaviors."
• move_base.cpp#L1019
  • "Failed to find a valid plan. Even after executing recovery behaviors."
• move_base.cpp#L1023
  • "Robot is oscillating. Even after executing recovery behaviors."
• move_base.cpp#L1036
  • "Reached a case that should not be hit in move_base. This is a bug, please report it."

PREEMPTD

• move_base.cpp#L767
  • "Move base preempting the current goal"

小生では，上記以外は見つけられませんでした^^;

気になるのがmove_baseが"Goal reached."を吐くのがsetSucceededメソッドのみとなっている点です。

つまり，コードのロジック上，"Goal reached."かつSUCEEDED以外となるようなパスは存在しないかもしれません．move_baseは本当に自分がゴールしたと信じてしまっている可能性があります．

実挙動時の値も見てみないことには確証は持てませんが，原因追求の参考になれば．

[MoriKen254]

すみません，これもご存知と思いますが，move_baseのロガーレベルをDEBUGにすれば，もう少し情報が得られるかもしれません．原因究明に繋ればと思い，念のため確認です．

rosservice call /move_base/set_logger_level ros.move_base DEBUG

[forno]

仮に真にゴールだと move_base が判断したとしても、それは元の Action が終了した状態に過ぎないと仮定しております。
つまり、最終的にはSUCCEEDED、LOSTかABORTEDのいずれにせよ、 Action が終了している以上 move_base は動かず、Waypointを更新する手続きが必要である。と言えると思います。

[forno]

ソースコードの調査、帰ってくるステータスの確認を行っていただき大変ありがとうございます。 関数でステータスを割り振っているのを見逃していた事がよくわかりました。

[forno]

ところで、これは少々主題とはズレますが、ROSの Action の仕様を考えると、移動が完了したかどうかは move_base に判断してもらって、それに合わせて情報を出してやる設計が良いのでは無いかと思うのです。
なので現状のwaypointサーバ（Actionのクライアント側）がロボットの位置を把握してGoalを move_base 動作途中で上書きする動作自体に疑問を持っています。

[AriYu]

@forno
ロボットの位置をActionlibのFeedback callbackで取得するのはありだと思っていて、実際つくばチャレンジ2015のコードではそうなっています。まぁそこはどっちでもいいと思います。
ただ、ActionlibでGoalに到達したかを判定してwaypointを送るようにすると、ロボットがwaypointに到達するたびに一時停止または減速してしまうのではないかと思います。

[forno]

@AriYu
ご意見ありがとうございます。
あれから調べた内容で、Goalが処理されきる前に次のゴールを送っても問題が無いことがわかってきました。
既存の構成の90秒止まっていることを検出する部分を、ActionのStatusをチェックしてActionが終了していないか確認し、終わっていたら次(あるいは現在)のwaypointを送る処理にするのがよいと思います。

私の方ではStatusの移行についてはまだ若干の調べ不足があります。
move_base が処理している最中に新しい Goal を送信すると恐らく Preempted がセットされた後すぐに通常の流れが始まってすぐに Active に戻るのでは無いかとは思うのですが、推測の域を出ません。
actionlib/DetailedDescription でステータス移行やサーバ、クライアントの動作についてわかりやすく書いてあるようなので、情報共有まで。

[AriYu]

@forno
そうですね。そうしたほうがいいと思うのでそうしましょう。

[forno]

前々から気になっていた、Action処理中に追加のゴールを送った時の動作についての文章を見つけた気がします。報告まで。

actionlib/DetailedDescription Multiple Goal Policy に書いてある内容を読むと、新しくゴールを送った場合、以前送信されたゴールは参照できなくなるが、キャンセルはされないと書いてあります。
また、 同 Simple Action Server の Goal Reception の説明より、現在動作中のGoal (Current Goal)と次のGoal (Pending Goal)があり、前述のクライアントからのGoal追加送信は、Pending Goalの更新動作となると読み取れます。

これはSimple Action Serverの記述ですがmove_baseの実装はきちんとsimple_action_serverとなっています。参照

現在のwaypoint_navigatorは動作をキャンセルしていないため、 move_base は現在の目標位置(Current Goal)に到達するまでCurrent Goalを目指し、新しく送信したゴールには向かわない、という動作になると考えます。

少々Issue違いですが、ご容赦ください。

さらに言えば、目標を発見した時にcancelGoal()しますが、これは新しく送信した方のGoalの取り下げであり、Current Goalを取り下げることはできず、そのため停止しないのではないかと考えます。