Yield Plugin

A CARMA Platform tactical plugin that modifies an existing trajectory to safely yield to obstacles and cooperating vehicles. It operates as a ROS 2 lifecycle node (YieldPluginNode) wrapping the worker class (YieldPlugin).

Role in the Stack

The yield plugin sits downstream of tactical plugins (e.g., inlanecruising_plugin, cooperative_lanechange). It receives a fully formed trajectory from the planner and either passes it through unchanged or replaces the timestamps to slow/stop the vehicle before a predicted collision.

Upstream planner  -->  [plan_trajectory service]  -->  YieldPlugin  -->  Controller
                                                          ^
                   external_object_predictions  ----------+
                   incoming_mobility_request    ----------+

Two Operating Modes

1. Object Avoidance (default)

Runs whenever the ego trajectory needs to be checked against external objects from /external_object_predictions (populated from onboard sensors and/or V2X msgs such as SDSM). This is the default path: it is skipped only when enable_cooperative_behavior is true (see config/parameters.yaml:76) and the urgency of the latest MobilityRequest (clc_urgency_, set in mobilityrequest_cb, see yield_plugin.cpp:185) exceeds acceptable_urgency (see config/parameters.yaml:85).

Note that the object avoidance is not a handoff to a different tactical plugin. Yield_plugin’s update_traj_for_object logic re-times the trajectory already produced by the upstream tactical plugin (see “Role in the Stack”).

Pipeline:

1. Concurrent collision detection — get_collision_times_concurrently() checks every external object received on /external_object_predictions for a predicted collision with the ego trajectory, using either a CPU or GPU approach. The GPU approach is used by default, falling back to CPU if a CUDA error occurs (e.g., GPU driver not accessible). See “Collision Detection: CPU vs. CUDA” for more.

2. Earliest collision selection — of the objects with a predicted collision, the one with the closest time to collision is chosen as the adversary.

3. Goal velocity — the ego’s target speed is set to the obstacle’s velocity projected along the ego trajectory direction at the collision time (get_predicted_velocity_at_time). Stationary obstacles produce a goal velocity of 0.

4. Safety gap — the stopping distance ahead of the obstacle is:

   gap_time   = max(0, object_downtrack_lead - minimum_safety_gap_in_meters) / initial_velocity
   safety_gap = max(goal_velocity * gap_time, minimum_safety_gap_in_meters)
   

where object_downtrack_lead is the obstacle’s downtrack distance ahead of the ego vehicle. If enable_adjustable_gap is true, any DigitalMinimumGap regulatory element found along the trajectory overrides this value upward.

1. JMT generation — a Jerk Minimizing Trajectory is computed (see below) to reach goal_pos = object_lead - safety_gap - vehicle_length at goal_velocity.

2. Commit-to-stop — if the goal velocity is effectively zero and the collision is within time_horizon_until_collision_to_commit_to_stop_in_s seconds, the resulting trajectory is saved as last_traj_plan_committed_to_stopping_. On subsequent planning calls, while the vehicle is still moving, this saved trajectory is re-used (trimmed to the nearest point to the current vehicle position) instead of re-planning, preventing oscillation.

2. Cooperative Behavior (V2X Cooperative Lane Change)

NOTE: This logic has not been tested since around 2021.

Enabled via enable_cooperative_behavior: true. Activated when a MobilityRequest with strategy carma/cooperative-lane-change and urgency above acceptable_urgency is received and is not stale (within acceptable_passed_timesteps planning cycles).

Mobility request handling (mobilityrequest_cb):

1. Validates that the request targets a lanechange maneuver (CHANGE_LANE_LEFT or CHANGE_LANE_RIGHT).
2. Converts the requester’s ECEF trajectory (encoded as a base location + cumulative offsets in cm) to map-frame 2D points via convert_eceftrajectory_to_mappoints.
3. Parses strategy_params JSON for the requester’s target speed (s.f as integer + fractional tenths), start lanelet, and end lanelet.
4. Stores the request and calls set_incoming_request_info.
5. Rejects the request if the available planning window (expiration - now) is less than min_obj_avoidance_plan_time_in_s, or if the previous call to update_traj_for_cooperative_behavior determined the request was geometrically unacceptable.
6. Publishes a MobilityResponse and a LaneChangeStatus. The response’s is_accepted flag is only ever true if both the geometric/feasibility check above passed and always_accept_mobility_request is true (default false); otherwise the request is reported as rejected regardless of feasibility.

Trajectory update (update_traj_for_cooperative_behavior):

1. detect_trajectories_intersection finds points in the requester’s trajectory within intervehicle_collision_distance_in_m of the ego’s trajectory (using Boost.Geometry distance).
2. If an intersection exists, the goal position is set to distance_to_intersection - max(minimum_safety_gap, digital_gap), and the planning time is taken from the request expiration window.
3. A feasibility check compares the required planning time against delta_v / yield_max_deceleration_in_ms2. If infeasible, the request is rejected and the original trajectory is returned.
4. If feasible, generate_JMT_trajectory is called and cooperative_request_acceptable_ is set true.
5. If trajectories do not intersect, the ego can accept without modifying its own path.

Collision Detection: CPU vs. CUDA

The plugin supports two back-ends for get_collision_times_concurrently. At startup, cuda_is_available() checks for a reachable CUDA device; the GPU path is used when one is found and falls back to CPU on any runtime error.

get_collision_times_concurrently()
        │
        ├─ cuda_is_available()? ──Yes──> get_collision_times_concurrently_cuda()
        │                                        │
        │                       on-route filter (CPU, strided, polygon within)
        │                                        │
        │                       cuda_check_all_collisions()  ← single GPU kernel launch
        │                                        │
        │                       post-process: behind-vehicle check (CPU)
        │
        └─ No / exception ──> get_collision_times_concurrently_cpu()
                                       │
                               one std::thread per object
                               get_collision_time() per object (strided CPU loop)

CPU path

How it works:

• One OS thread is spawned per external object via std::thread + std::packaged_task.
• Each thread runs get_collision_time(), which:
  1. Prepends the object’s current pose to its prediction list.
  2. Runs a strided on-route check using wm_->getLaneletsFromPoint() to confirm at least one predicted state falls on a route lanelet.

File truncated at 100 lines see the full file