feat(si): supervised single output with startup mode selection and native VisionPilot ingress (#66)

* feat(si): supervised single output with per-source watchdogs and VP_CONTROL

Implement the v0.1 SI supervisor of the vp_si_control_contract
(#63/#64/#65):

- All actuator-facing output is produced by one gate:
publishApprovedRequest() writes /control/safety_island/approved_request
(safety_island_msgs/ApprovedRequest, new IDL: SI session, strictly
increasing output_sequence, decision NORMAL/SI_STOP/HOLD, configured
mode, selected source, VP cycle identity, fault_id) and post-gates the
legacy control_cmd topic and the CAN output with the approved payload;
the follower output and the VP command are candidates only.
- The any-input staleness gate is replaced: per-source arrival checks
(candidate 1.0 s, ego 0.4 s, steering/opmode 0.5 s, SI arrival clock,
never message stamps) latch SI_STOP; the explicit stop carries the last
known steering and a signed negative acceleration demand and publishes
every cycle; normal driving resumes only on an explicit re-enable topic
(std_msgs/Bool /control/safety_island/reenable) when all sources are
fresh again.
- VP_CONTROL (build-time --supervision-mode vp): accept the checked
DrivingCommand (valid, source-stamped, finite, in actuation bounds,
strictly increasing cycle; regression/reorder/corrupt latches) and pass
it through without recomputing; visionpilot_msgs IDL generated from the
fork .msg files via rosidl_adapter.

* docs(design): vp/si control contract draft with the measured cadence

Aggregate the #62 contract as a design doc: wire layout with the
implemented types, the measured VPS cadence and identity evidence, the
implemented SI supervisor state (attack plan v0.1), and the remaining
decisions (distinct VP candidate ingress, cross-vendor type check,
actuator, per-scenario gates).

* fix(si): volatile op-mode subscription and edge-only latch logging

The operation-mode reader requested TRANSIENT_LOCAL, which is
incompatible with the volatile writer the ros2 domain bridge (0.5.0)
creates on the SI side of the rig: the bridge logged 'requesting
incompatible QoS ... DURABILITY_QOS_POLICY' and never delivered a state,
so the follower never became ready (observed 2026-09-25 on the E2E rig;
first full-loop run with the supervisor). Volatile is compatible and the
publisher republishes at 10 Hz, so a late-joining SI still sees a state
within one period.

Also log the fault edge once instead of once per 150 ms control tick:
the continuing latch state is already reported by latchFault when the
reason changes, and a per-tick warn would flood the UART console for as
long as the fault lasts.

* feat(si): one-binary startup selection and identity-bearing VP candidate ingress

- Replace compile-time CONFIG_SI_SUPERVISION_MODE with a startup-only
  selection: SI_SUPERVISION_MODE (si|vp) and SI_TRAJECTORY_SOURCE
  (autoware|vp) are read once before any DDS endpoint is created; invalid
  or missing combinations exit 1 and never fall back silently
- In SI_CONTROL subscribe only to the selected source: native Autoware
  Trajectory, or safety_island_msgs/TrajectoryCandidate carrying the
  original VP session/cycle plus a standard trajectory
- Validate candidate shape/finiteness and source identity: duplicates and
  repeated camera stamps do not refresh the watchdog, cycle/stamp
  regressions latch SI_STOP, retired VP sessions stay rejected after a
  producer restart
- Keep one follower/output path and the explicit re-enable rule; add the
  TrajectoryCandidate IDL, host tests for the startup/identity logic, and
  update the control contract with the measured status

* fix(si): resume NORMAL on the tick that clears the latch

callbackTimerControl() cleared the latch on a fresh re-enable but stayed in the
latched branch and published one more SI_STOP (fault_id 0, no active fault)
before NORMAL on the next tick — a one-cycle (15 ms) artifact seen in the rig's
re-enable evidence. Move the latch into a dependency-free header with a
stopThisTick() step: a request consumed with every source fresh clears the
latch and the same tick continues with NORMAL supervision; a request that
meets a stale source stays pending. Host test supervision_latch_test pins the
behaviour (it fails on the old clearing tick).

* feat(si): ingest VisionPilot's DrivingReference natively in SI_CONTROL

In SI_CONTROL with the VP source, SI subscribed to a TrajectoryCandidate that
an external E2E adapter built from VP's DrivingReference. SI now reads
/vehicle/driving_reference itself and builds the follower trajectory
(vp_reference.hpp): the base_link lane polynomial is placed with the ego pose
of the same simulator frame (bounded ~10 s odometry history, exact stamp
match), and VP's speed schedule becomes a spatial profile with the adapter's
13-point layout, 1 m speed lead and near-field arcs. On 3000 randomized
references the output equals the retired adapter's to 1e-9.

Identity and freshness are unchanged: session/cycle/source-stamp checks
latch SI_STOP on regression and ignore duplicates, and only an accepted
reference refreshes the selected-source watchdog. References VP marks
unusable, or without a same-frame ego sample, are ignored and logged, never
turned into a stop; malformed values latch. Host test vp_reference_test runs
in the --unit-test build.

* chore(si): drop the retired TrajectoryCandidate IDL

Since e88f78f the SI reads VisionPilot's DrivingReference directly and the
E2E adapter that published TrajectoryCandidate is gone, so nothing sends or
reads that type. Remove the IDL and its message alias.

* fix(si): boot with a build-default mode/source when none is set

Targets without an environment (Zephyr, X5H, bare FreeRTOS) and the CI
smoke run start the SI with neither SI_SUPERVISION_MODE nor
SI_TRAJECTORY_SOURCE set; the SI exited at boot. With both unset it now
uses SI_DEFAULT_SUPERVISION_MODE/SI_DEFAULT_TRAJECTORY_SOURCE (si/autoware,
the upstream behaviour, overridable at compile time) and logs it. A
half-set pair or an invalid value is still fatal.

run-freertos-posix-ci.sh passes on a clean checkout (smoke, unit, DDS
pair, CAN).

* fix(si): make SourceWatch arrival time a lock-free atomic

SourceWatch::note() runs on CycloneDDS callback threads while stale() and
ageSec() run on the controller thread, but the arrival time was a plain
double plus a separate bool. A 64-bit double store is not single-copy-atomic
on ARMv8-R AArch32, so a torn read could fabricate a wild age and latch a
spurious SI_STOP, which only an explicit re-enable clears. InputStalenessGate
already closed this with std::atomic<double>; the new watchdogs regressed it.

Keep the arrival time in one std::atomic<double> (static_assert lock-free),
with -1.0 as the never-seen sentinel so ever() and the age cannot disagree and
every reader takes a single load. Move the class into the dependency-free
supervision_latch.hpp so supervision_latch_test covers it: never-seen is not
stale, the timeout boundary, refresh, and a concurrent note()/ageSec() smoke
test.

* fix(si): HOLD carries the conservative stopped payload, not zeros

publishHold() sent an all-zero ControlMsg. The ApprovedRequest consumer keeps
its previous state on HOLD, but the legacy control_cmd/CAN surface applies the
payload, where zero acceleration is a brake release on every not-ready cycle
(no accepted VP command yet, input data not ready, controllers not ready).

Build the HOLD payload with supervision::StopControl (velocity 0, the stop
deceleration, last known steering) and stamp it, and correct the Decision::HOLD
comment that claimed the previous payload is actuated.

* feat(si): SI_LEGACY_CONTROL_CMD policy for the legacy control_cmd topic

ApprovedRequest is the only authoritative SI output, but the legacy DDS
control_cmd topic still carries every approved payload (NORMAL/HOLD/SI_STOP)
for bridge compatibility. A consumer that lets a fresh SI command win over its
own driver, such as the X5H CES demo's bench arbiter (openadkit#147), would
then hear SI continuously and never let its driver act.

Add a startup-only policy, SI_LEGACY_CONTROL_CMD = always | stop_only, read
with the other startup selection (build default SI_DEFAULT_LEGACY_CONTROL_CMD
= always, so every existing rig is unchanged; an invalid value is fatal).
stop_only publishes only SI_STOP on the legacy DDS topic. It never filters
ApprovedRequest and does not affect CAN output.

Also record in the design note that SI #64 is only partly met: supervision
runs before the follower in the same timer/thread, so a hung follower still
stops the supervisor.

* fix(si): bound the work vp_reference::convert does per reference

convert() ran on the SI control thread and sampled the lane polynomial up to
the x_max VisionPilot reported, then densified every segment to 0.25 m. A
finite but degenerate reference passed every shape check and was unbounded: a
curvature of 1e4 took 3.5 s and 1.5 GB in a host probe, an x_max of 1e6 took a
million samples. On the target that either blocks the only control thread
(no ApprovedRequest, no watchdog) or throws bad_alloc out of the DDS callback
and ends the SI, and in both cases no SI_STOP is published.

Sample no further than kExtentCapM (the arc is never shorter than the x it
spans, and the follower reads at most that much), stop densifying right after
the first point that reaches the cap, clamp the per-segment point count before
the int conversion, and reject a shape whose coordinates overflow. The output
is a prefix of the unbounded result: on 20000 random references (x_max 0.5 to
80 m, curved and straight) old and new outputs are identical (max difference
0), and the same probes now take about 0.01 ms and 4 MB.

vp_reference_test covers the extreme curvatures, an infinite x_max, the
overflow rejection and x_max beyond the cap, under an alarm so a regression
fails instead of hanging; it fails on the previous implementation.

* docs(si): correct the SourceWatch threading rationale, drop the false test

e67fa53 made SourceWatch's arrival time an atomic on the premise that note()
runs on DDS callback threads while stale() runs on the control thread. It does
not: Node::main_thread_entry_ polls the DDS subscriptions (callbacks run
inside execute_subscriptions()) and then the control timer on one thread, and
no DDS listener is registered. The comment copied from input_staleness_gate.hpp
was wrong for this code, and the concurrent note()/ageSec() test exercised a
situation that never occurs.

Keep the atomic (it is lock-free and costs nothing, and keeps the class correct
if a callback ever moves to another thread), state the real threading model in
the comment, and remove the test.

* fix(si): check the follower output against the actuation envelope

The steering, speed and acceleration limits were applied only to VisionPilot's
DrivingCommand in VP_CONTROL. In SI_CONTROL the follower's output went straight
to NORMAL, although the PR description and the design note say an
out-of-envelope or bad normal output must trigger SI's fault response. A finite
speed horizon of 1e6 m/s from VP (accepted by the reference checks) reached the
actuator as a NORMAL velocity demand, as would any non-finite or over-limit
MPC/PID output.

Check the follower output in publishControlCommand(), the single place where a
follower candidate becomes NORMAL, against the same max_steering_rad /
max_abs_velocity_mps / max_abs_accel_mps2 limits. A violation latches SI_STOP
("follower <reason>") and publishes the stop in that cycle instead of the
command. The check is a pure envelopeViolation() in supervision_latch.hpp,
written so NaN fails every bound (a plain "x > max" is false for NaN), and is
covered by supervision_latch_test: inclusive symmetric bounds, each bound on its
own, a runaway speed, NaN and inf.

* fix(si): expiring re-enable, VP_CONTROL feedback guard, stop steering fallback

Three supervisor gaps found in review:

- A re-enable request that met a stale source stayed pending forever, and a
  different fault did not clear it. A press nobody acted on resumed driving
  minutes later when a source recovered, and an old press could clear a newer
  fault. The request is now a ReenableRequest with a validity window
  (reenable_window_s, default 5 s): one press still suffices once every source
  is fresh, an unanswered press expires, and a fault with a different reason
  clears it. The window keeps the single-shot re-enable of the E2E probes
  working, which a consume-on-first-tick rule would not.
- VP_CONTROL approved VisionPilot commands as NORMAL even if odometry,
  acceleration, steering or operation mode never arrived, because a source that
  was never seen is not stale and VP_CONTROL has no follower to gate on
  processData(). It now publishes HOLD until each feedback source has been seen
  once.
- A stop or hold before any NORMAL command centred the steering. The stopped
  payload (now shared by SI_STOP and HOLD through one StopControl call, which
  removes a hand-written duplicate) falls back to the vehicle's steering report
  when no command was approved yet.

ReenableRequest is pure and covered by supervision_latch_test: window boundary,
expiry on one call, a new press restarting the window, and that an expired
press cannot clear the latch. The controller wiring itself is verified by build
and start-up only.

* docs(si): fix stale comments and contradictions found in review

- StopControl said the follower's stop machinery keeps the vehicle parked during
  SI_STOP; the follower does not run while latched, the repeated stop demand
  does.
- ApprovedRequest.fault_id is documented for SI_STOP only; it is 0 for HOLD too.
- The design note said elapsed fault-to-brake timing uses SI system-clock
  stamps and, elsewhere, that system_clock stamps must not be used. State the
  condition (one shared, recorded clock) in both.
- kMaxHorizon was 200 doubles (1600 B), above the 1400 B DDS message limit the
  note sets. Lower it to 100 (800 B); VP sends 20 and a larger reference could
  not be delivered anyway. Document the bound and the new reenable_window_s.

* fix(si): bound the follower's steering by the vehicle limit, not the VP one

3235ab7 checked the follower output against max_steering_rad (0.6 rad), the
bound chosen for VisionPilot's request. The MPC clamps its own output at the
vehicle's steering limit (vehicle_info max_steer_angle, 0.70 rad), so a healthy
follower that saturates steering hit the envelope. On the openadkit-e2e rig
(VP -> SI_CONTROL, CARLA 0.9.16) the SI latched SI_STOP about 24 s into the
first run with "follower steering out of range: steer 0.700 rad" while driving
normally; it needs an explicit re-enable to move again. The unit test passed
because it only exercised the helper with the VP bound.

Use the vehicle's steering limit for the follower path: the controller reads
the same max_steer_angle node parameter the MPC reads (declare_parameter
returns an existing value), plus 1e-3 rad because the MPC clamps in float.
Speed and acceleration keep the shared limits. The test now pins that a
saturated MPC output (0.70 rad, float) is inside the follower envelope and
outside the tighter VP bound.

Rig validation of this fix on openadkit-e2e, SI at this tree: no latch over
75 s of VP -> SI_CONTROL driving, VP restart and replay gates pass with a
single re-enable (3 NORMAL), the 500 ms stop gate passes in all three
configurations (4 ms VP -> SI, 5 ms VP_CONTROL, 9 ms Autoware).

* fix(si): reject selected input the follower cannot run

SI ingress accepted selected-source trajectories the follower then rejected,
refreshed the selected-source watchdog on every one, and published HOLD each
tick instead of latching SI_STOP. HOLD makes the ApprovedRequest consumer keep
the previous approved payload (the E2E carla_actuator returns on HOLD), so a
source that kept publishing unusable input looked fresh while the last NORMAL
command kept being actuated, with no fault_id and no watchdog expiry.

Enforce the follower's own predicate at ingress:

- Autoware: require >= 3 points (the MPC isReady() floor) and validate every
  field the follower consumes finite (position z, the orientation quaternion,
  lateral velocity, heading rate, front/rear wheel angle) in addition to the
  planar pose, speed and acceleration already checked. A short or non-finite
  trajectory is malformed selected input and latches SI_STOP.
- VisionPilot: convert() rejects a converted path shorter than
  kMinFollowerPoints as an unusable shape (out cleared), so the callback
  ignores and logs it and the selected-source watchdog owns the stop, as the
  contract requires for unusable references.

test_vp_reference pins the threshold (x_max 0.5 m rejected, 1.0 m accepted)
and the bounded-work cases now assert either a usable trajectory or a bounded
rejection. The contract note records the minimum-point rule for both sources;
an indentation error in its wire table is fixed too (docutils parses cleanly).

* docs(si): document the supervised interfaces in the deployed pages

topics.rst still said five subscriptions and control_cmd only, so an operator
following it had no ApprovedRequest, no reenable topic, no record of the
startup-conditional VisionPilot inputs, and no documented way to clear a
latched SI_STOP. List the full subscription set with the startup configuration
that activates each input, the authoritative approved_request output, the
SI_LEGACY_CONTROL_CMD policy for the legacy control_cmd topic, and the
per-source watchdog rates.

architecture.rst and the demo bridge config are updated to match: the
supervised output and re-enable input stay on domain 2 (the E2E rig's actuator
and tools use them there), while the demo bridge carries the legacy control_cmd
topic only.

* fix(si): match operation-mode durability to the deployment

The operation-mode reader was made VOLATILE because the E2E rig's bridge
publishes VOLATILE, where a TRANSIENT_LOCAL request does not match at all.
But the in-repo demo bridge and user guide retain the on-change state with
TRANSIENT_LOCAL for late-joining safety islands: with a VOLATILE reader
that sample is never delivered, the follower never becomes ready and the
SI publishes HOLD forever (the regression 29995ea had fixed).

Restore transient_local as the default and make the request
startup-selectable via SI_OPERATION_MODE_DURABILITY (transient_local |
volatile; build default SI_DEFAULT_OPERATION_MODE_DURABILITY =
transient_local; invalid value fatal) so a volatile-offering bridge can be
served without weakening the in-repo contract. The E2E deployment selects
volatile explicitly.

Signed-off-by: Oguz Ozturk <oguzkaganozt@gmail.com>

* fix(si): make the SI session conversion defined and boot-varying

ApprovedRequest.session was computed with a direct double->uint32_t cast
of the epoch-millisecond value (~1.8e12), which is out of uint32_t range:
undefined behavior, and on the ARM targets' saturating conversion a
constant 0x7FFFFFFF on every boot, so the session could no longer
identify SI restarts as the IDL contract requires.

Move the computation into supervision::siSessionFromClock() (converted
through uint64_t first, masked to 31 bits, never zero) and cover the
nonzero, boot-varying behaviour in the host test.

Signed-off-by: Oguz Ozturk <oguzkaganozt@gmail.com>

* fix(si): reject references that overflow the derived schedule

A finite speed horizon with a sample near DBL_MAX makes convert()'s
fallback acceleration inf. The reference was accepted and kept the
selected-source watchdog fresh, while the MPC rejected the non-finite
trajectory field on every tick and the SI published HOLD: the
accepted-but-unrunnable input class 9488f6f rejects for Autoware
trajectories, but for VP references with no SI_STOP.

Treat the shape as unusable when any derived speed/time/accel is
non-finite (the selected-source watchdog then owns the stop, like the
other bad-case paths) and clamp the caller's time_from_start seconds
conversion into int32, which was undefined for a huge finite derived
time. Covered by vp_reference_test.

Signed-off-by: Oguz Ozturk <oguzkaganozt@gmail.com>

* fix(si): drop the age limit on the operation mode

The operation-mode watchdog (0.5 s, from arrival) assumed a 10 Hz publisher.
Autoware, and the classic demo's bridge, publish /system/operation_mode/state
only when the mode changes and retain it (transient_local): the recorded
Autoware bag in test/rosbag_test has 5 operation-mode messages in 43 s against
1534 for steering, and main's bridge config says so. After the last mode change
the source therefore went stale 0.5 s later while everything was healthy, the SI
latched SI_STOP, and allSourcesFresh() stayed false until the next mode change,
so /control/safety_island/reenable could not clear it either. With the default
SI_LEGACY_CONTROL_CMD=always the stop payload also reached control_cmd and CAN.
main's all-inputs gate was refreshed by any input, so this is a regression
against main. CI and the E2E rig did not show it because dds_pub and the rig's
operation-mode node both publish at 10 Hz. Reported in review.

Treat the operation mode as a state, not a heartbeat: no age limit, and the
"seen at least once" rule for HOLD and re-enable stays. A dead Autoware is still
caught by the trajectory watchdog. The limit is infinity (source_timeout_opmode_
remains a node parameter; note that nothing loads node parameters on the target),
and the start-up log prints "no limit". SourceWatch with an infinite timeout is
covered by supervision_latch_test; topics.rst and the design note are updated.

* fix(si): VP_CONTROL uses the same identity gate as the DrivingReference path

In VP_CONTROL the command reaches the actuator unchanged, so callbackDrivingCommand
is the only identity gate between VisionPilot and the vehicle, and it was weaker
than the DrivingReference path and than the contract ("old, duplicate or
reordered data must never become fresh again"):

- It tested the cycle number only. A VP cycle run on a repeated camera frame has
  the same source stamp and a new cycle; each was accepted and refreshed the
  watchdog, so a frozen frame approved NORMAL with no time limit.
- A session change reset the cycle counter and forgot the old session, so a late
  sample from a retired session became NORMAL after a restart.
- vp_cycle_ started at 0, so a producer whose first cycle is 0 would latch "vp
  cycle regression" at boot. (The VisionPilot fork numbers its first cycle 1.)

Use VpCandidateIdentity, as callbackDrivingReference does: a repeated frame or
cycle is ignored without refreshing the watchdog, a regression, a retired session
or an invalid stamp latches SI_STOP, and the first sample is accepted whatever
its cycle. The valid=false and no-source-stamp early returns and the finite and
envelope tests stay as they were; the identity is noted only for a command that
passes them. vp_session_, vp_session_valid_ and vp_cycle_ have no other user and
are removed. The restart rule itself (latch on a session change or not) stays
open in #67. VpCandidateIdentity is covered by test_candidate_selection.
Reported in review.

---------

Signed-off-by: Oguz Ozturk <oguzkaganozt@gmail.com>

Author: oguzkaganozt

actuation_module/freertos/CMakeLists.txt

@@ -334,4 +334,59 @@ if(BUILD_TEST STREQUAL "1")
         COMMENT "Running trajectory_buffer_policy host unit test"
         VERBATIM
     )
+
+    # Startup selection and source-identity checks are pure host logic; keep
+    # them in the existing --unit-test build rather than relying on a manual
+    # compiler command during rig verification.
+    set(CANDIDATE_SELECTION_TEST_MAIN
+        ${ACTUATION_DIR}/test/test_candidate_selection.cpp)
+    add_executable(candidate_selection_test ${CANDIDATE_SELECTION_TEST_MAIN})
+    set_source_files_properties(${CANDIDATE_SELECTION_TEST_MAIN} PROPERTIES
+        COMPILE_DEFINITIONS "main=main"
+    )
+    target_include_directories(candidate_selection_test PRIVATE
+        ${ACTUATION_DIR}/src/autoware/autoware_trajectory_follower_node/include
+    )
+    target_compile_options(candidate_selection_test PRIVATE -Wall -Wextra)
+    add_custom_command(TARGET candidate_selection_test POST_BUILD
+        COMMAND candidate_selection_test
+        COMMENT "Running candidate selection host unit test"
+        VERBATIM
+    )
+
+    # The SI fault latch and its re-enable step are pure host logic, same
+    # pattern as the tests above.
+    set(SUPERVISION_LATCH_TEST_MAIN
+        ${ACTUATION_DIR}/test/test_supervision_latch.cpp)
+    add_executable(supervision_latch_test ${SUPERVISION_LATCH_TEST_MAIN})
+    set_source_files_properties(${SUPERVISION_LATCH_TEST_MAIN} PROPERTIES
+        COMPILE_DEFINITIONS "main=main"
+    )
+    target_include_directories(supervision_latch_test PRIVATE
+        ${ACTUATION_DIR}/src/autoware/autoware_trajectory_follower_node/include
+    )
+    target_compile_options(supervision_latch_test PRIVATE -Wall -Wextra)
+    add_custom_command(TARGET supervision_latch_test POST_BUILD
+        COMMAND supervision_latch_test
+        COMMENT "Running supervision latch host unit test"
+        VERBATIM
+    )
+
+    # VP DrivingReference -> follower trajectory (SI_CONTROL + VP ingress) is
+    # pure host logic too.
+    set(VP_REFERENCE_TEST_MAIN
+        ${ACTUATION_DIR}/test/test_vp_reference.cpp)
+    add_executable(vp_reference_test ${VP_REFERENCE_TEST_MAIN})
+    set_source_files_properties(${VP_REFERENCE_TEST_MAIN} PROPERTIES
+        COMPILE_DEFINITIONS "main=main"
+    )
+    target_include_directories(vp_reference_test PRIVATE
+        ${ACTUATION_DIR}/src/autoware/autoware_trajectory_follower_node/include
+    )
+    target_compile_options(vp_reference_test PRIVATE -Wall -Wextra)
+    add_custom_command(TARGET vp_reference_test POST_BUILD
+        COMMAND vp_reference_test
+        COMMENT "Running VP reference host unit test"
+        VERBATIM
+    )
 endif()

actuation_module/include/platform/freertos/freertos_config_generated.h.in

@@ -20,6 +20,10 @@
 #define CONFIG_CONTROL_CMD_CAN_OUTPUT @CONFIG_CONTROL_CMD_CAN_OUTPUT@
 #define CONFIG_CONTROL_CMD_DDS_OUTPUT @CONFIG_CONTROL_CMD_DDS_OUTPUT@
 
+// Supervision mode: "si" (follower follows the selected trajectory source)
+// or "vp" (VisionPilot command supervised and passed through, never
+// recomputed). See vp_si_control_contract / SI issues #63/#64/#65.
+
 // Threading
 #define CONFIG_THREAD_STACK_SIZE @CONFIG_THREAD_STACK_SIZE@
 

actuation_module/src/autoware/autoware_msgs/include/autoware/autoware_msgs/messages.hpp

@@ -24,6 +24,10 @@
 #include "Float32MultiArrayStamped.h"
 #include "Float32Stamped.h"
 #include "Twist.h"
+#include "DrivingCommand.h"
+#include "DrivingReference.h"
+#include "ApprovedRequest.h"
+#include "Bool.h"
 
 using Float32MultiArrayStampedMsg = tier4_debug_msgs_msg_Float32MultiArrayStamped;
 using Float32StampedMsg = tier4_debug_msgs_msg_Float32Stamped;
@@ -47,6 +51,19 @@ using SteeringReportMsg = autoware_vehicle_msgs_msg_SteeringReport;
 using AccelWithCovarianceStampedMsg = geometry_msgs_msg_AccelWithCovarianceStamped;
 using TrajectoryPointMsg = autoware_planning_msgs_msg_TrajectoryPoint;
 using PoseStampedMsg = geometry_msgs_msg_PoseStamped;
+using DrivingCommandMsg = visionpilot_msgs_msg_DrivingCommand;
+using DrivingReferenceMsg = visionpilot_msgs_msg_DrivingReference;
+using ApprovedRequestMsg = safety_island_msgs_msg_ApprovedRequest;
+using BoolMsg = std_msgs_msg_Bool;
+
+// supervision octet values; keep in sync with ApprovedRequest.idl
+#define SIR_DECISION_NORMAL 0
+#define SIR_DECISION_SI_STOP 1
+#define SIR_DECISION_HOLD 2
+#define SIR_MODE_SI_CONTROL 0
+#define SIR_MODE_VP_CONTROL 1
+#define SIR_SOURCE_FOLLOWER 0
+#define SIR_SOURCE_VP_COMMAND 1
 
 #define OPERATION_MODE_STATE_UNKNOWN autoware_adapi_v1_msgs_msg_OperationModeState_Constants_UNKNOWN
 #define OPERATION_MODE_STATE_STOP autoware_adapi_v1_msgs_msg_OperationModeState_Constants_STOP
@@ -69,4 +86,4 @@ typedef struct TrajectoryMsg
     }
 } TrajectoryMsg;
 
-#endif // COMMON__DDS__MESSAGES_HPP_
+#endif // COMMON__DDS__MESSAGES_HPP_

actuation_module/src/autoware/autoware_msgs/msg/ApprovedRequest.idl

@@ -0,0 +1,74 @@
+#ifndef APPROVED_REQUEST_IDL_
+#define APPROVED_REQUEST_IDL_
+
+// Written for the Safety Island supervised output (vp_si_control_contract).
+// Not generated from a rosidl .msg: the C headers on the ROS/Jazzy side are
+// produced from the matching visionpilot-style definition by hand until the
+// safety_island_msgs package exists on the ROS side.
+
+#include "Time.idl"
+#include "Control.idl"
+
+module safety_island_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "The only actuator-facing Safety Island output: one approved driving" "\n"
+      "request per SI control cycle. SI is the only publisher. decision and" "\n"
+      "the identity fields let the observer/actuator correlate each output" "\n"
+      "with the SI sequence and with the fault event that produced its stop.")
+    struct ApprovedRequest {
+      @verbatim (language="comment", text=
+        "SI system clock when this request was produced.")
+      builtin_interfaces::msg::Time stamp;
+
+      @verbatim (language="comment", text=
+        "SI boot/session identity (changes on SI restart).")
+      uint32 session;
+
+      @verbatim (language="comment", text=
+        "Strictly increasing output sequence within one SI session.")
+      uint64 output_sequence;
+
+      @verbatim (language="comment", text=
+        "Requested driving decision:" "\n"
+        "0 = NORMAL   (carry control, computed by the follower or passed" "\n"
+        "             through from VisionPilot in VP_CONTROL)" "\n"
+        "1 = SI_STOP  (supervisor stop; see fault_id)" "\n"
+        "2 = HOLD     (keep the previous approved state)" "\n"
+        "Any value outside 0..2 is treated as HOLD by the actuator.")
+      octet decision;
+
+      @verbatim (language="comment", text=
+        "Configured supervision mode of this SI instance:" "\n"
+        "0 = SI_CONTROL (follower follows the selected trajectory source)" "\n"
+        "1 = VP_CONTROL (VisionPilot command supervised by SI, not recomputed)")
+      octet mode;
+
+      @verbatim (language="comment", text=
+        "Where the NORMAL control payload comes from:" "\n"
+        "0 = FOLLOWER (trajectory_follower on the selected trajectory)" "\n"
+        "1 = VP_COMMAND (accepted VisionPilot DrivingCommand, passed through)")
+      octet selected_source;
+
+      @verbatim (language="comment", text=
+        "VisionPilot cycle identity of the accepted input this output carries" "\n"
+        "(0/0 when the source is not VisionPilot or no cycle was accepted).")
+      uint32 source_session;
+
+      uint64 source_cycle;
+
+      @verbatim (language="comment", text=
+        "Identity of the fault event that caused the current SI_STOP." "\n"
+        "Increments per detected fault; 0 whenever decision is NORMAL or HOLD.")
+      uint32 fault_id;
+
+      @verbatim (language="comment", text=
+        "The actuation payload: for NORMAL, the approved driving request;" "\n"
+        "for SI_STOP, an explicit stop (target speed 0, signed negative" "\n"
+        "acceleration) so the actuator brakes without re-deciding itself.")
+      autoware_control_msgs::msg::Control control;
+    };
+  };
+};
+
+#endif  // APPROVED_REQUEST_IDL_

actuation_module/src/autoware/autoware_msgs/msg/Bool.idl

@@ -0,0 +1,21 @@
+#ifndef BOOL_IDL_
+#define BOOL_IDL_
+
+// generated from rosidl_adapter/resource/msg.idl.em (Humble 1.0.6)
+// with input from std_msgs/msg/Bool.msg
+// generated code does not contain a copyright notice
+
+module std_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "This was originally provided as an example message." "\n"
+      "It is deprecated as of Foxy" "\n"
+      "It is recommended to create your own semantically meaningful message." "\n"
+      "However if you would like to continue using this please use the equivalent in example_msgs.")
+    struct Bool {
+      boolean data;
+    };
+  };
+};
+
+#endif  // BOOL_IDL_

actuation_module/src/autoware/autoware_msgs/msg/DrivingCommand.idl

@@ -0,0 +1,48 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from visionpilot_msgs/msg/DrivingCommand.msg
+// generated code does not contain a copyright notice
+
+#include "Time.idl"
+
+module visionpilot_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "One VisionPilot driving command produced from a single camera cycle." "\n"
+      "" "\n"
+      "VP_CONTROL: the Safety Island checks this request and may replace it with" "\n"
+      "its own stop/hold response, but must not recompute VP's normal decision.")
+    struct DrivingCommand {
+      @verbatim (language="comment", text=
+        "When VP produced this command (VP host clock).")
+      builtin_interfaces::msg::Time stamp;
+
+      @verbatim (language="comment", text=
+        "Capture time of the camera frame this cycle's decision came from." "\n"
+        "Carried through from the image header so consumers can match the same" "\n"
+        "camera cycle instead of using arrival/publication time.")
+      builtin_interfaces::msg::Time source_stamp;
+
+      boolean has_source_stamp;
+
+      @verbatim (language="comment", text=
+        "VP process session (changes on restart) and strictly increasing cycle" "\n"
+        "counter for duplicate/order detection.")
+      uint32 session;
+
+      uint64 cycle;
+
+      @verbatim (language="comment", text=
+        "False when the upstream pipeline produced no usable plan this cycle.")
+      boolean valid;
+
+      @verbatim (language="comment", text=
+        "Steering tyre angle (rad, positive left) and longitudinal intent:" "\n"
+        "signed acceleration plus the speed VP intends after the planning horizon.")
+      double steering_tire_angle_rad;
+
+      double target_speed_mps;
+
+      double acceleration_mps2;
+    };
+  };
+};

actuation_module/src/autoware/autoware_msgs/msg/DrivingReference.idl

@@ -0,0 +1,61 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from visionpilot_msgs/msg/DrivingReference.msg
+// generated code does not contain a copyright notice
+
+#include "Time.idl"
+
+module visionpilot_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "One VisionPilot path and speed/stop intent from a single camera cycle." "\n"
+      "" "\n"
+      "SI_CONTROL: the Safety Island selects this reference (or the Autoware" "\n"
+      "trajectory) and its follower proposes the command. The path and the" "\n"
+      "speed horizon belong to the same cycle; consumers must reject them if a" "\n"
+      "matched pair from one cycle is not available.")
+    struct DrivingReference {
+      @verbatim (language="comment", text=
+        "When VP produced this reference (VP host clock).")
+      builtin_interfaces::msg::Time stamp;
+
+      @verbatim (language="comment", text=
+        "Capture time of the camera frame this cycle came from; see DrivingCommand.")
+      builtin_interfaces::msg::Time source_stamp;
+
+      boolean has_source_stamp;
+
+      @verbatim (language="comment", text=
+        "VP process session and strictly increasing cycle counter.")
+      uint32 session;
+
+      uint64 cycle;
+
+      @verbatim (language="comment", text=
+        "False when the upstream pipeline produced no usable reference this cycle.")
+      boolean valid;
+
+      @verbatim (language="comment", text=
+        "Fused lane-center polynomial in base_link:" "\n"
+        "  y = a x^2 + b x + c, x forward [m], y left [m]." "\n"
+        "Consumers sample geometry from the polynomial instead of relying on a" "\n"
+        "fixed point count. path_valid=false means the fit is unavailable and the" "\n"
+        "reference must be rejected.")
+      boolean path_valid;
+
+      double path_a;
+
+      double path_b;
+
+      double path_c;
+
+      double path_x_max_m;
+
+      @verbatim (language="comment", text=
+        "Planned speed schedule: v m/s at t = i * horizon_dt_s.")
+      @unit (value="i")
+      double horizon_dt_s;
+
+      sequence<double> speed_horizon_mps;
+    };
+  };
+};