You find the source code for our examples in the software-folder:
cd ~\dev\software\central_routing_example
A variable number of vehicles drive randomly on the map with no collisions:
The program is based on the routing graph for the map layout.
Sources:
cd ~/dev/software/central_routing_example/src/lane_graph.hpp
cd ~/dev/software/matlab_scripts/map_print/map_print2/lane_graph.m
Each node is a base point for the trajectory. 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.
In this class implement the follwing requirements:
The reference state of a vehicle in the graph is stored as the triple (edge_index, path_index, s_offset [in meters])
:
size_t current_edge_index = 0;
size_t current_edge_path_index = 0;
double delta_s_path_node_offset = 0;
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
moves the reference state along the given route by a given distance.
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).
cd ~/dev/software/central_routing_example/src/LAneGraphTools.cpp
In this class implement the follwing requirements:
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.
cd ~/dev/software//central_routing_example/src/geometry.hpp
Priorities
cd ~/dev/software/central_routing_example/src/MultiVehicleTrajectoryPlanner.cpp#L49
cd ~/dev/software/central_routing_example/src/VehicleTrajectoryPlanningState.cpp#L121
cd ~/dev/software/central_routing_example/src/VehicleTrajectoryPlanningState.cpp#L162
The program shall start in waiting mode. It will transition into the running mode, when the following conditions are met.
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:
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.
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.