Interface Reference

This page describes the messages, topics, coordinate frames, units and timing a motion planner works with. See Your First Motion Planner for a complete example and Vehicle Limits and Simulation for what the vehicles can physically do.

Frames and Units

All positions, speeds and angles are SI units at lab scale: meters, meters per second and radians. One lab meter corresponds to 18 meters of the real-world map, see Lanelet2 Maps.
All poses are in the map frame. Its origin is a corner of the lab track; x and y span about 4.5 m x 4.0 m on the lab maps. The vehicle controller ignores commands whose header.frame_id is not map.
The yaw angle is measured counter-clockwise from the x axis. Messages carry it as quaternion; for a planar pose z = sin(yaw / 2) and w = cos(yaw / 2).
The pose of a vehicle is the center of the vehicle.
Times are ROS times (builtin_interfaces/Time) of the lab clock. The vehicles synchronize their clocks with the central workstation.

Warning

For a life-size display in RViz and Lichtblick, the TF frames vehicle_<id> and the map markers are scaled by 18. The messages themselves are always in lab meters. Do not use TF to transform lab data; use the poses in the messages directly.

Topics of a Motion Planner

The planner runs in ROS_DOMAIN_ID=1. All topics are in the /planner namespace:

Topic	Type	Description
`/planner/vehicle_states`	`cpm_lab_lab_msgs/VehicleStateList`	States of all deployed vehicles, every planner period while the experiment runs.
`/planner/map`	`cpm_lab_map_msgs/Lanelet2MapBin`	The map, published once with transient local durability.
`/planner/system_trigger`	`cpm_lab_lab_msgs/SystemTrigger`	Coordination between bridge and planner.
`/planner/command`	`cpm_lab_lab_msgs/VehicleCommand`	Commands of the planner, one per vehicle and period. Use a reliable publisher.

The lab domain (ROS_DOMAIN_ID=21) contains the same data per vehicle, for example to observe an experiment: /lab/vehicle_<id>/vehicleState, /lab/vehicle_<id>/vehicleObservation, /lab/vehicle_<id>/command, /lab/map and /lab/system_trigger.

Messages

VehicleStateList

Field	Description
`stamp`	Lab time when the bridge sent the list. Use it as the reference time of your planning step.
`state_list`	One `VehicleState` per deployed vehicle.

VehicleState

The current state of a vehicle, estimated by its vehicle controller from the IPS, the odometer and the IMU.

Field	Description
`vehicle_id`	ID of the vehicle, 1 to 20.
`valid_after_stamp`	Time at which the state was valid.
`pose`	Position and orientation in the `map` frame, z is always 0.
`twist.linear.x`	Speed in m/s, positive when driving forward.
`twist.angular.z`	Yaw rate measured by the IMU.
`accel.linear`	IMU acceleration forward (x), left (y) and up (z).
`ips_update_age`	Time since the last IPS observation, stored as duration. The vehicle stops path and trajectory commands if it exceeds 3 s.
`odometer_distance`	Driven distance in m.
`battery_voltage`, `motor_current`	Battery voltage in V and the measured motor current.
`motor_throttle`, `steering_servo`	Actuator outputs of the current control cycle, see `DIRECT` below.

VehicleCommand

The command for one vehicle. The type selects how the vehicle controller follows it, see Understanding the Vehicle Controller.

Field	Description
`vehicle_id`	ID of the vehicle the command is for. Must be set.
`type`	`DIRECT` (1), `PATH_TRACKING` (2), `SPEED_CURVATURE` (3) or `TRAJECTORY` (4). `INVALID` (0) is ignored.
`header`	`frame_id` must be `map`; `stamp` is the creation time.
`valid_after_stamp`	Time from which the command is applied, see Timing.
`motor_throttle`, `steering_servo`	`DIRECT` only, see below.
`speed`	Target speed in m/s for `SPEED_CURVATURE` and `PATH_TRACKING`.
`curvature`	Target curvature in 1/m for `SPEED_CURVATURE`, positive to the left.
`path`	List of `PathPoint` for `PATH_TRACKING` and `TRAJECTORY`.

PathPoint

Field	Description
`pose`	Position and heading of the point. Always required.
`distance`	`PATH_TRACKING` only: distance from the first point along the path in m. Must strictly increase.
`time`	`TRAJECTORY` only: time at which the vehicle should be at this point.
`speed`	`TRAJECTORY` only: speed at this point in m/s.

Command Types

PATH_TRACKING

The vehicle follows a geometric path at the constant speed speed. The controller interpolates the path with cubic Hermite splines over distance and steers with a Stanley controller. Requirements:

at least 4 points, with strictly increasing distance,
the path must continue for at least two points beyond the point closest to the vehicle, so send a path that reaches well beyond the distance the vehicle drives in one period,
the headings of the points should match the direction of the path.

This is the easiest type to start with.

TRAJECTORY

The vehicle follows a path in space and time. Every point has a time and a speed; the controller interpolates between the points over time and tracks the position with a model predictive controller. Requirements, checked every 20 ms:

at least 2 points, sorted by time,
the first point must lie before now + 0.11 s,
the last point must lie at least 0.36 s in the future.

If a requirement is violated or the controller cannot follow the trajectory, it logs a warning and the vehicle rolls out without throttle. Send trajectories that reach several periods into the future so a delayed command does not stop the vehicle.

SPEED_CURVATURE

The vehicle drives with the given speed and curvature. Useful for simple reactive controllers. Curvatures above about 4.1 1/m saturate the steering.

DIRECT

Sets the actuators directly: motor_throttle in [-1, 1] (positive forward; values within ±0.05 brake on the real vehicle) and steering_servo in [-1, 1] (positive to the left). There is no speed control and no safety check besides the stop mechanisms. Used by manual control.

SystemTrigger

Type	Meaning
`START` (0)	The experiment starts. Sent by the control flow and forwarded by the bridge.
`STOP` (1)	Stop all vehicles. Sent by the control flow or the bridge; a planner may send it on a fatal error.
`READY` (2)	A component is ready. A planner sends one per vehicle with `source_id` `planner_<vehicle_id>`.
`MAP_REQUEST` (10)	Ask the map server to publish the map again. `source_id` must start with `planner`.
`READY_REQUEST` (20)	The bridge asks all components whether they are ready. Sent every period until ready.
`LED_REQUEST` (30)	Used by the IPS to control the vehicle LEDs.

The bridge only forwards triggers whose source_id starts with planner from the planner domain and with lab from the lab domain.

Timing

The vehicle controllers run every 20 ms. The IPS measures the vehicles at 50 Hz.
The bridge sends the vehicle states every planner period (default 400 ms, chosen when deploying). Your planner should answer within one period.
A command is applied from its valid_after_stamp on. Each vehicle keeps a queue of received commands and, every 20 ms, applies the newest command whose valid_after_stamp has passed. Set it to the stamp of the state list plus a small lead time, e.g. 40 to 60 ms, to compensate the network latency. An unset valid_after_stamp (0) applies the command immediately and breaks the evaluation.
A command stays active until it is replaced. There is no timeout for old commands, so a vehicle keeps following its last path or trajectory if your planner stops sending.
After a STOP the vehicle stays in stop mode for at least 1 s and until a new command becomes valid.
The bridge stops the experiment if the state of a deployed vehicle is older than 1 s.