Closed mchlswyr closed 2 years ago
sv/SDVPlanner.py
, added:
...
if frenet_traj is None:
log.warn("plan_maneuver return invalid trajectory.")
if cand is not None:
for traj in cand:
print(traj.unfeasibility_cause)
pass
else:
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_intersection_redlight.osm
until the vehicle "got stuck"In sv/CostFunctions.py
, added:
def direction_cost(frenet_traj:FrenetTrajectory, start_state):
''' Rejects trajectories that have a backwards component. '''
T = frenet_traj.T
# if end state is before start state
if start_state[0] > frenet_traj.fs(T):
print("End before start.")
return 1
# if velocity is negative at any point
dt = float(T) / 100.0
for i in range(100):
t = dt * i
if frenet_traj.fs_vel(t) < 0:
print("Negative vel.")
return 1
return 0
python3.8 GSServer.py -s scenarios/test_scenarios/gs_intersection_redlight.osm
until the vehicle "got stuck"sv/CostFunctions.py
, added:
def direction_cost(frenet_traj:FrenetTrajectory, start_state):
''' Rejects trajectories that have a backwards component. '''
T = frenet_traj.T
# if end state is before start state
if start_state[0] > frenet_traj.fs(T):
print("End before start.")
return 1
# if velocity is negative at any point
dt = float(T) / 100.0
for i in range(100):
t = dt * i
if frenet_traj.fs_vel(t) < 0:
print(T)
print(t)
print(frenet_traj.fs_vel(t))
print("Negative vel.")
return 1
return 0
t
was 0.0 and frenet_traj.fs_vel(t)
was -8.881784197001252e-16@rodrigoqueiroz in direction_cost()
can we add some room for very small values? So if frenet_traj.fs_vel(t) <= -0.001
, for example? What would be a good value?
In scenarios like
scenarios/test_scenarios/gs_intersection_redlight.osm
andscenarios/test_scenarios/gs_intersection_redlight_follow.osm
, the vehicle sometimes "gets stuck" on the stop line after the red light. In the frenet plot in the dashboard, all of the "stuck" vehicle's trajectories are red (invalid), and new red ones keep being generated. The reason the vehicle "gets stuck" is because all of the trajectories being planned are invalid, so the vehicle just stays where it was when it had stopped.To resolve this issue, we'll need to know why all of the trajectories being planned after the light turns green are invalid.