This page describes the usage and function of the Central Routing Example.
Usage
First make sure that the supporting software (LCC, IPS) is running and that the vehicles are online and placed on the map.
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.
How it works
The program is based on the routing graph for the map layout.
Sources:
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.
Waiting Mode
The program starts in a 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 runs independently of the vehicle's actual behaviour. It is assumed that the vehicles follow the reference trajectories with sufficient accuracy.
Random Route Selection
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.
Moving along the Route
The current reference state of a vehicle in the graph is stored as the triple (edge_index, path_index, s_offset [in meters]). It is stored here:
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).
Speed Profile + Path => Trajectory
The combination of the route and the path nodes define a path which determines where a vehicle should drive, but not when.
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 v(t). 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. This is done in two places in the implementation, here for the planning, and here and here for the execution of the plan.
The trajectory must contain the vehicle velocity vector (see Vehicle Commands for details). The velocity vector is calculated from the direction vector and the speed, with the equation:
[velocity_x, velocity_y] = [speed * direction_x, speed * direction_y] = [speed * cos(yaw), speed * sin(yaw)]
The direction vector is given for every path node in the graph.
Collision Test
TODO
Collision Avoidance
TODO
To be continued ...
Overview
Content Tools