...

• 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

  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"

...