You find the source code for our examples in the software-folder:

C++

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

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 original lane graph is defined as followed:

Each node represents the index in the lane graph to receive the information you need for your trajectory:

struct LaneGraph{

vector node_x;
vector node_y;
vector nodes_cos;
vector nodes_sin;
vector edges_start_index;
vector edges_end_index;
vector edges_x;
vector edges_y;
vector edges_sin;
vector edges_cos;
}

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. All values from your lane graph are already included in the struct which you find in:

https://github.com/embedded-software-laboratory/cpm_lab/blob/master/high_level_controller/examples/cpp/central_routing/include/lane_graph_full/lane_graph.hpp


You can modify this lane graph by using the programs written in Matlab and can be found in:

https://github.com/embedded-software-laboratory/cpm_lab/blob/master/tools/map_print/map_print2/lane_graph.m

Tools include: adding edges, deleting edges, deleting points and selecting points.

Preparation I : Write a class "LaneGraphTools"

In this class implement the follwing requirements:

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

Preparation II: Write a class called "VehicleTrajectoryPlanningState"

In this class implement the follwing requirements:

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

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

Speed reduction
https://github.com/embedded-software-laboratory/cpm_lab/blob/master/high_level_controller/examples/cpp/central_routing/src/VehicleTrajectoryPlanningState.cpp#L217

Preparation III: Write a class called "MultiVehicleTrajectoryPlanner"

Main Program

Waiting Mode

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

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.

Define phases for your program:

Apply

Start via Lab Control Center

If you use the real lab application, make sure that the supporting software (LCC, IPS) is running and that the vehicles are online (only the ID LED is still blinking, all three other LEDs are turned on) and placed on the map. Assure that you activated Lab mode in the LCC. Then load your example in the LCC and hit "Deploy". The example does not start, if collision cannot be avoided. Therefore make sure not to have to many vehicles selected. Usually up to 10 vehicles will work fine.

Start via Terminal

In a new terminal, run:

cd ~/dev/software/central_routing_example
./run.bash 2,3,5,7,11

The list of integers in the run command must be the exact list of vehicle IDs for the active vehicles. The resulting trajectories will be shown in the LCC.