Design: Control Flow

The control flow (package cpm_lab_control_flow, node cpm_lab_control_flow_node) runs in the lab domain and owns every process of an experiment. The Control Center in RViz and the CPM control dashboard in Lichtblick are only user interfaces: they call its services and display its topics. Anything they can do can also be done with ros2 service call, see Running the Lab.

Source: src/bridge/cpm_lab_control_flow/component/ — control_flow_node.cpp (ROS interface and state machine), process_manager.cpp (starting and stopping processes), ros_process.cpp (one ros2 run/ros2 launch process), control_flow_utils.cpp (discovery of planners, maps and IPS configurations).

Interface

Name

Type

Notes

/lab/control_flow/map_server

service MapServer

start (map file name) or stop the map server

/lab/control_flow/ips

service Ips

start (configuration file name) or stop the IPS

/lab/control_flow/simulation

service Simulation

start or stop a simulated vehicle

/lab/control_flow/deploy

service Deploy

deploy (planner, period, recording) or abort

/lab/control_flow/experiment

service Experiment

start or stop the deployed experiment

/lab/control_flow/catalog

ControlCatalog, transient local

available planners, maps, IPS configurations and warnings

/lab/control_flow/state

ControlState, transient local

deploy state, running processes, online vehicles, recording state

/lab/control_flow/status

ControlStatus

event log shown in the user interface

/lab/system_trigger

SystemTrigger

publishes START/STOP (source_id lab_control_flow), receives READY of the bridge and external STOP

The services and messages are in cpm_lab_control_flow_msgs. Parameters (config/default.yaml): vehicle_ids (vehicles it listens to, 1 to 20), vehicle_timeout_seconds (1.0; a vehicle is offline after this time without state), timeout_check_period_ms (500), planner_domain_id (1), recording_root, recording_timeout_ms (10000) and recording_heartbeat_timeout_ms (1500).

Catalog

The catalog lists what can be started:

  • Planners: every package with an executable whose name contains planner, a *planner*launch.py or a *.launch.xml that declares the argument vehicle_ids. Packages under /opt only count if their name starts with cpm_lab; message packages are skipped.

  • Maps: the *.osm files of cpm_lab_maps/maps/ and of $CPM_LAB_HOME/maps/.

  • IPS configurations: the *.yaml files of cpm_lab_indoor_positioning_system/config/ and of $CPM_LAB_HOME/ips/.

A file in $CPM_LAB_HOME replaces a packaged file of the same name. The services only accept plain file names, which are resolved to these directories. The catalog also carries the warnings for a ROS_DOMAIN_ID other than 21 and for FastDDS.

Deploy State Machine

        ---
config:
  layout: elk
  startOnLoad: false

---
stateDiagram-v2
   [*] --> IDLE
   IDLE --> WAITING: Deploy (vehicles online)
   WAITING --> READY: bridge READY and recorder ARMED
   READY --> RUNNING: Start (recorder acknowledged Begin)
   WAITING --> IDLE: Abort or STOP
   READY --> IDLE: Abort or STOP
   RUNNING --> IDLE: Stop, external STOP or failure

Deploy states of the control flow

Deploy takes all vehicles that are online at this moment. It starts the recorder (if Record experiment is set), the bridge in the planner domain with these vehicle IDs and the period, and the internal planner, if one is selected. If any start fails, the processes started so far are stopped again and the state stays IDLE.

Ready: the bridge sends READY once the planner reported ready for every vehicle and every vehicle sent a state (see Design: Bridge). With recording, the recorder must also be ARMED. The flowchart in Deploy Checks shows these checks from the user’s point of view.

Start publishes START on /lab/system_trigger. With recording, the recorder must acknowledge Begin first.

Stop publishes STOP first and then tears down: the recording is finished (or cancelled if driving never started or the stop was not a normal stop), the bridge and the planner are stopped, and the state returns to IDLE. The reason is stored and shown in the user interface. The map server, IPS and simulated vehicles keep running for the next experiment.

The control flow stops the experiment itself when

  • a STOP from another source arrives (external_stop), e.g. the emergency stop of the bridge,

  • the recorder reports FAILED, its process exits or its heartbeat times out,

  • the recorder rejects Prepare or Begin.

Processes

Each component is a child process. RosProcess sets ROS_DOMAIN_ID per process (the bridge and the planner get the planner domain), passes vehicle_ids and the period as parameters or launch arguments and forwards the output to the terminal of the control flow. A process that does not start within 4 s is an error. Stopping sends SIGINT to launch files and SIGTERM to executables and kills the process if it has not exited after 2 s. Stopping the control flow (Ctrl+C) stops all child processes.

Simulated vehicles are mid_level_controller_node processes in simulation mode. If the map defines a spawn point for the vehicle ID (a line string with the attribute spawn_point), the vehicle starts there, see Lanelet2 Maps.

Changing the Control Flow

  • New user-facing actions need a service or field in cpm_lab_control_flow_msgs, the RViz Control Center and the Lichtblick extension. Follow Changing Messages and Services.

  • Keep the logic testable without ROS in control_flow_utils; test/ contains the parameter and file discovery tests.

  • A new process belongs in ProcessManager; stop it in the destructor and in stopExperiment if it is part of an experiment.