...

• The constructor calculates the length of each waysegment between two neibourghed points.
• The constructor defines a vector called "jobs" where all jobs are listed with the information, if the minimal distance of 0.01 between two vehicles is undergone (called collision) or not. All possible collisions, between all pairs of path nodes are checked. The result (boolean: collision yes/no?) is cached. The collision decision is based on the rectangle-rectange distance between the vehicles.
• Create a map_match_pose(): A function which defines for each path if it can be matched to an edge on the map
• Create a find_subsequent_edges(): A function which finds the subsequent edges of a trajectoryCreate a function which helps you to move along the route. Take into account the remaining distance on the current edge to next node and the changing to the next edge if neccessary.
  

  The reference state of a vehicle in the graph is stored as the triple (edge_index , path(called current_edge_index , s_offset [in meters]):

  size_t current_edge_index = 0;
  size_t

  in the sourcecode), path_index (called current_edge_path_index

  = 0;
  double

  in the sourcecode), s_offset [in meters](called delta_s_path_node_offset

  = 0;

  in the sourcecode))

  The following image shows a reference state that would be stored as (edge_index = 11, path_index = 1, s_offset = 0.01m).

  The function LaneGraphTools::

move_along_route(): A function which helps you to move along the route. It moves the reference state along the given route by a given distance and takes into account the remaining distance

...

on the current edge to next node and the changing to the next edge if neccessary. For example, assume that the distance between path nodes in the above image is 0.02 m (except for the end nodes 0/3, which are in the same location). We want to move the reference state by 0.055 m.

Inputs: route = (11, 42), state = (11, 1, (0.01+0.055))

The state input is then modified to (42, 1, 0.015).

https://github.com/embedded-software-laboratory/cpm_lab/blob/master/high_level_controller/examples/cpp/central_routing/src/lane_graph_tools.cpp

...

• Create a vector that contains the speed profile of each vehicle. The minimum of the maximum speed of the vehicle and the speed of the previous timestep is to be chosen and written in the vector.

  How the vehicle moves along the path over time is defined by the speed profile. The speed profile is the function of the vehicle speed over time. When the speed profile is constructed and modified, it is done such that the given maximum speed, minimum speed and maximum acceleration are never exceeded. When both the speed profile and the path are given, they can be combined to form the trajectory. The traveled distance (arc length) is calculated by integrating (i.e. summing) the speed profile.

• Create extend_random_route(): a function that randomly adds new edges to the current trajectory for each vehicle. For each vehicle a random route is selected. This route is dynamically extended as time progresses. The route is represented as a list of edge indices. Since every edge is part of a cycle, the route can be extended indefinitely.

https://github.com/embedded-software-laboratory/cpm_lab/blob/master/high_level_controller/examples/cpp/central_routing/src/geometry.hpp

• Create apply_timestep(): 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 extend_random_route().
• get_trajectory_points():  a function to get trajectory points for each vehicle. This shall contain It contains the time elapsed in nanoseconds, the position in x and y and the speed for the direction in x and in y.Create
• avoid_collisions(): 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

  https://github.com/embedded-software-laboratory/cpm_lab/blob/master/high_level_controller/examples/cpp/central_routing/src/MultiVehicleTrajectoryPlanner.cpp#L83

  
  Collision prediction

...

Class "MultiVehicleTrajectoryPlanner"

• Create add_vehicle(): a function that can add a new vehicle.
• Create start(): a function called "start" that will buffer the trajectory planning. Implement This function includes the collision avoidance in this function.

Main Program

Waiting Mode

...