External Motion Planner

This guide shows how to run a motion planner yourself, in a terminal of your own, instead of letting the control flow start it. This is how you run your own planner during development and in the lab. As an example, it starts the SFB planner that is installed with the CPM Lab (Install from Debian Packages).

Note

Prerequisites: have ROS2 sourced (source /opt/ros/jazzy/setup.bash, see Supported Platforms), and the CPM Lab packages installed.

1. Launch the CPM Lab

Launch the control flow together with RViz and the Control Center. The control flow starts all components you select in the Control Center, see Running the Lab.

source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=21
ros2 launch cpm_lab cpm_lab_rviz.launch.xml

Tip

You can use Lichtblick instead of RViz, see Visualization with Lichtblick. The CPM control dashboard panel offers the same steps as the Control Center.

2. Configure the Control Center

When you start the Control Center you may be presented with a few configuration screens.

Note

The warning about FastDDS is expected if you have not switched to another RMW implementation. Use CycloneDDS or Zenoh for simulations and Zenoh for the real lab, see Middleware; then the warning does not appear.

Note

If you installed the indoor positioning package the Control Center may prompt to start it. For simulations this is optional and can be skipped.

Next, select the map to use for the simulation (maps starting with “Lab” are provided). In this guide we chose “Lab Outer Circle”. Click Start to launch the map server.

Warning

If you run FastDDS as the RMW implementation the map might not appear in RViz due to known QoS incompatibilities. The simulation and planners still work; the missing map is a display issue.

If you are using CycloneDDS or Zenoh you should see the map in RViz immediately.

After the map server is running, start one or more vehicle simulations. Select the vehicle(s) in the Control Center selection screen and click Start.

Start a motion planner next. Uncheck “Use internal ROS2 HLC” and simply click “Next” to proceed to the experiment setup screen.

Open a second terminal and start your external motion planner in the planner domain, ROS_DOMAIN_ID=1, with the same middleware as the lab:

source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=1
ros2 launch cpm_lab_sfb_py default.launch.xml vehicle_ids:="[1,2,3]" period:=100

Note

The example launches the SFB motion planner for the vehicles 1, 2 and 3 with a period of 100 ms. The period must be the same as the update period you choose in the experiment setup screen below, and vehicle_ids must contain all deployed vehicles. Your own motion planner follows the interface described in Understanding the Motion Planner.

Tip

You can start the planner before or after Deploy: after Deploy, the bridge asks it every period whether it is ready, see the deploy checks in Running the Lab. This guide starts it first.

In the experiment setup screen you can set the motion planner update rate (ms) and start the bridge by clicking “Deploy”. The bridge will query each component (vehicles and planner) for readiness; once all components report ready the Control Center enables the “Start” button. If you use FastDDS you may then also see the map in RViz. You should see the motion planner communicate with the Control Center in the terminal output.

Click “Start” to begin the simulation. Vehicles should move in RViz according to the motion planner commands.

Click “Stop” to stop the simulation. You should see the motion planner stopping as well. The map server and the simulated vehicles keep running, so you can deploy the next experiment right away. Press Ctrl+C in the terminal of the launch file to stop the CPM Lab and all components started by the control flow. This does not stop the external motion planner you started in the second terminal; stop it with Ctrl+C in its terminal.

Middleware

This guide works with CycloneDDS or Zenoh. For experiments with real vehicles, Zenoh is required; see Middleware for how to install it and start its router.