feat(app): add autoware module (lib.rs) by nokosaaan · Pull Request #690 · tier4/awkernel

nokosaaan · 2026-05-08T16:54:16Z

Description

Summary

The test_autoware app (applications/autoware/src/lib.rs) was still calling ekf_localizer's old API (initialize(Pose), predict(dt), update_velocity(vx, wz), get_current_pose(bool), etc.), which no longer matches the current ekf_localizer crate (TF-aware initialize, measurement_update_pose/measurement_update_twist, the delay-compensation buffer). This PR reconnects the DAG wiring to the current API.

While doing so, this PR also fixes two bugs found along the way:

gyro_odometer was labeling its output twist message with the pre-transform input frame name instead of the transformed-to frame.
The handoff from the ekf_sink reactor to the UDP sender task used an unsynchronized static mut, which is a genuine data race across tasks/cores.

This is still the "provisionally complete" stage using a dummy pose source (pose_dummy_generator; no real NDT-style localization is wired in yet). Swapping in a real pose source is planned for a separate branch (derived from trajectory_follower).

Background

The ekf_localizer crate itself has already been through a separate round of work addressing 5 review findings (missing measurement_update_pose, a simplified measurement_update_twist, a missing low-speed gate, a non-functional delay-compensation buffer, and a non-TF-aware initialize()), followed by a full-file fidelity review. However, the test_autoware DAG wiring that actually calls into EKFModule had been left behind, still calling the pre-review API. This PR updates that wiring to match the current API.

Changes

1. Update the `ekf_localizer` reactor to the current API (`applications/autoware/src/lib.rs`)

The DAG reactor registered under the name "ekf_localizer" was rewritten as follows.

Old API call	New API call
`ekf.initialize(pose)` (bare `Pose`, first tick only)	`ekf.initialize(&pose_stamped, &Transform::identity())` (`PoseWithCovarianceStamped` + TF, first tick only)
`ekf.predict(dt)`	`ekf.accumulate_delay_time(dt)` + `ekf.predict_with_delay(dt)` (both are required -- calling only one leaves the delay buffer non-functional per its own contract)
`ekf.update_velocity(vx, wz)`	`apply_twist_observability_gate(&mut twist, threshold)` + `ekf.measurement_update_twist(&twist, current_time)`
(never called)	`ekf.measurement_update_pose(&pose_stamped, current_time)` (every tick after the first)
`ekf.get_current_pose(false)` -> `Pose`	`ekf.get_current_pose(false, current_time)` -> `PoseStamped` (take `.pose`)
`ekf.get_current_twist()` -> `Twist`	`ekf.get_current_twist(current_time)` -> `TwistStamped` (take `.twist`)

measurement_update_pose is the function the earlier review flagged as present upstream but never called at all; this is the first time it is actually invoked from the DAG.

pose_dummy_generator still emits a fixed (0, 0, 0, identity) placeholder (no real sensor behind it), and the covariance fed into measurement_update_pose is likewise a small fixed placeholder value. Swapping in a real pose-estimation source is out of scope here and planned for a separate branch.

graph LR
    dummy_pose["pose_dummy_generator (fixed 0,0,0,identity)"] --> ekf["ekf_localizer reactor"]
    gyro["gyro_odometer"] --> ekf
    ekf -- "measurement_update_pose + measurement_update_twist" --> ekf
    ekf --> sink["ekf_sink (JSON formatting)"]
    sink -- "try_send" --> chan["bounded channel (queue_size=1, overwrite-oldest)"]
    chan -- "try_recv" --> udp["periodic_udp_sender_task"]
    udp -- "socket.send" --> ext["external (UDP recv -> topic publish)"]

2. Fix `gyro_odometer`'s `frame_id` bug (`applications/autoware/gyro_odometer/src/lib.rs`)

GyroOdometerCore::process_and_get_result built its output TwistWithCovarianceStamped with header.frame_id set to the pre-transform input frame (the gyro sensor's own frame_id, imu_link in practice) instead of the TF target frame (output_frame, "base_link" by default). The angular velocity values themselves were correctly transformed into output_frame, but the label on the message still said the pre-transform frame -- a mismatch that tripped ekf_localizer::measurement_update_twist's frame_id != "base_link" check on every single tick, logging twist frame_id must be base_link, got imu_link continuously.

Changed output_frame's field type from String to &'static str (matching the same-purpose imu_corrector::ImuCorrectorConfig::output_frame, which was already &'static str), which let the output message's frame_id be set directly to self.output_frame instead of the pre-transform frame name.

3. Replace the racy `ekf_sink` -> UDP sender handoff with a channel (`applications/autoware/src/lib.rs`)

Before: the ekf_sink reactor (a synchronous callback scheduled by the DAG) wrote into static mut LATEST_JSON_DATA: Option<String>, which an independently-scheduled periodic_udp_sender_task read via unsafe. Since the two tasks can run concurrently on different cores, this unsynchronized String handoff was a genuine data race (undefined behavior), only partially masked by two separate Atomics (JSON_DATA_READY/JSON_DATA_LENGTH) that never protected the actual data.

After: a single channel is created via awkernel_async_lib::channel::bounded::new::<String>(Attribute { queue_size: 1, flow_control: false, lifespan: Permanent }). ekf_sink calls try_send (a non-blocking, synchronous call -- required, since reactor callbacks are synchronous and cannot .await); the UDP sender task polls with try_recv(). queue_size: 1 + flow_control: false reproduces the original "only the newest odometry matters, dropping intermediate ticks is fine" behavior (overwrite the oldest entry when full) while removing the race.

The now-dead LATEST_JSON_DATA/JSON_DATA_READY/JSON_DATA_LENGTH statics, the startup "waiting for JSON data to become ready" diagnostic loop that depended on them, and the now-unused core::sync::atomic import were removed.

Validation

cargo test -p gyro_odometer -- 5 passed
make x86_64 RELEASE=1 -- succeeds
cargo clippy_x86 -- no new warnings in applications/autoware/src/lib.rs (pre-existing, unrelated clippy findings remain in gyro_odometer/ekf_localizer, in code this PR did not touch)
QEMU: the twist frame_id must be base_link, got imu_link warning is gone
IMU/velocity CSVs are loaded from awkernel_script/sensor_inputs/ (kept outside this repo by policy) via the Makefile's IMU_CSV_PATH/VELOCITY_CSV_PATH (?=-defaulted to the sibling directory, overridable); confirmed 5708/5699 real rows load successfully