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を取り下げることはできず、そのため停止しないのではないかと考えます。

[AriYu]

@forno
情報をありがとうございます。恥ずかしながら初めて読みました。

For simplicity, the Simple Action Client tracks only one goal at a time. When a user sends a goal with the simple client, it disables all callbacks associated with the previous goal and also stops tracking its state. Note that it does not cancel the previous goal!

確かにwikiによればそのような動作になりそうですね…ということは毎回move_baseで設定したgoal_tolerance以内にロボットが到達していることになりますね。ただ、ゴールに到達した場合、標準出力にはGoal reachedが出力されますが、現状では上記のバグ？以外には出力がありません。

[forno]

先ほどの文章を再度引用します。

actionlib/DetailedDescription Multiple Goal Policy に書いてある内容を読むと、新しくゴールを送った場合、以前送信されたゴールは参照できなくなるが、キャンセルはされないと書いてあります。

つまり、新しいPending Goalを設定したSimple Action Client は Current Goal を参照することができなくなります。
今回のバグでは次のGoalを送信する前に move_base がGoalに到達したためにResultが得られ（※Pending Goalを設定したらCorrent Goalは参照できないためResultも無い）出力が出るものと思われます。

[forno]

追記：
Simple Action Clientが標準出力に吐いていると仮定して書きました。が、その出力が move_base によってはかれている場合は、Current Goalが終了し、Pending Goalがきていない場合に出力されると予想されます。
この部分はコードを追ってないので推測になります

[AriYu]

@forno
現在のwaypoint_navigatorではactionlib clientを利用して得られるResultは見ていません。
そして、Goal Reachedが出力されるのは、move_baseが起動しているTerminal（つまりactionlib serverが起動している方）です。実際、Goal Reachedを出力するコードは、
DWAPlannerROS::isGoalReached()
にあります。

[forno]

@AriYu
コードの位置も示していただきありがとうございます。
整理すると、

1. ClientからWaypointを送信し続けている場合（想定する正常な状態での動作が続いている場合）はGoal Reachedが出力されることはない。
2. 今回のIssueの場合が起きた時、及び最後のGoalにたどり着いた場合にのみGoal Reachedが出力される。

ということでしょうか

[AriYu]

@forno
そういうことになります。
ちなみに新しいwaypointがactionlib clientから送信されると
Got new plan
が出力されます。ただし、本当に新しいゴールをreceiveしたときに表示されるわけでは無く、そのままの意味で、DWAplannerで新しいパスが見つかった時に表示されるようです。

[AriYu]

@forno
最後のwaypointに到達した場合にはGoal Reachedは出力されません。また、最後のwaypointに到達した場合にはcancelGoal()を呼び出しています。

[forno]

そうでしたね。　ありがとうございます。

[MoriKen254]

素晴しい情報をありがとうござます。ご提供いただいたような詳しい仕様について完全に無知でしたので、大変勉強になります。

一旦頭をスッキリさせて思考実験をしてみます。以下のような現象が起こる可能性は考えられないでしょうか？

1．今、ロボットがwaypoint (以下wp) 1に向かって走行中とします。

2．その途中で、次のゴールであるwp2上に突如として障害物が出現したとします。

3．障害物がそこに留まったまま、ロボットとwp1間の距離がwaypoint navigatorが判断する閾値以下になり、waypoint nabigator‘がゴールをwp2に上書きしようと試みるはずです。

4．ここでグローバルプランナーが発動しますが、ゴールの真上に障害物なんてあったらプランなんて作れねーよ、とうだうだ頑張り始めたり、あるいはエラーを吐いたりすることはないでしょうか？もしこの状態になったとしたら、move_baseとしてはプランを作れない位置を次のゴールに採用するわけにはいかず、時間が過ぎていくことになります。

5．すると次のゴールはwp1のままとなる。時間が経てばロボットの位置とwp1の間の距離が、move_baseあるいはlocal_plannerで設定されたgoal_tolerance以下となり、Goal Reachedが出力され停止状態となる。

こうなったらあとはwaypoint navigatorが次のゴール（おそらく、wp3？）を設定してくれるのを待つほかありません。

もし矛盾があれば、ご指摘をお願いします。

[forno]

大体の流れがわかってきたので確認も兼ねて流れを追いたいと思います。

まず、 Server State Machine より、サーバは目標を入手するとステートマシンを作成します。
これはSimple Action Serverの処理で、新しく入ってきたGoalがPendingに追加された場合、この手順を持って新しいGoalに対応するステートマシンが作成されることを示していると理解されます。（ClientのgetStateで手に入るのも新しく作られたこのステートマシンの状態になるはずです）

個々のステートマシンはsimple_action_serverのコンストラクタで指定されたコールバック関数 executeCb を実行します。
この中の処理に executeCycle が存在し、前述の通常のゴールへ向かう処理が記述されています。通常、この関数がtrueを返せばそのまま executeCb は return して終了します。
しかし、今回追うのは executeCb それ自体です。

今回該当する処理ですが、move_base.cpp 663, 664行目 がそれであると思います。
この処理は明示的に次のゴールが来ていないかチェックし、あればacceptNewGoalを用いて次のゴールの処理を開始します。

つまり、 simple_action_server の仕様的にClientから送ったGoalはPendingになるけれど、move_baseがそれを感知して自動的に次のGoalに進むように目標位置を更新しているようです。

この動作を行うと、ステートマシンのGoalが置き換わり、新しいGoalの動作が始まります。
このため、新しいGoalを送信すると自動的にCurrent Goalが最新のものになるようです。

[forno]

@MoriKen254
思考実験ありがとうございます。
思考実験の成否を確認するためのテストとしては、Waypointを壁に埋め込んで走らせる手法が考えられますね。

特に矛盾なく考えられますが、最後のGoalが置き換わらないという点が気になります。
恐らく、Goalは前述（前のコメント）の処理によって置き換わり、グローバルプランナーが計算できないことを通達することで、有効なGoalが全てなくなりその場で止まるのだと思います。
そして、これこそが初めの問題である

しかしながら、当然ながらmove_baseで設定したしきい値よりもロボットはゴールから離れており、waypoint_navigatorのしきい値よりも離れているためwaypointの更新が行われません。
move_baseのしきい値　< waypoint_navigatorのしきい値
です。

を引き起こしているのではないかと思います。

追記：タイプミスがあったので少し修正しました。

[forno]

こんばんは。すこし落ち着きました。
Goal readedと通達される理由の説明をするには私の説は成り立たない可能性が高いことに気づきました。
一旦頭を整理して、後ほど流れをもう一度追ってきたいと思います。

[forno]

なお、私の案が成り立つのは、例えばWaypointを壁に埋め込んで停止した際にmove_baseがGoal reachedと吐く場合です。　そのあたりの検証もできれば待っています。

[AriYu]

@MoriKen254 @forno
まず、障害物上にゴールが設定された場合に関してですが、navfnでは前述の問題が発生していました。
しかし、 @RyodoTanaka くんより carrot_plannerを教えていただき、これにglobal plannerを変更したところ、障害物上にもゴールを設定することができるようになり、エラーも出力されなくなりました。（今回、waypointを1mおきにおいているため直線で結んでも問題にならないかつ、waypointをおいている場所には大きな障害物が無いことが分かっているためcarrot plannerのようなプランニングで問題が生じません。）

ここで、障害物上にゴールが設定されてしまった場合でもローカルプランナーができるだけ近づくようなプランニングを行い、実際障害物はそれほど大きくないため、ある程度近づくとwaypointが更新されます。

ここまでを考えると、move_baseの処理で新しいゴールをreceiveした時点でpendingをしないで新しいゴールを目指すようになると考えるのが妥当かもしれません。コードを追っていないので推測です。

[AriYu]

carrot plannerのwikiにあるfeasible goalというのがキモかもしれません。

障害物上にゴールを設定した場合には、
ローカルプランナーに送られるゴールはfeasible goalになるが、waypoint_navigatorがゴールだと思っているのはoriginal goalで、ロボットとoriginal goalまでの距離がwaypoint_navigatorのしきい値（1.5[m]）以上だが、ロボットとfeasible goalまでの距離はmove_baseのしきい値（goal tolerance）以下になっていた。
ということかもしれません。

[MoriKen254]

@forno @AriYu
コメント、ありがとうございます。

一旦頭を整理して、後ほど流れをもう一度追ってきたいと思います。

了解いたしました。

ローカルプランナーに送られるゴールはfeasible goalになるが、waypoint_navigatorがゴールだと思っているのはoriginal goalで、ロボットとoriginal goalまでの距離がwaypoint_navigatorのしきい値（1.5[m]）以上だが、ロボットとfeasible goalまでの距離はmove_baseのしきい値（goal tolerance）以下になっていた。
ということかもしれません。

carrot plannerがその仕様なら、納得です。解決方法としては、carrot plannerからfeasible goalの情報を取得できれば、waypoint navigator側でうまく障害物を検知してwaypointのインデックスを進めるとか？

[forno]

waypoint は waypoint が持っているポイントと比べていたので、move_base側の目標位置とのズレは考えられますね。

デフォルトのグローバルプランナーが http://wiki.ros.org/global_planner だったとすると、default_tolerance パラメータを設定しない限りは指定した地点へズレのないGoalが設定されるものだと思われますが、いかがでしょうか。

[AriYu]

@forno
グローバルプランナがデフォルトのnavfnの場合、default_toleranceが設定されてなければ、指定した地点へズレのないgoalが設定されます。しかし、navfnの場合、指定したゴールが障害物上になってしまうと、move_baseがハングアップしてしまうという問題があります。

[AriYu]

@MoriKen254
とりあえず、feasible goalに正しくゴールできていればactionlib clientからゴールしたかどうかが分かるはずですので、とりあえずはそれを試してみるのがいいかもしれません。

[forno]

@AriYu
確認ありがとうございます。
carrot plannerでのテスト報告お待ちしてます。
自分は元々の動作がどのような組み合わせで生じているのかをこれからもしばらくは追ってみます。

[MoriKen254]

@AriYu
ありがとうございます。理解しました。

まず、通常運転中は、actionlibがゴールしないことを前提とする。
定期的にactionlibのstatusをチェックし、もしゴールしてしまっていたらそれは異常な状態と判断する。そこで、waypointのインデックスをインクリメントする、といったイメージですかね。

現在、30秒ごとに経過時間を出力していると思いますが、あそこの分岐に入れ込むのは悪くないかもしれませんね。

[AriYu]

@MoriKen254 @forno
いろいろご意見ありがとうございました。
とりあえず、

現在、30秒ごとに経過時間を出力していると思いますが、あそこの分岐に入れ込むのは悪くないかもしれませんね。

なども含めて実装後、実験を行ってみたいと思います。
実験で改善した場合にはcloseしたいと思います。