You find the source code for our examples in the software-folder:
C++
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:
You can modify this lane graph by using the programs written in Matlab and can be found in:
Tools include: adding edges, deleting edges, deleting points and selecting points.
Class "LaneGraphTools"
In this class implement the follwing requirements:
- 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.
- map_match_pose(): A function which defines for each path if it can be matched to an edge on the map
- find_subsequent_edges(): A function which finds the subsequent edges of a trajectory
The reference state of a vehicle in the graph is stored as the triple (
edge_index(calledcurrent_edge_indexin the sourcecode),path_index(calledcurrent_edge_path_indexin the sourcecode),s_offset[in meters](calleddelta_s_path_node_offsetin the sourcecode))The following image shows a reference state that would be stored as (edge_index = 11, path_index = 1, s_offset = 0.01m).
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).
Class "VehicleTrajectoryPlanningState"
In this class implement the follwing requirements:
- 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.
- 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.
- 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 extend_random_route().
- get_trajectory_points(): a function to get trajectory points for each vehicle. It contains the time elapsed in nanoseconds, the position in x and y and the speed for the direction in x and in y.
- avoid_collisions(): 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
Collision prediction
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
Class "MultiVehicleTrajectoryPlanner"
- add_vehicle(): a function that can add a new vehicle.
- start(): a function that will buffer the trajectory planning. This function includes the collision avoidance.
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.
Define phases for your program:
- 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
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 follow the steps described in Running an HLC. 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.
Overview
Content Tools