...

cd ~\dev\software\central_routing_example

Goal

A variable number of vehicles drive randomly on the map with no collisions:

...

Map representation

The program is based on the routing graph for the map layout.

...

The graph edges correspond to center lines, along which a vehicle may drive. Parallel lanes are not modeled, instead extra edges are added for lane changes in discrete locations.

Preparation I : Write a class "LaneGraphTools"

In this class implement the follwing requirements:

...

cd ~/dev/software/central_routing_example/src/LAneGraphTools.cpp

Preparation II: Write a class called "VehicleTrajectoryPlanningState"

In this class implement the follwing requirements:

...

• Create a function that can apply the movement of the vehicle for each timestep to the map. It will buffer the transition from the planning phase to the buffer phase. Use the function "move_along_route" of your LaneGraphTools. Get new random route description from your new function.
• Write a function to get trajectory points for each vehicle. This shall contain the time elapsed in nanoseconds, the position in x and y and the speed for the direction in x and in y.
• Create a function that sets the speed for each vehicle.
• Create a function that avoids collisions between two vehicles by measuring the distance between two vehicles and reducing the speed of one vehicle. Use priority based decision making, when collision is predicted. In this case choose the vehicle witht he smaller ID to reduce the speed. This process has to be repeated until all predicted collisions are resolved. If it is not possible to resolve all collisions, the program shall stop. As a result, vehicle 1 never slows down. Vehicle 2 slows to avoid vehicle 1. Vehicle 3 slows to avoid vehicles 1 and 2, and so on.  The speed is increased and decreased in small increments over time, to maintain the maximum acceleration.

  

  Sources:

  Priorities  cd ~/dev/software/central_routing_example/src/MultiVehicleTrajectoryPlanner.cpp#L49
  
  Collision prediction  cd ~/dev/software/central_routing_example/src/VehicleTrajectoryPlanningState.cpp#L121
  
  Speed reduction  cd ~/dev/software/central_routing_example/src/VehicleTrajectoryPlanningState.cpp#L162

Preparation III: Write a class called "MultiVehicleTrajectoryPlanner"

• Create a function that can add a new vehicle.
• Create a function called "start" that will buffer the trajectory planning. Implement the collision avoidance in this function.

Main Program

Waiting Mode

The program shall start in waiting mode. It will transition into the running mode, when the following conditions are met.

• Vehicle IPS observations were received for all expected vehicles.
• All vehicles could be matched to the graph. All vehicles must be close to a center-line and oriented correctly.

Running Mode

The route planning and collision avoidance shall run independently of the vehicle's actual behaviour. It is assumed that the vehicles follow the reference trajectories with sufficient accuracy.

...

• Routing phase: Select route sufficiently far into the future. However the timeline is not precisely defined at this stage.
• Planning phase: Fix route and phase. Only the speed profile can be changed to avoid collisions. The planning phase considers a constant (but configurable) time span (currently 48 sec).
• Buffering phase: Fully define and commit trajectory. The purpose of the buffer phase is to allow for delays, both in sending the commands over the network and in the planning phase. For a particularly difficult collision avoidance, the planning phase may have a long computation time. The buffer phase can "absorb" this delay jitter.

Apply

If you use the real lab application, make sure that the supporting software (LCC, IPS) is running and that the vehicles are online and placed on the map. Assure that you activated Lab mode.

...