Spline, Global Path and Ref Path, and Lane Swerve Changes

[mchlswyr]

Changes

Global Path and Reference Path

• There are now two types of paths for frenet transformations: the global path and the reference path
• These paths are managed in the SDVPath class, in sv/SDVRoute.py
• The global path is a list of Point3ds generated from the centerline of a lane (a list of Lanelets)
• The reference path is a slice of the global path
• The SDVRoute class, in sv/SDVRoute.py, creates an SDVPath object for each lane in the route (lanelet_route)
• update_global_path(), in SDVRoute, is used to update the current SDVPath object depending on the position that is passed as argument
• update_reference_path(), in SDVRoute, is used to "move" the reference path along the global path
• This is done in the SDVPath class, in update_ref_path(): the reference path is centered around ref_path_origin (which is the origin of the reference path along the global path) and extends from _ref_path_s_start to _ref_path_s_end (which is -100.0 to 100.0 right now)
• The global path is now only used in three places: twice (once when starting from sim state and once when starting from frenet state) in SDV.__init__(), in sv/Vehicle.py, and once in get_planner_state(), in sv/SDVPlanner.py, when finding the new ref_path_origin
• Everywhere else, the reference path is used, which makes the frenet transformations much faster in cases where the global path is long (like on Ring Road)

Spline Fitting

• When generating the global path from a lane, the centerline of that lane is used
• A spline is then fit to that centerline, and equally spaced points along that spline are used for the global path
• The density of these points can be configured in SimConfig.py under POINTS_PER_METER
• You can view each spline (in blue) on top of the route (in red) and map (in black), by uncommenting all of this

Frenet Transformations

• All of the frenet transformations have a new parameter: s_start
• This is the s-value of the first point in the ref_path that is passed as argument
• This is needed since the first point in the reference path will be around -100.0, instead of 0 like in the case of the global path

Behaviour Trees

• Two new btree conditions were added: should_lane_swerve and min_long_vel
• should_lane_swerve is True whenever the planner state's lane_swerve_target is not None
• min_long_vel is True when the vehicle's s_vel is greater than or equal to the vel passed as argument
• Also, the lane swerve maneuver, M_LANESWERVE, was added
• The SDVPath class has member variables for marking the start and end of a "lane swerve zone", as well as the direction of the lane swerve
• The SDVRoute class uses these member variables to keep the vehicle on its route: if the vehicle is coming up to the end of its route along the current global path, and update_reference_path() with plan_lane_swerve=True has been called, then get_lane_swerve_direction will return the direction of the lane swerve
• This is used in sv/SDVPlanner.py to set lane_swerve_target in the planner state
• btrees/sdv/drive.btree has been updated to keep the vehicle on its route: if the vehicle is going faster than 1 m/s, and should_lane_swerve is True, then the planner executes a lane swerve maneuver in the direction of lane_swerve_target
• Note: MLaneSwerveConfig's target_lid can be set in the btree; if it is set, and it is not set to None, then the lane swerve maneuver will be made towards that lane; if it is not set, or it is set to None, then the lane swerve maneuver will be made towards the planner state's lane_swerve_target

New Scenarios

• Added a copy of scenarios/maps/lanelet2_university_weber_alt.osm called scenarios/maps/lanelet2_university_weber_alt_modified.osm
• One of the solid lines was changed to a dashed line
• Added a new scenario: scenarios/test_scenarios/gs_university_weber_right_turn.osm
• In this scenario, the vehicle uses the new drive.btree to make a lane swerve in order to stay on its route and make a right turn towards its goal
• The line the vehicle crosses during the lane swerve is the one that was changed in scenarios/maps/lanelet2_university_weber_alt_modified.osm
• Another new scenario was added: scenarios/test_scenarios/gs_bathurst_cul_de_sac.osm
• This is used to test that routes containing cul-de-sacs work, since there was an issue with this

Other

• In sv/SDVPlanner.py, get_planner_state() has been split into two functions: project_dynamic_objects() and get_planner_state()
• project_dynamic_objects() projects the planner's vehicle's, other vehicles', and pedestrians' future sim state
• get_planner_state() transforms the projected sim state to frenet state, and returns a PlannerState object
• When projecting the sim state of other vehicles and pedestrians, expected_planner_time is used instead of delta_time since delta_time is the time since the start of the planner's vehicle's last trajectory
• In sv/SDVPlanner.py, get_plan() only returns a plan when it is valid and new, and it returns None otherwise
• This way, in sv/Vehicle.py, tick() will only update the global path once when plan.new_frenet_frame is True
• In sv/Vehicle.py, compute_vehicle_state() now returns if sim_time is less than the plan's start time
• Before this change, it wouldn't return and the vehicle would start its new plan early which caused jump back to be reported
• Added PLOT_VEHICLE_ROUTES to SimConfig.py
• When set to True, figures will be opened for each of the vehicle's global paths
• These figures contain the map, route, and global path
• These figures are opened for each vehicle in the scenario
• Note: after the plots are closed for each vehicle, the program will crash with an XIO error (I'm not sure why), so the scenario can't be run when this option is set

Closed Issues

• Closes #54
• This was a problem because the frenet trajectories would go beyond the end of the global path
• The frenet trajectories are now relative to the vehicle's reference path, so it is no longer an issue
• Closes #19
• Spline fitting was added
• Closes #22
• This is related to #19
• Closes #16
• btrees/sdv/drive.btree now includes a lane swerve to stay on route
• Even if the vehicle doesn't perform a lane swerve, it will continue following the global path which will extend as far as possible
• Closes #76
• See here
• Closes #71
• Trajectories in the dashboard are now shifted into the frenet frame they were originally positioned in

Related Issues

• Runtime switch to New Route: #12
• This is an older issue, but with these changes it can probably be resolved pretty easily: if the vehicle isn't on its route anymore, then update lanelet_route, and, if a route was successfully generated, create a new SDVRoute object
• Point Not on Ref Path in Sim to Frenet Transformation: #72
• All of the scenarios/test_scenarios work with the temporary fix

Testing

• Run all of the scenarios in scenarios/test_scenarios:

  bash scripts/run_scenarios.bash -ov

• In scenarios with stop lines, vehicles may get stuck at the stop line; this doesn't always happen and it isn't new
• There should not be any jump back except in one case: on gs_ringroad_stress_loop.osm, some vehicles will cross from the end of the loop back to the start and a jump back of roughly 2500 meters will be reported for these vehicles; this can be ignored
• In gs_straight_reverse.osm, the vehicle just drives in reverse for 10 seconds before the scenario times out
• Otherwise, there should be no jump back, and the vehicles should reach their goals

[mchlswyr]

One optional comment: it would be great to add some more type annotations to function parameters to ease code understanding.

Right. I did this for the SDVRoute and SDVPath classes since we discussed before that type annotations should be used. Maybe we could add the type annotations to all other functions in another PR?

[mantkiew]

Maybe we could add the type annotations to all other functions in another PR?

I think it should be an ongoing effort: whenever PRs add new functions, they should be annotated. Whenever existing function signatures change, they should have the missing annotations added.