Vehicle "Gets Stuck" On Stop Line After Red Light

[mchlswyr]

In scenarios like scenarios/test_scenarios/gs_intersection_redlight.osm and scenarios/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.

[mchlswyr]

• In 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:
  ...

• Ran python3.8 GSServer.py -s scenarios/test_scenarios/gs_intersection_redlight.osm until the vehicle "got stuck"
• When the vehicle got stuck, the unfeasibility cause was direction

[mchlswyr]

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

• Ran python3.8 GSServer.py -s scenarios/test_scenarios/gs_intersection_redlight.osm until the vehicle "got stuck"
• When the vehicle got stuck, "Negative vel." was printed

[mchlswyr]

• 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(T)
          print(t)
          print(frenet_traj.fs_vel(t))
          print("Negative vel.")
          return 1
  return 0

• When the vehicle "got stuck", t was 0.0 and frenet_traj.fs_vel(t) was -8.881784197001252e-16

[mchlswyr]

@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?