feat: add virtual traffic light modifier plugin by shanpeng-m · Pull Request #3553 · tier4/autoware_universe

shanpeng-m · 2026-10-02T05:57:24Z

Summary

Add a VirtualTrafficLightStop plugin to stop candidate trajectories at map-defined stop or end lines based on virtual traffic light approval and finalization states.

Support approval checks, state timeouts, dead-line handling, and end-line holding while waiting for finalization.
Update coordination states and module lifecycle once per candidate batch while evaluating each trajectory independently.
Keep state subscriptions within the plugin and publish infrastructure commands once after the last candidate.
Preserve existing stop constraints, apply shared braking parameters, and handle short trajectories using vehicle-front geometry.
Add plugin registration, parameters, planning factors, debug outputs, and unit/integration coverage.

Design Doc

https://tier4.atlassian.net/wiki/spaces/CB/pages/5534744673/Virtual+Traffic+Light+Stop+Design

Request Jira Ticket

https://tier4.atlassian.net/browse/L4A-2306

…e2e-virtual-traffic-light-stop

mergify · 2026-10-14T00:59:11Z

Tick the box to add this pull request to the merge queue (same as @mergifyio queue).

Queue this pull request

sonarqubecloud · 2026-10-14T01:18:04Z

Quality Gate passed

Issues
46 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
2.4% Duplication on New Code

See analysis details on SonarQube Cloud

Kotakku · 2026-10-15T01:24:54Z

  <arg name="input_tracked_objects" default="/perception/object_recognition/tracking/objects"/>
  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
  <arg name="input_route" default="/planning/mission_planning/route"/>
+  <arg name="input_virtual_traffic_light_states" default="/system/v2x/virtual_traffic_light_states"/>


(question) The input default /system/v2x/virtual_traffic_light_states is the raw topic before awapi_awiv_adapter, while the behavior velocity planner subscribes to /awapi/tmp/virtual_traffic_light_states, which carries the aggregated states (merged per instrument, stale ones dropped) as well as the states from /api/external/set/virtual_traffic_light/states. Which topic did you publish the states to in your psim check? Since update_module_states clears a module whose id is not in the latest array, I think the raw topic can drop approvals when instruments are published separately, and the states via the external API would not arrive. If there is no specific reason for /system/v2x/..., could you align the default with behavior_planning.launch.xml?

Kotakku · 2026-10-15T01:24:54Z

    <remap from="~/input/tracked_objects" to="$(var input_tracked_objects)"/>
    <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>
    <remap from="~/input/route" to="$(var input_route)"/>
+    <remap from="~/input/virtual_traffic_light_states" to="$(var input_virtual_traffic_light_states)"/>


(question) As far as I can see, ~/output/infrastructure_commands is not remapped, so I think the commands would not reach the API relay or dummy_infrastructure (both subscribe to /planning/scenario_planning/status/infrastructure_commands). How did you check this in psim? If the states were published manually, it would work without the commands being received, so I may be missing something. If the remap is indeed missing, could you add an output_infrastructure_commands arg and remap like the other output_* topics here?

Kotakku · 2026-10-15T01:24:54Z

+    const bool externally_finalized =
+      module.virtual_traffic_light_state && module.virtual_traffic_light_state->is_finalized;
+    if (
+      !externally_finalized && std::abs(*end_arc) < planner_param_.near_line_distance && stopped) {
+      set_state(module, ModuleState::FINALIZING, module.active_end_line->id);
+    } else {
+      set_state(module, ModuleState::PASSING);
+    }


Once FINALIZING is entered, the else branch sets PASSING again as soon as is_finalized becomes true (the normal reply to FINALIZING) or the ego speed goes slightly over ego_stopped_vel_th, so the command goes FINALIZING → PASSING → FINALIZED. In the original module (scene.cpp) FINALIZING is never moved back to PASSING. dummy_infrastructure does not distinguish these states, so this does not show up in psim, but real infrastructure that uses FINALIZING as a trigger may be affected. Could you keep FINALIZING once entered (e.g. also take this branch when module.state == ModuleState::FINALIZING), and add a test for "FINALIZING → finalized=true → still FINALIZING"?

Kotakku · 2026-10-15T01:24:54Z

+  return *collision - ego_s - front_offset;
+}
+
+std::optional<double> calc_arc_length_on_centerline(


calc_arc_length_on_centerline clamps the projection to the lane endpoints, so when ego is upstream of the module lane, ego's arc length becomes 0. If the start line is on a preceding lanelet (likely, since the map format requires the start line to be at least $v_0^2 / 2|a_{min}|$ before the stop line), its arc length is also 0, so start_arc = -front_offset < 0 in update_module_lifecycle and REQUESTING is sent from anywhere upstream on the route. The original module only starts requesting after the path reaches the stop line and the start line is crossed. The test map has only one lanelet, so this case is not covered.

Could you gate the lifecycle with the trajectory crossing (as process_module does), or use an arc length over the route lanelets (e.g. lanelet::utils::getArcCoordinates) instead of the clamped projection? A test with the start line on a preceding lanelet would also help.

Kotakku · 2026-10-15T01:24:54Z

+    "VirtualTrafficLightStop::prepare_cycle", *get_time_keeper());
+  cycle_initialized_ = true;
+  cycle_time_ = get_clock()->now();
+  cycle_odometry_ = input.current_odometry


nit: current_odometry / current_acceleration are guaranteed non-null by TrajectoryModifier::make_input_data(), so the optional copy and the null checks (here, L251-253) are not needed. Keeping the ConstSharedPtr is enough to freeze them for the batch.

Kotakku · 2026-10-15T01:24:54Z

+    module.debug_data.message_age_sec =
+      (cycle_time_ - rclcpp::Time(module.virtual_traffic_light_state->stamp)).seconds();
+    module.debug_data.timeout = *module.debug_data.message_age_sec > planner_param_.max_delay_sec;


nit: This duplicates is_state_timeout, and debug.timeout is overwritten at L773 anyway. Could you keep only one?

Kotakku · 2026-10-15T01:24:54Z

    use_stop_point_fixer: true
    use_obstacle_stop: true
    use_traffic_light_stop: true
+    use_virtual_traffic_light_stop: true


(question) This is true here while the struct/schema default is false (and use_detection_area_stop is false). Without a VTL state publisher (e.g. psim), every VTL on the route stops with NO_STATE. Is enabling it by default intended?

Kotakku · 2026-10-15T01:24:54Z

+};
+}  // namespace autoware::trajectory_modifier::plugin
+
+#endif  // VIRTUAL_TRAFFIC_LIGHT_STOP_HPP_


nit: The closing comment should match the guard: AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__VIRTUAL_TRAFFIC_LIGHT_STOP_HPP_.

shanpeng-m added 5 commits October 2, 2026 14:11

feat: add vtl stop plugin for trajectory modifier

5679df5

fix: msg type

3c6832e

fix: correct VTL stop position and add debug visualization

587e7ee

fix: fix vtl stop

be80365

fix: preserve VTL stop constraints and account for vehicle front

6be10a5

shanpeng-m changed the title ~~Feat/v0.64/e2e virtual traffic light stop~~ feat: add virtual traffic light modifier plugin Oct 2, 2026

shanpeng-m added 6 commits October 5, 2026 17:54

fix: publish VTL commands once per batch

16f962e

fix: update virtual traffic light timing checks

fb97141

Merge remote-tracking branch 'origin/feat/v0.64/e2e' into feat/v0.64/…

3b9df07

…e2e-virtual-traffic-light-stop

refactor: refine virtual traffic light stop cycle logic

02a2ad4

refactor: scope VTL state polling to plugin

fc1524a

Merge remote-tracking branch 'origin/feat/v0.64/e2e' into feat/v0.64/…

c3c71fa

…e2e-virtual-traffic-light-stop

shanpeng-m marked this pull request as ready for review October 14, 2026 00:58

shanpeng-m requested review from GasutaRobo, Kotakku, Naophis and tkimura4 October 14, 2026 01:00

Kotakku reviewed Oct 15, 2026

View reviewed changes

Navigation Menu

Repository navigation

feat: add virtual traffic light modifier plugin - #3553

feat: add virtual traffic light modifier plugin#3553
shanpeng-m wants to merge 11 commits into
feat/v0.64/e2efrom
feat/v0.64/e2e-virtual-traffic-light-stop

shanpeng-m commented Oct 2, 2026 •

edited

Loading

Uh oh!

mergify Bot commented Oct 14, 2026

Uh oh!

sonarqubecloud Bot commented Oct 14, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

Kotakku Oct 15, 2026

Uh oh!

This branch has not been deployed

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

shanpeng-m commented Oct 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Design Doc

Request Jira Ticket

Uh oh!

mergify Bot commented Oct 14, 2026

Uh oh!

sonarqubecloud Bot commented Oct 14, 2026

Quality Gate passed

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

This branch has not been deployed

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

shanpeng-m commented Oct 2, 2026 •

edited

Loading