search

sbgisen / autoware_ai_planning

Apache License 2.0
0 stars 0 forks source link

Aster回避後のwaypointsと回避前のwaypointsのindexの数が合わない #13

Closed Alpaca-zip closed 1 year ago

Alpaca-zip commented 1 year ago

概要 astar_avoid.cppにおいてstate_ == AstarAvoid::STATE::AVOIDINGからstate_ == AstarAvoid::STATE::RELAYINGに戻る際に、goal_waypoint_indexとend_of_avoid_indexが一致しない。 https://github.com/sbgisen/autoware_ai_planning/blob/a9553172cdb642b8af8120ea10e553933754b91f/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L367 https://github.com/sbgisen/autoware_ai_planning/blob/a9553172cdb642b8af8120ea10e553933754b91f/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L315

修正しないとどう困るか updateClosestWaypoint()にてcurrent_waypoints_がbase_waypoints_に変化した時に、indexのずれによってclosest_local_indexが-1になる場合がある。 https://github.com/sbgisen/autoware_ai_planning/blob/a9553172cdb642b8af8120ea10e553933754b91f/waypoint_planner/src/astar_avoid/astar_avoid.cpp#L447-L455 これが発生するとすべてのwaypointの中から最も近いwaypointを探すことになるので、経路がジャンプする原因となる。 回避経路が出た際に別のwaypointに戻ってしまう #5

修正案

  1. goal_waypoint_indexとend_of_avoid_indexが一致するようにastar_avoid.cpp内で調整する bugfix/aster_avoid_index, bugfix/waypoint_plannerにて作業中。 発生条件がAVOIDING->RELAYINGやAVOIDING->STOPPING->PLANNING->AVOIDING->RELAYINGなど複数存在しており、すべての条件分岐について網羅ができていない状況。
  2. current_waypoints_をbase_waypoints_に戻さず、mergeAvoidWaypoints()で更新されるavoid_waypoints_を利用する bugfix/closest_waypoint_indexにて作業中。 常に線形にwaypointを追い続けることができるが、Asterで無理なpathが生成された際のリカバリが難しい。
  3. goal_waypoint_indexとend_of_avoid_indexが一致するようにpath.poses.size()を変更できるようにする 未検証。 astar_search.cppを修正する必要あり。
nyxrobotics commented 1 year ago

bugfix/waypoint_plannerで検証しましたのでDraft PR出しておきます。indexの切り替え自体はうまく行くようになったと思います。