Your First Motion Planner

This tutorial shows how to write your own motion planner for the CPM Lab. It is based on the example planner cpm_lab_example_planner_py, a complete but minimal Python planner: every vehicle follows the center line of the lanelets in front of it at a constant speed. You copy it, run it in simulation, and then replace the planning logic with your own.

Note

Prerequisites: you completed First Simulation and know how to create and build a ROS2 Python package, see Start Here. Look up message fields, units and frames in the Interface Reference while you work through this tutorial.

How a Planner Works

A planner runs in the planner domain (ROS_DOMAIN_ID=1) in the /planner namespace. It only talks to the bridge:

        ---
config:
  layout: elk
  startOnLoad: false

---
sequenceDiagram
   participant B as Bridge
   participant P as Your planner
   B->>P: /planner/map (once, transient local)
   loop every period until ready
      B->>P: READY_REQUEST on /planner/system_trigger
      P->>B: READY with source_id planner_<id>, for every vehicle
   end
   B->>P: START
   loop every period
      B->>P: /planner/vehicle_states
      P->>B: /planner/command, one per vehicle
   end
   B->>P: STOP

Messages between the bridge and a motion planner during one experiment

The planner therefore needs four parts:

  1. Map: subscribe to map, decode it and build a routing graph, see Lanelet2 Maps.

  2. Ready handshake: answer every READY_REQUEST with one READY per vehicle. The experiment can only start when the planner is ready for all deployed vehicles.

  3. Planning: on every vehicle_states message, compute one VehicleCommand per vehicle and publish it on command.

  4. Stop: stop publishing commands after STOP.

The vehicle states only arrive while the experiment is running, so the planning step is driven by the bridge. You do not need your own timer.

1. Create Your Package

Create a workspace for your planner and copy the example planner into it. With a source build it is in src/high-level-controller/cpm_lab_example_planner_py. With a Debian installation, clone the high-level-controller repository (SSH key required) and check out the tag of your installed version, so the example matches your packages:

apt policy ros-jazzy-cpm-lab          # "Installed: 0.3.0-..." -> tag 0.3.0
git clone git@git.rwth-aachen.de:CPM/Project/Lab/ros2/components/high-level-controller.git ~/high-level-controller
git -C ~/high-level-controller checkout 0.3.0

Then copy it:

mkdir -p ~/planner_ws/src
cd ~/planner_ws/src
cp -r ~/high-level-controller/cpm_lab_example_planner_py my_planner

Rename the package so it does not collide with the installed example. Replace cpm_lab_example_planner_py with my_planner in:

  • the directory names my_planner/cpm_lab_example_planner_py and resource/cpm_lab_example_planner_py,

  • package.xml, setup.py, setup.cfg and launch/default.launch.xml,

  • the import in example_planner_node.py.

cd ~/planner_ws/src/my_planner
mv cpm_lab_example_planner_py my_planner
mv resource/cpm_lab_example_planner_py resource/my_planner
grep -rl cpm_lab_example_planner_py . | xargs sed -i 's/cpm_lab_example_planner_py/my_planner/g'

The package contains:

File

Content

my_planner/example_planner_node.py

The ROS2 node: map, handshake, planning and commands.

my_planner/path_utils.py

Geometry helpers without ROS, easy to unit test.

launch/default.launch.xml

Starts the node in the planner namespace with the arguments vehicle_ids and period.

config/default.yaml

Default parameters, e.g. speed and path_length.

test/

Unit tests and the ROS2 linters.

2. Build and Run It in Simulation

Build your workspace on top of the CPM Lab installation:

cd ~/planner_ws
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install
source install/setup.bash

With --symlink-install, changes to the Python files take effect without rebuilding. Changes to setup.py, the launch or config files still need colcon build.

Start the lab in a terminal where you sourced your workspace:

export ROS_DOMAIN_ID=21
ros2 launch cpm_lab cpm_lab_rviz.launch.xml

In the Control Center, start the map server with Lab Full.osm and simulated vehicles 1 and 2.

Your planner can now be started in two ways:

  • As internal planner: the control flow lists every package with a launch file that declares the argument vehicle_ids. Select my_planner and default.launch.xml in the planner step and deploy. The control flow starts your planner with the online vehicles and the selected period.

  • As external planner: deploy without the internal planner and start it yourself in a second terminal. This is more convenient while debugging because you see its output directly:

    source ~/planner_ws/install/setup.bash
export ROS_DOMAIN_ID=1
ros2 launch my_planner default.launch.xml vehicle_ids:="[1,2]" period:=400

Once Start is enabled, start the experiment. The vehicles follow the lanes and turn into the first successor at every intersection. If Start never appears, see Deploy Checks.

3. Understand the Code

Parameters. The control flow passes the deployed vehicles and the period as vehicle_ids and period. All other parameters come from config/default.yaml:

self.vehicle_ids = [
    int(vehicle_id) for vehicle_id in self.declare_parameter('vehicle_ids', [1]).value]
self.period_ms = int(self.declare_parameter('period', 400).value)
self.speed = float(self.declare_parameter('speed', 0.5).value)

Map. The map is published once with transient local durability. Subscribing with the same durability delivers it even if your planner starts later:

map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
self.create_subscription(Lanelet2MapBin, 'map', self.on_map, map_qos)

def on_map(self, message):
    self.lanelet_map = cpm_lab_lanelet2_extension_py.from_bin_msg(list(message.data))
    traffic_rules = lanelet2.traffic_rules.create(Locations.Germany, Participants.Vehicle)
    self.routing_graph = RoutingGraph(self.lanelet_map, traffic_rules)

Ready handshake. The planner only reports ready once it has the map. Otherwise it asks the map server to send the map again with a MAP_REQUEST:

if message.type == SystemTrigger.READY_REQUEST:
    if self.lanelet_map is None:
        self.send_trigger(SystemTrigger.MAP_REQUEST, 'planner')
    else:
        for vehicle_id in self.vehicle_ids:
            self.send_trigger(SystemTrigger.READY, f'planner_{vehicle_id}')

The source_id must be planner_<vehicle_id>. The bridge ignores triggers whose source_id does not start with planner.

Planning and commands. For every vehicle the planner builds a path along the lane center line and sends it as PATH_TRACKING command:

stamp = Time.from_msg(message.stamp)
valid_after = stamp + Duration(nanoseconds=self.command_delay_ms * 1_000_000)

command = VehicleCommand()
command.vehicle_id = state.vehicle_id
command.type = VehicleCommand.PATH_TRACKING
command.header.stamp = stamp.to_msg()
command.header.frame_id = 'map'
command.valid_after_stamp = valid_after.to_msg()
command.speed = self.speed
# one PathPoint per point with position, orientation and distance from the first point

Three details matter for every planner:

  • Use the time of the vehicle states (message.stamp) as reference, not the current time of your computer. Then your commands are synchronized with the lab even if planning takes a while.

  • Set valid_after_stamp slightly in the future. The vehicle applies the command at that time, and the evaluation uses it to know which command was active. A command with an unset valid_after_stamp cannot be evaluated correctly.

  • Send a path that starts at the vehicle and reaches well beyond the distance it drives in one period. A PATH_TRACKING path needs at least four points and must continue for at least two points beyond the vehicle, see Interface Reference.

4. Make It Your Own

Typical next steps, from easy to hard:

  • Tune the parameters: change speed and path_length in config/default.yaml.

  • Routing: replace following[0] in plan_path with a route to a goal, e.g. with routing_graph.shortestPath, see Lanelet2 Maps.

  • Speed limits: use the speed limit of the current lanelet instead of a constant speed.

  • Interaction: use the states of the other vehicles in message.state_list to keep a safe distance or to handle intersections.

  • Trajectories: send TRAJECTORY commands with a time for every point to control exactly when a vehicle is where, see Interface Reference.

Keep the planning logic separate from the ROS code, as path_utils.py does. Then you can test it with plain pytest without ROS:

cd ~/planner_ws
colcon test --packages-select my_planner
colcon test-result --verbose

5. Record and Evaluate

Enable Record experiment when you deploy, then evaluate the run with cpm_eval, see Recording and Evaluating Experiments. command_follow shows how well the vehicles followed your commands, and vehicle_collision, boundary_violation and lanelet_collision show whether your planner kept them safe.

When everything works in simulation, continue with Experiments with Real Vehicles.

Tip

For a C++ planner, use cpm_lab_path_tracking as template. It implements the same handshake and command in a composable C++ node, but follows a fixed path instead of the map.