mchlswyr
commented
3 years ago The suggested solution doesn't really work. It places the vehicle in the correct position along its trajectory, but every other object is in the wrong position relative to the vehicle and its trajectory. Instead, it looks like the problem is that the trajectory is outdated: since there are no new valid trajectories, the old one keeps being followed. The planner's vehicle, other vehicles, pedestrians, goal point, regulatory elements, and static objects are all positioned in a new frenet frame (with a new ref_path_origin), but the trajectory is positioned in an old frenet frame.
So, to resolve this issue, we need to update the old trajectory so that it is postioned in the new frenet frame.
A vehicle's frenet trajectory is now relative to that vehicle's reference path, so the trajectory will now start at an s-value of 0 (instead of some nonnegative value along the global path). In
get_planner_state(), insv/SDVPlanner.py, the planner's vehicle's s-value is always set to 0. This is then passed along, inrun_planner_process(), to the dashboard. Thus, the dashboard always shows the vehicle at the start of its trajectory, at an s-value of 0.You can see this when running
scenarios/test_scenarios/gs_intersection_greenlight.osm, for example. As the vehicle approaches its goal, no new trajectories are produced, and it follows a short trajectory to its goal. That short trajectory appears to be pushed by the vehicle passed the goal line in the frenet plot. All that is really happening is that the frenet plot is centered around the vehicle, the vehicle is always at an s-value of 0, and the trajectory always starts at an s-value of 0. So the vehicle is always at the start of its trajectory, while it moves passed the goal line in the background. Thus, it looks like it is pushing its trajectory passed the goal line.To solve this issue, we will have to update the planner's vehicle's position along its current trajectory before sending the vehicle's state to the dashboard.