search

sbgisen / autoware_ai_planning

Apache License 2.0
0 stars 0 forks source link

Invalid input index range #22

Closed nyxrobotics closed 1 year ago

nyxrobotics commented 1 year ago

概要 asterでの回避動作中、下記のwarning Invalid input index range https://github.com/sbgisen/autoware_ai_planning/blob/be56f15de2f670cb808a4ba1e16ee4b92d70cdcc/waypoint_planner/src/velocity_set/velocity_set_path.cpp#L196

デバッグ情報追加して表示すると下記→endがおかしいようです

[ INFO] [1681191206.078246121, 1411.590000000]: PLANNING -> STOPPING, Cannot find path
[ INFO] [1681191206.443538471, 1411.692000000]: STOPPING -> PLANNING, Start A* planning
[ INFO] [1681191208.315942409, 1412.196000000]: Found obstacle while AVOIDING -> replanning
[ INFO] [1681191208.328846176, 1412.202000000]: Found GOAL at goal_waypoint_index = 436
[ INFO] [1681191208.352237752, 1412.202000000]: PLANNING -> AVOIDING, Found path
[ INFO] [1681191211.991163505, 1413.192000000]: AVOIDING -> STOPPING, Abort avoiding
[ INFO] [1681191213.859748837, 1413.690000000]: STOPPING -> PLANNING, Start A* planning
[ INFO] [1681191215.685226616, 1414.197000000]: Found obstacle while AVOIDING -> replanning
[ INFO] [1681191215.701872843, 1414.215000000]: Found GOAL at goal_waypoint_index = 436
[ INFO] [1681191215.721180679, 1414.233000000]: PLANNING -> AVOIDING, Found path
[ WARN] [1681191217.936359789, 1414.773000000]: Waiting for current_pose topic ...
[ WARN] [1681191217.936401797, 1414.773000000]: Waiting for current_velocity topic ...
[ WARN] [1681191217.958148214, 1414.791000000]: Invalid input index range: begin = 13, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191217.958178045, 1414.791000000]: Invalid input index range: begin = 12, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191217.958190379, 1414.791000000]: Invalid input index range: begin = 11, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191217.958201229, 1414.791000000]: Invalid input index range: begin = 10, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191219.765059476, 1415.271000000]: Invalid input index range: begin = 13, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191219.765104541, 1415.271000000]: Invalid input index range: begin = 12, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191219.765115811, 1415.271000000]: Invalid input index range: begin = 11, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191219.765125157, 1415.271000000]: Invalid input index range: begin = 10, end = 9, PrevWaypointsSize = 200
[ WARN] [1681191221.614062210, 1415.772000000]: Invalid input index range: begin = 12, end = 8, PrevWaypointsSize = 200
[ WARN] [1681191221.614097783, 1415.772000000]: Invalid input index range: begin = 11, end = 8, PrevWaypointsSize = 200
[ WARN] [1681191221.614109293, 1415.772000000]: Invalid input index range: begin = 10, end = 8, PrevWaypointsSize = 200
[ WARN] [1681191221.614114783, 1415.772000000]: Invalid input index range: begin = 9, end = 8, PrevWaypointsSize = 200
[ WARN] [1681191223.170295891, 1416.270000000]: Invalid input index range: begin = 11, end = 7, PrevWaypointsSize = 200
[ WARN] [1681191223.170344212, 1416.270000000]: Invalid input index range: begin = 10, end = 7, PrevWaypointsSize = 200
[ WARN] [1681191223.170354327, 1416.270000000]: Invalid input index range: begin = 9, end = 7, PrevWaypointsSize = 200
[ WARN] [1681191223.170362144, 1416.270000000]: Invalid input index range: begin = 8, end = 7, PrevWaypointsSize = 200
[ WARN] [1681191225.019371965, 1416.771000000]: Invalid input index range: begin = 10, end = 6, PrevWaypointsSize = 200
[ WARN] [1681191225.019404648, 1416.771000000]: Invalid input index range: begin = 9, end = 6, PrevWaypointsSize = 200
[ WARN] [1681191225.019412233, 1416.771000000]: Invalid input index range: begin = 8, end = 6, PrevWaypointsSize = 200
[ WARN] [1681191225.019419127, 1416.771000000]: Invalid input index range: begin = 7, end = 6, PrevWaypointsSize = 200
[ WARN] [1681191226.841554214, 1417.272000000]: Invalid input index range: begin = 9, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191226.841591114, 1417.272000000]: Invalid input index range: begin = 8, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191226.841600781, 1417.272000000]: Invalid input index range: begin = 7, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191226.841608263, 1417.272000000]: Invalid input index range: begin = 6, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191228.677989050, 1417.770000000]: Invalid input index range: begin = 9, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191228.678018746, 1417.770000000]: Invalid input index range: begin = 8, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191228.678028902, 1417.770000000]: Invalid input index range: begin = 7, end = 5, PrevWaypointsSize = 200
[ WARN] [1681191228.678036840, 1417.770000000]: Invalid input index range: begin = 6, end = 5, PrevWaypointsSize = 200
[ INFO] [1681191233.744515934, 1419.186000000]: [getClosestWaypoint] current_path.waypoints.size() < 2
[ WARN] [1681191233.744544867, 1419.186000000]: [AstarAvoid::updateClosestWaypoint] getClosestWaypoint failed, -> RELAYING
[ INFO] [1681191233.751979578, 1419.195000000]: RELAYING -> STOPPING, Decelerate for stopping

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

nyxrobotics commented 1 year ago

VelocitySetPath::calcIntervalの引数のbeginがendより大きい ↓ VelocitySetPath::calcChangedVelocityの引数のrange.at(0)がrange.at(1)より大きい ↓ VelocitySetPath::changeWaypointsForDecelerationで発生 https://github.com/sbgisen/autoware_ai_planning/blob/be56f15de2f670cb808a4ba1e16ee4b92d70cdcc/waypoint_planner/src/velocity_set/velocity_set_path.cpp#L91

[ INFO] [1681196945.980337540, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 12 -> 8
[ WARN] [1681196945.980365066, 6149.463000000]: Invalid input index range: begin = 12, end = 8, PrevWaypointsSize = 200
[ INFO] [1681196945.980376581, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 11 -> 8
[ WARN] [1681196945.980385718, 6149.463000000]: Invalid input index range: begin = 11, end = 8, PrevWaypointsSize = 200
[ INFO] [1681196945.980394723, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 10 -> 8
[ WARN] [1681196945.980404308, 6149.463000000]: Invalid input index range: begin = 10, end = 8, PrevWaypointsSize = 200
[ INFO] [1681196945.980413492, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 9 -> 8
[ WARN] [1681196945.980424002, 6149.463000000]: Invalid input index range: begin = 9, end = 8, PrevWaypointsSize = 200
[ INFO] [1681196945.980434313, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 8 -> 8
[ INFO] [1681196945.980444497, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 7 -> 8
[ INFO] [1681196945.980459720, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 6 -> 8
[ INFO] [1681196945.980470018, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 5 -> 8
[ INFO] [1681196945.980481238, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 4 -> 8
[ INFO] [1681196945.980492010, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 3 -> 8
[ INFO] [1681196945.980502331, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 2 -> 8
[ INFO] [1681196945.980512963, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 1 -> 8
[ INFO] [1681196945.980523580, 6149.463000000]: VelocitySetPath::changeWaypointsForDeceleration -> calcChangedVelocity: 0 -> 8
[ INFO] [1681196945.980534243, 6149.463000000]: VelocitySetPath::avoidSuddenAcceleration -> calcChangedVelocity: 0 -> 0
nyxrobotics commented 1 year ago

https://github.com/sbgisen/autoware_ai_planning/blob/be56f15de2f670cb808a4ba1e16ee4b92d70cdcc/waypoint_planner/src/velocity_set/velocity_set_path.cpp#L79

このextraの4回分で発生しているようです 修正案↓ https://github.com/sbgisen/autoware_ai_planning/blob/be56f15de2f670cb808a4ba1e16ee4b92d70cdcc/waypoint_planner/src/velocity_set/velocity_set_path.cpp#L90 ↓

    std::array<int, 2> range = {index, obstacle_waypoint + extra};