eliminate false invalid trajectory exceptions - implement proper vehicle stopping

[mantkiew]

Currently, we're getting false invalid trajectory exceptions which happen when the vehicle is coming to a stop. For example:

pixi run gss -s scenarios/coretest_scenarios/straightdrive.gs.osm
...
I0324 16:48:38.261729 32884 SDVPlanner.py:183] VID 1 started maneuver M_STOP
W0324 16:48:42.739907 32884 ManeuverModels.py:367] PLAN STOP Vehicle 1 target position 0.8385198894792154 is too close or behind
W0324 16:48:42.894244 32884 SDVPlanner.py:226] VID 1 plan_maneuver return invalid trajectory.
W0324 16:48:42.928690 32884 ManeuverModels.py:367] PLAN STOP Vehicle 1 target position 0.4816815854623684 is too close or behind
W0324 16:48:43.097340 32884 SDVPlanner.py:226] VID 1 plan_maneuver return invalid trajectory.
W0324 16:48:43.147627 32884 ManeuverModels.py:367] PLAN STOP Vehicle 1 target position 0.25584369571735976 is too close or behind
W0324 16:48:43.300171 32884 SDVPlanner.py:226] VID 1 plan_maneuver return invalid trajectory.
W0324 16:48:43.340917 32884 ManeuverModels.py:367] PLAN STOP Vehicle 1 target position 0.12317520480068822 is too close or behind
W0324 16:48:43.503281 32884 SDVPlanner.py:226] VID 1 plan_maneuver return invalid trajectory.
W0324 16:48:43.553952 32884 ManeuverModels.py:367] PLAN STOP Vehicle 1 target position 0.03659852013829834 is too close or behind
W0324 16:48:43.706186 32884 SDVPlanner.py:226] VID 1 plan_maneuver return invalid trajectory.
I0324 16:48:44.122864 32884 SDVPlanner.py:274] PLANNER PROCESS END. Vehicle1
E0324 16:48:44.143295 32813 GSServer.py:164] Vehicle under test reached its target

So, eventually, the vehicle reaches the speed of 0.03 but we see invalid trajectory a few times.
The maneuver M_STOP does return None, None because it has no way to return a proper stopping trajectory.
That causes the exception.

So, we should only be getting invalid trajectory in true cases, for example, when it is impossible to actually stop when the speed is too high and maximum deceleration would be exceeded.

@rodrigoqueiroz I cannot add SheaCardozo to this issue. Why?

[rodrigoqueiroz]

From what I can see from my side, he didn’t accept the invitation yet.

[drodeur]

@mantkiew You meant that in your example you are reaching a longitudinal distance (i.e., s frenet offset) of 0.03659852013829834 with respect to the targetted stop position. However, there is no guarantee of what is the speed of the vehicle at that state.

For instance, imagine you were planning a stop behaviour 2 meters aways from your current position while driving at 100km/h. You certainly not have the time to stop there altough the difference between your current s offset with the target position s offset is decreasing. So it should raise invalid trajectory that are distinguishable from the one in Michal example for which the deceleration is feasible.

Also, when a deceleration is feasible, it needs to return a valid trajectory. So remove the following hack:

geoscenarioserver/sv/ManeuverModels.py

Line 365 in 55bd497

if target_pos < 1:

Having None, None at line 370 performs a stop by forcing the vehicle to have no best nor candidate trajectory. Such wrongly claimed invalid trajectory are convoluting with improper usage of maneuvers, for which the simulation needs to immediately terminate since any further state would be invalid or for which we need to replan with another maneuver/configuration.

Rephrasing, without disturbance, a valid planned trajectory need to be implementable along its planned interval. I.e., for a 3 second trajectory that was valid 2s ago, when there has been no disturbance, it should still be valid and implementable without hack.