回避経路が出た際に別のwaypointに戻ってしまう

[Tacha-S]

概要

以前最近傍点の探索に関するバグを修正したが、 2023/02/27に竹芝エリアで走行した際に経路がほぼ重なり合っている状態で発生した。 おそらく https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L431-L457 この関数内で、 https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L447 の結果がロボットの向き等の条件で-1となったときや 以下のタイミングでも-1でリセットするので https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L173 https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L188

以下で全経路中最も近い点を探し、 https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L433-L436 経路がほぼ重なり合っているために別の経路に乗ってしまった可能性が高い。

再現手順 修正しないとどう困るか 原因 修正案

[Tacha-S]

https://github.com/sbgisen/outdoor_navigation_tools/issues/30 の調査では直ったので我々での改修は反映しなくていいと思っていたが、 https://github.com/autowarefoundation/autoware_ai_planning/pull/36 で以下のあたりは埋め込まれているので要改善 https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L173 https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L188

[Tacha-S]

おそらく落ちるバグを回避すべく不要に上記の行は追加されていそうなので消したほうがいい それでもまだ経路がほぼ重なり合っている環境で経路がジャンプするのであれば closest_waypoint_index_の過去の値が-1でないときに-1を代入せず過去の値を保持する等の工夫が必要な可能性あり。

[Alpaca-zip]

rosbagで検証を進めていたところ、この問題は関数updateClosestWaypoint()が呼ばれてから次に呼ばれるまでに、コールバック関数closestWaypointCallback()によってclosest_waypoint_index_が上書きされてしまうことが原因であることがわかりました。

https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L429

https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L83

別のwaypointに乗ってしまう瞬間は、殆どの場合で以下のように/closest_waypointがジャンプしています。

このようなジャンプが発生し、closest_waypoint_index_が上書きされると、関数updateClosestWaypoint()内の以下の処理に入ったとしても、ジャンプした先の限られた領域における近傍点の探索が行われてしまいます。 https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L437-L456

[Alpaca-zip]

updateClosestWaypoint()が周期的に呼ばれている間は、コールバック関数closestWaypointCallback()によってclosest_waypoint_index_が上書きされないようにした方が良いかもしれません。

[Tacha-S]

closestWaypointCallback()はlane_selectがpublishしたものによるcallbackですか？

[Alpaca-zip]

そうです。以下の/closest_waypointトピックをclosestWaypointCallback()で受け取っています。

[Tacha-S]

previous_numberから少し進んだところまでの範囲内で探していそうなんですが、本当に/closest_waypointが発端ですか？ https://github.com/sbgisen/autoware_ai_planning/blob/f4633fbe7c40b108fba525c13a7ef6f866ab6b09/lane_planner/nodes/lane_select/lane_select_core.cpp#L728-L788 astar_avoidで別の点にjumpする->現在位置が変わる->lane_selectがその点基準にclosest waypointを更新する->astar_avoidのcallbackで強制的に上書きされたように思うとなってたりしませんか？

[Alpaca-zip]

ご確認いただきありがとうございます。すみません、lane_selectまで確認が及んでいなかったので今一度検証してみたいと思います。

[Tacha-S]

いやv1.14.0で確認しているかつdocbaseの方のように/closest_waypointの値に-1が入っているのであれば上の指摘はノイズだと思ってください。

[Alpaca-zip]

docbaseの方に書かせていただいた内容では、autoware_ai_planningをv1.14.0に戻してaster_avoidのみを変更して検証を行っていました。そのときは/closest_waypointの値に-1が入る現象を確認しましたが、autoware_ai_planningをmasterに戻してrosbagで検証した際には/closest_waypointの値に-1が入ることはありませんでした。

[Alpaca-zip]

bugfix/closest_waypoint_indexブランチでprev_indexという変数を追加し、コールバック関数closestWaypointCallback()によってclosest_waypoint_index_が上書きされないように変更を行ったところ、正しい経路に進む挙動を確認することができました。

ただし、以下の処理に到達した結果、別の経路に乗ってしまう場合もありました。 https://github.com/sbgisen/autoware_ai_planning/blob/239a634787239cf362d8c9c7f7cdaa5572dc87f8/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L465-L469

closest_waypoint_index_の過去の値が-1でないときに-1を代入せず過去の値を保持する等の工夫が必要な可能性あり。

こちらも含めて検討してみたいと思います。

[Tacha-S]

bugfix/closest_waypoint_indexブランチでprev_indexという変数を追加し、コールバック関数closestWaypointCallback()によってclosest_waypoint_index_が上書きされないように変更を行った

closestWaypointCallback() で-1を受け取っても、closest_waypoint_index(_なし)が正しそうな値であればclosest_waypoint_index_はclosest_waypoint_index(_なし)の値を使うのかと解釈してましたが、

closestWaypointCallback() で-1を受け取ると

https://github.com/sbgisen/autoware_ai_planning/commit/c815d92f1ba6793e2a34e74b99c50a08d3a7c865#diff-7266c11db1a16d620ca7acb656f9248470e266f601f764b365d7960a7c7e79efR436

ここで、prev_indexが-1となりclosest_waypoint_index(_なし)も経路全体から探す(jumpする可能性あり)。 そしてそのjumpした可能性のあるclosest_waypoint_index(_なし)をもとに新たな位置をclosestWaypointCallback() で受け取り双方が正しそう(-1ではない)値となるとその値(jumpしている可能性あり)を信用するに見えます。

[Tacha-S]

No2が起点で生じるjumpに関してですが、

回避経路走行中に回避経路更新を試みた際に、 https://github.com/sbgisen/autoware_ai_planning/blob/c815d92f1ba6793e2a34e74b99c50a08d3a7c865/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L335-L342 ここでavoid_waypoints_を消した後に現在の経路で最も近い位置が角度の問題等で見つからないとavoid_waypoints_が空になったまま、AVOIDING-> STOPPING -> PLANNING を回り続けupdateClosestWaypointでavoid_waypoints_が空なのでNo2に入る可能性があるかもしれません

resetの部分をadd waypoints before start indexの後にすることで回避できるかもしれないです。

[nyxrobotics]

13 と #19 #20 #21 で対応し、これ以降発生していないので一旦解決とします