Implementation:CARLA simulator Carla MotionPlanStage

Knowledge Sources	CARLA
Domains	Traffic_Management, Autonomous_Driving
Last Updated	2026-02-15 05:00 GMT

Overview

The MotionPlanStage class computes final vehicle control commands (throttle, brake, steering) or teleportation transforms for each traffic-managed vehicle, integrating data from localization, collision avoidance, and traffic light stages.

Description

MotionPlanStage is the final computational stage in the Traffic Manager pipeline before commands are applied to vehicles. It inherits from Stage and implements Update(const unsigned long index). The stage performs:

Target Velocity Computation: Calculates the maximum target velocity for each vehicle based on the speed limit, user-configured speed difference percentage or exact desired speed, landmark-based speed reduction (GetLandmarkTargetVelocity), and turn-based speed reduction (GetTurnTargetVelocity using three-point circle radius estimation).
Collision Response: The CollisionHandling method processes collision hazard data and traffic light hazard flags to determine if an emergency stop is needed and to compute a dynamically adjusted target velocity. When a collision hazard is detected, velocity is reduced proportionally to the available distance margin.
Junction Safety: The SafeAfterJunction method checks whether there is sufficient free space after a junction before allowing a vehicle to enter, preventing vehicles from blocking intersections.
PID Control: When physics is enabled, applies separate PID controllers for longitudinal (speed) and lateral (steering) control. Different PID parameter sets are used for urban and highway driving conditions based on vehicle speed relative to HIGHWAY_SPEED.
Hybrid Physics Mode (Teleportation): When a vehicle has physics disabled (hybrid mode), computes a teleportation transform by interpolating along the waypoint buffer at a distance proportional to the target velocity and time step (HYBRID_MODE_DT). The vehicle is moved via ApplyTransform commands instead of physics-based actuation.
Dormant Vehicle Respawning: When a vehicle is dormant and RespawnDormantVehicles is enabled, the stage attempts to teleport the vehicle to a free waypoint near the hero vehicle within configurable distance bounds.
State Persistence: Maintains a pid_state_map to store PID controller state (previous deviation, integral) across ticks, and a teleportation_instance map tracking timestamps for hybrid mode teleportation timing.

Usage

MotionPlanStage is created by TrafficManagerLocal and runs as the last computational stage per tick. Its output is a ControlFrame containing either VehicleControl commands or ApplyTransform commands for each vehicle, which are then applied via batch commands to the CARLA server.

Code Reference

Source Location

Repository: CARLA
File: LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp
Header: LibCarla/source/carla/trafficmanager/MotionPlanStage.h

Signature

class MotionPlanStage : Stage {
public:
  MotionPlanStage(const std::vector<ActorId> &vehicle_id_list,
                  SimulationState &simulation_state,
                  const Parameters &parameters,
                  const BufferMap &buffer_map,
                  TrackTraffic &track_traffic,
                  const std::vector<float> &urban_longitudinal_parameters,
                  const std::vector<float> &highway_longitudinal_parameters,
                  const std::vector<float> &urban_lateral_parameters,
                  const std::vector<float> &highway_lateral_parameters,
                  const LocalizationFrame &localization_frame,
                  const CollisionFrame &collision_frame,
                  const TLFrame &tl_frame,
                  const cc::World &world,
                  ControlFrame &output_array,
                  RandomGenerator &random_device,
                  const LocalMapPtr &local_map);

  void Update(const unsigned long index) override;
  void RemoveActor(const ActorId actor_id);
  void Reset();
};

Import

#include "carla/trafficmanager/MotionPlanStage.h"

I/O Contract

Inputs

Name	Type	Required	Description
vehicle_id_list	const std::vector<ActorId>&	Yes	List of registered vehicle actor IDs
simulation_state	SimulationState&	Yes	Cached kinematic state for all actors (also updated for teleported vehicles)
parameters	const Parameters&	Yes	Per-vehicle speed, physics, and respawn configuration
buffer_map	const BufferMap&	Yes	Waypoint buffers from LocalizationStage
track_traffic	TrackTraffic&	Yes	Path overlap tracking for junction safety checks and hero location
urban_longitudinal_parameters	const std::vector<float>&	Yes	PID gains [Kp, Ki, Kd] for urban longitudinal control
highway_longitudinal_parameters	const std::vector<float>&	Yes	PID gains [Kp, Ki, Kd] for highway longitudinal control
urban_lateral_parameters	const std::vector<float>&	Yes	PID gains [Kp, Ki, Kd] for urban lateral control
highway_lateral_parameters	const std::vector<float>&	Yes	PID gains [Kp, Ki, Kd] for highway lateral control
localization_frame	const LocalizationFrame&	Yes	Output from LocalizationStage with target waypoints and flags
collision_frame	const CollisionFrame&	Yes	Output from CollisionStage with hazard data
tl_frame	const TLFrame&	Yes	Output from TrafficLightStage with traffic light hazard flags
world	const cc::World&	Yes	CARLA world for timestamp retrieval
random_device	RandomGenerator&	Yes	Random number generator for respawn distance sampling
local_map	const LocalMapPtr&	Yes	In-memory map for respawn waypoint queries

Outputs

Name	Type	Description
output_array	ControlFrame&	Array of control commands: either carla::rpc::Command::ApplyVehicleControl or carla::rpc::Command::ApplyTransform per vehicle

Usage Examples

// Created inside TrafficManagerLocal:
motion_plan_stage(MotionPlanStage(vehicle_id_list,
                                  simulation_state,
                                  parameters,
                                  buffer_map,
                                  track_traffic,
                                  longitudinal_PID_parameters,
                                  longitudinal_highway_PID_parameters,
                                  lateral_PID_parameters,
                                  lateral_highway_PID_parameters,
                                  localization_frame,
                                  collision_frame,
                                  tl_frame,
                                  world,
                                  control_frame,
                                  random_device,
                                  local_map));

// Called per vehicle in the parallel update loop:
motion_plan_stage.Update(vehicle_index);

// Control commands applied to CARLA server:
episode_proxy.Lock()->ApplyBatchSync(control_frame);

Related Pages

Page Connections

Double-click a node to navigate. Hold to expand connections.

Principle

Implementation

Heuristic

Environment