Understanding the Motion Planner

The CPM Lab motion planner is the high-level controller that turns the current vehicle state, map data, and system triggers into motion commands for one or more vehicles. In the source code it is also called the high-level controller, because it sits above the vehicle controller and below the experiment setup.

The planner runs in the planner domain (ROS_DOMAIN_ID=1). When you deploy with an internal planner, the control flow starts it in the /planner/ namespace and passes the selected vehicle IDs and the planner period as launch parameters; the Control Center is only the user interface for this. The planner variants that come with the CPM Lab are listed below.

Planner overview

The planner receives the vehicle state list and the lanelet map from the bridge, then publishes motion commands back to the bridge. It also listens for system trigger messages so it can stop when the experiment stops or when a safety condition is triggered.

        ---
config:
  layout: elk
  startOnLoad: false

---
flowchart TB
     BRIDGE[Bridge]

     subgraph PLANNER["Domain ID 1 (1 Hz - 50Hz)"]
         direction TD
         M1[Motion Planner 1]
         DOTS[...]
         MN[Motion Planner N]
     end

     BRIDGE -->|states| PLANNER
     BRIDGE <-->|trigger| PLANNER
     BRIDGE -->|map| PLANNER
     PLANNER -->|commands| BRIDGE
     linkStyle 2 stroke-dasharray: 5
     style DOTS fill:transparent,stroke:transparent,color:#111

Motion planner variants and their bridge interface

The planner variants share the same communication pattern, but they differ in how they compute the motion command:

  • Central routing is the general multi-vehicle example planner. It works for arbitrary maps with up to 10 vehicles, but beyond that it may no longer find valid solutions.

  • Path tracking is a simple example planner that controls a single vehicle on the outer loop.

  • Manual control lets you control all selected vehicles with a gamepad.

  • Example planner lets every vehicle follow the lane center line. It is the template for your own planner, see Your First Motion Planner.

  • SFB (cpm_lab_sfb_py) is the Python planner of the SFB/TRR 339 demonstration.

  • SigmaRL (cpm_lab_sigmarl_control) drives the vehicles with a reinforcement-learning policy; it needs a trained model, see Command Reference.

The packages are in the high-level-controller repository.

All planner variants use the same basic ROS interface: they subscribe to the vehicle state list, map, and system trigger topics, and they publish motion commands back to the bridge. The bridge then forwards the selected commands to the vehicle controller side of the system.

Planner setup

When you deploy with an internal planner, the control flow starts the selected package in ROS_DOMAIN_ID=1. A launch file is started with ros2 launch <package> <launch-file> vehicle_ids:=[...] period:=<ms>, an executable with the parameters vehicle_ids and period. The vehicle IDs are all vehicles that were online when you deployed, and the period is the update period selected in the deploy step.

The Control Center lists a package as planner if it is installed and contains one of the following:

  • a launch file *.launch.xml that declares the argument vehicle_ids,

  • a launch file whose name contains planner and ends with launch.py,

  • an executable whose name contains planner.

Packages installed under /opt are only listed if their name starts with cpm_lab. To make your own planner selectable, give its launch file the arguments vehicle_ids and period and install it in your workspace.

If you run a planner externally, use ROS_DOMAIN_ID=1 so it can communicate with the bridge in the planner domain, and deploy without the internal planner, see External Motion Planner.

Planner interface

All topics are in the /planner namespace of ROS_DOMAIN_ID=1:

Topic

Type

Direction

/planner/vehicle_states

cpm_lab_lab_msgs/VehicleStateList

bridge ➝ planner, every period

/planner/map

cpm_lab_map_msgs/Lanelet2MapBin

bridge ➝ planner (transient local)

/planner/system_trigger

cpm_lab_lab_msgs/SystemTrigger

both directions

/planner/command

cpm_lab_lab_msgs/VehicleCommand

planner ➝ bridge (reliable)

Before an experiment can start, the planner has to report that it is ready:

  1. After the deployment, the bridge publishes a READY_REQUEST trigger on /planner/system_trigger every period.

  2. The planner answers with a READY trigger for every vehicle it controls. The source_id must be planner_<vehicle_id>, for example planner_3.

  3. As soon as all vehicles are online and the planner is ready for all deployed vehicle IDs, the bridge reports ready and the experiment can be started.

  4. The START and STOP triggers of the experiment are forwarded to /planner/system_trigger. The planner should stop sending commands when it receives STOP.

During the experiment the bridge forwards each command to /lab/vehicle_<id>/command of the vehicle given in the command.