From c9c177d7fa622e7ad1e67432cda0f344d5ebf230 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 01:53:40 +0900 Subject: [PATCH 01/20] add: test_autoware/lib.rs Signed-off-by: nokosaaan --- applications/tests/test_autoware/Cargo.toml | 3 + applications/tests/test_autoware/src/lib.rs | 636 +++++++++++++++++++- userland/Cargo.toml | 2 +- 3 files changed, 639 insertions(+), 2 deletions(-) diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/tests/test_autoware/Cargo.toml index f745c6851..048eec8d5 100644 --- a/applications/tests/test_autoware/Cargo.toml +++ b/applications/tests/test_autoware/Cargo.toml @@ -13,6 +13,9 @@ libm = "0.2" csv-core = "0.1" awkernel_async_lib = { path = "../../../awkernel_async_lib", default-features = false } awkernel_lib = { path = "../../../awkernel_lib", default-features = false } +common_types = { path = "./common_types", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } +gyro_odometer = { path = "./gyro_odometer", default-features = false} +ekf_localizer = { path = "./ekf_localizer", default-features = false} diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/tests/test_autoware/src/lib.rs index 438b2d083..ebb9ba161 100644 --- a/applications/tests/test_autoware/src/lib.rs +++ b/applications/tests/test_autoware/src/lib.rs @@ -1,4 +1,638 @@ #![no_std] +#![allow(static_mut_refs)] extern crate alloc; -pub async fn run() {} +use alloc::{borrow::Cow, format, string::String, vec, vec::Vec}; +use awkernel_async_lib::dag::{create_dag, finish_create_dags}; +use awkernel_async_lib::net::IpAddr; +use awkernel_async_lib::scheduler::SchedulerType; +use awkernel_lib::delay::wait_microsec; +use awkernel_lib::sync::mutex::{MCSNode, Mutex}; +use core::net::Ipv4Addr; +use core::time::Duration; +use csv_core::{ReadRecordResult, Reader}; +use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; + +pub use common_types::Header; +use imu_corrector::{ImuCorrector, ImuWithCovariance}; +use imu_driver::{build_imu_msg_from_csv_row, ImuCsvRow, ImuMsg, TamagawaImuParser}; +use vehicle_velocity_converter::{ + build_velocity_report_from_csv_row, reactor_helpers, Twist, TwistWithCovariance, + TwistWithCovarianceStamped, VehicleVelocityConverter, VelocityCsvRow, +}; +use ekf_localizer::{ + get_or_initialize_default_module, EKFOdometry, Point3D, Pose, PoseWithCovariance, Quaternion, +}; + +const LOG_ENABLE: bool = false; + +const INTERFACE_ID: u64 = 0; +const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 64); +const UDP_TCP_DST_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 2); +const UDP_DST_PORT: u16 = 26099; + +static mut LATEST_JSON_DATA: Option = None; +static JSON_DATA_READY: AtomicBool = AtomicBool::new(false); +static JSON_DATA_LENGTH: AtomicUsize = AtomicUsize::new(0); + +const IMU_CSV_DATA_STR: &str = include_str!("../imu_raw.csv"); +const VELOCITY_CSV_DATA_STR: &str = include_str!("../velocity_status.csv"); + +static mut IMU_CSV_DATA: Option> = None; +static mut VELOCITY_CSV_DATA: Option> = None; +static IMU_CSV_COUNT: Mutex = Mutex::new(0); +static VELOCITY_CSV_COUNT: Mutex = Mutex::new(0); + +pub async fn run() { + wait_microsec(1000000); + + if let Err(e) = initialize_csv_data() { + log::warn!("Failed to initialize CSV data: {}", e); + } + + log::info!("Starting Autoware test application with simplified TCP networking"); + + let dag = create_dag(); + let _dag_id = dag.get_id(); + + dag.register_periodic_reactor::<_, (i32, i32, i32)>( + "start_dummy_data".into(), + move || -> (i32, i32, i32) { + (1, 2, 3) + }, + vec![ + Cow::from("start_imu"), + Cow::from("start_vel"), + Cow::from("start_pose"), + ], + SchedulerType::GEDF(5), + Duration::from_millis(50), + ) + .await; + + dag.register_reactor::<_, (i32,), (ImuMsg,)>( + "imu_driver".into(), + move |(_start_msg,): (i32,)| -> (ImuMsg,) { + let mut node = MCSNode::new(); + let mut count_guard = IMU_CSV_COUNT.lock(&mut node); + let count = *count_guard; + let data = unsafe { IMU_CSV_DATA.as_ref() }; + + let imu_msg = if let Some(csv_data) = data { + if csv_data.is_empty() { + let mut parser = TamagawaImuParser::new("imu_link"); + let awkernel_timestamp = get_awkernel_uptime_timestamp(); + let static_dummy_data = parser.generate_static_dummy_data(awkernel_timestamp); + parser + .parse_binary_data(&static_dummy_data, awkernel_timestamp) + .unwrap_or_default() + } else { + let idx = count % csv_data.len(); + let row = &csv_data[idx]; + let awkernel_timestamp = get_awkernel_uptime_timestamp(); + build_imu_msg_from_csv_row(row, "imu_link", awkernel_timestamp) + } + } else { + let mut parser = TamagawaImuParser::new("imu_link"); + let awkernel_timestamp = get_awkernel_uptime_timestamp(); + let static_dummy_data = parser.generate_static_dummy_data(awkernel_timestamp); + parser + .parse_binary_data(&static_dummy_data, awkernel_timestamp) + .unwrap_or_default() + }; + + *count_guard += 1; + if *count_guard >= 5700 { + *count_guard = 0; + log::info!("rust_e2e_app: finish csv for IMU"); + loop {} + } + + if LOG_ENABLE { + log::debug!( + "IMU data in imu_driver_node,num={}, timestamp={}", + count, + imu_msg.header.timestamp + ); + } + + (imu_msg,) + }, + vec![Cow::from("start_imu")], + vec![Cow::from("imu_data")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (i32,), (TwistWithCovarianceStamped,)>( + "vehicle_velocity_converter".into(), + move |(_start_msg,): (i32,)| -> (TwistWithCovarianceStamped,) { + let converter = VehicleVelocityConverter::default(); + + let mut node = MCSNode::new(); + let mut count_guard = VELOCITY_CSV_COUNT.lock(&mut node); + let count = *count_guard; + let data = unsafe { VELOCITY_CSV_DATA.as_ref() }; + + let csv_data = data.expect("VELOCITY_CSV_DATA must be initialized"); + let idx = count % csv_data.len(); + let row = &csv_data[idx]; + let awkernel_timestamp = get_awkernel_uptime_timestamp(); + let velocity_report = + build_velocity_report_from_csv_row(row, "base_link", awkernel_timestamp); + + *count_guard += 1; + if *count_guard >= 5700 { + *count_guard = 0; + log::info!("rust_e2e_app: finish csv for Velocity"); + loop {} + } + + let twist_msg = converter.convert_velocity_report(&velocity_report); + + if LOG_ENABLE { + log::debug!("Vehicle velocity converter: Converted velocity report to twist - linear.x={:.3}, angular.z={:.3}, awkernel_timestamp={}", + twist_msg.twist.twist.linear.x, + twist_msg.twist.twist.angular.z, + twist_msg.header.timestamp + ); + } + + (twist_msg,) + }, + vec![Cow::from("start_vel")], + vec![Cow::from("velocity_twist")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (ImuMsg,), (ImuWithCovariance,)>( + "imu_corrector".into(), + |(imu_msg,): (ImuMsg,)| -> (ImuWithCovariance,) { + let corrector = ImuCorrector::new(); + let corrected = corrector.correct_imu_with_covariance(&imu_msg, None); + (corrected,) + }, + vec![Cow::from("imu_data")], + vec![Cow::from("corrected_imu_data")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, ( + ImuWithCovariance, + TwistWithCovarianceStamped, + ), (TwistWithCovarianceStamped,)>( + "gyro_odometer".into(), + |(imu_with_cov, vehicle_twist): ( + ImuWithCovariance, + TwistWithCovarianceStamped, + )| + -> (TwistWithCovarianceStamped,) { + let current_timestamp = imu_with_cov.header.timestamp; + let current_time = get_awkernel_uptime_timestamp(); + + let gyro_odometer = match gyro_odometer::get_or_initialize() { + Ok(core) => core, + Err(_) => { + return (reactor_helpers::create_empty_twist(current_timestamp),); + } + }; + + gyro_odometer.add_vehicle_twist(vehicle_twist); + gyro_odometer.add_imu(imu_with_cov); + + match gyro_odometer.process_and_get_result(current_time) { + Some(result) => (gyro_odometer.process_result(result),), + None => (reactor_helpers::create_empty_twist(current_timestamp),), + } + }, + vec![Cow::from("corrected_imu_data"), Cow::from("velocity_twist")], + vec![Cow::from("twist_with_covariance")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (i32,), (Pose,)>( + "pose_dummy_generator".into(), + move |(_start_msg,): (i32,)| -> (Pose,) { + let x = 0.0; + let y = 0.0; + let z = 0.0; + + let pose = Pose { + position: Point3D { x, y, z }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + }; + + (pose,) + }, + vec![Cow::from("start_pose")], + vec![Cow::from("dummy_pose")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (Pose, TwistWithCovarianceStamped), (Pose, EKFOdometry)>( + "ekf_localizer".into(), + |(pose, twist): (Pose, TwistWithCovarianceStamped)| -> (Pose, EKFOdometry) { + let ekf = get_or_initialize_default_module(); + + static mut INITIALIZED: bool = false; + unsafe { + if !INITIALIZED { + ekf.initialize(pose.clone()); + INITIALIZED = true; + } + } + + // Use a fixed 50ms timestep to match the Autoware publisher cadence. + const FIXED_DT: f64 = 0.05; + let dt = FIXED_DT; + + if dt > 0.0 { + ekf.predict(dt); + } + + let vx = twist.twist.twist.linear.x; + let wz = twist.twist.twist.angular.z; + ekf.update_velocity(vx, wz); + + let ekf_pose = ekf.get_current_pose(false); + + let pose_covariance = ekf.get_current_pose_covariance(); + let twist_covariance = ekf.get_current_twist_covariance(); + + let ekf_twist = ekf.get_current_twist(); + + let odometry = EKFOdometry { + header: common_types::Header { + frame_id: "map", + timestamp: twist.header.timestamp, + }, + child_frame_id: "base_link", + pose: PoseWithCovariance { + pose: ekf_pose.clone(), + covariance: pose_covariance, + }, + twist: TwistWithCovariance { + twist: Twist { + linear: common_types::Vector3::new(ekf_twist.linear.x, ekf_twist.linear.y, ekf_twist.linear.z), + angular: common_types::Vector3::new(ekf_twist.angular.x, ekf_twist.angular.y, ekf_twist.angular.z), + }, + covariance: twist_covariance, + }, + }; + + (ekf_pose, odometry) + }, + vec![Cow::from("dummy_pose"), Cow::from("twist_with_covariance")], + vec![Cow::from("estimated_pose"), Cow::from("ekf_odometry")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_sink_reactor::<_, (Pose, EKFOdometry)>( + "ekf_sink".into(), + move |(_pose, ekf_odom): (Pose, EKFOdometry)| { + + let json_data = format!( + r#"{{"header":{{"frame_id":"{}","timestamp":{}}},"child_frame_id":"{}","pose":{{"pose":{{"position":{{"x":{:.6},"y":{:.6},"z":{:.6}}},"orientation":{{"x":{:.6},"y":{:.6},"z":{:.6},"w":{:.6}}}}},"covariance":[{}]}},"twist":{{"twist":{{"linear":{{"x":{:.6},"y":{:.6},"z":{:.6}}},"angular":{{"x":{:.6},"y":{:.6},"z":{:.6}}}}},"covariance":[{}]}}}}"#, + ekf_odom.header.frame_id, + ekf_odom.header.timestamp, + ekf_odom.child_frame_id, + ekf_odom.pose.pose.position.x, + ekf_odom.pose.pose.position.y, + ekf_odom.pose.pose.position.z, + ekf_odom.pose.pose.orientation.x, + ekf_odom.pose.pose.orientation.y, + ekf_odom.pose.pose.orientation.z, + ekf_odom.pose.pose.orientation.w, + ekf_odom.pose.covariance.iter().map(|&x| format!("{:.6}", x)).collect::>().join(","), + ekf_odom.twist.twist.linear.x, + ekf_odom.twist.twist.linear.y, + ekf_odom.twist.twist.linear.z, + ekf_odom.twist.twist.angular.x, + ekf_odom.twist.twist.angular.y, + ekf_odom.twist.twist.angular.z, + ekf_odom.twist.covariance.iter().map(|&x| format!("{:.6}", x)).collect::>().join(",") + ); + + save_json_data_to_global(json_data); + }, + vec![Cow::from("estimated_pose"), Cow::from("ekf_odometry")], + SchedulerType::GEDF(5), + Duration::from_millis(50), + ) + .await; + + let result = finish_create_dags(&[dag.clone()]).await; + + match result { + Ok(_) => { + log::info!("Autoware test application DAGs created successfully"); + } + Err(errors) => { + log::error!("Failed to create Autoware test application DAGs"); + for error in errors { + log::error!("- {error}"); + } + } + } + + log::info!("Autoware test application DAG completed"); + + log::info!("=== Network test start ==="); + log::info!("Interface ID: {}", INTERFACE_ID); + log::info!("Interface IP: {}", INTERFACE_ADDR); + log::info!("Destination IP: {}", UDP_TCP_DST_ADDR); + awkernel_lib::net::add_ipv4_addr(INTERFACE_ID, INTERFACE_ADDR, 24); + log::info!( + "Configured IPv4 address {} on interface {}", + INTERFACE_ADDR, + INTERFACE_ID + ); + + log::info!("Waiting for network stack initialization..."); + awkernel_async_lib::sleep(Duration::from_secs(2)).await; + + log::info!("Starting periodic UDP sender task"); + start_periodic_udp_sender().await; + + log::info!("Waiting for JSON data to become ready..."); + let mut wait_count = 0; + const MAX_WAIT_COUNT: u32 = 3; + + while !JSON_DATA_READY.load(Ordering::Relaxed) && wait_count < MAX_WAIT_COUNT { + awkernel_async_lib::sleep(Duration::from_secs(1)).await; + wait_count += 1; + } + + if JSON_DATA_READY.load(Ordering::Relaxed) { + } else { + log::warn!("JSON data was not ready. Periodic UDP sender task remains waiting"); + } + + log::info!("Autoware test application completed"); +} + +fn initialize_csv_data() -> Result<(), &'static str> { + unsafe { + if IMU_CSV_DATA.is_none() { + let imu_data = parse_imu_csv(IMU_CSV_DATA_STR)?; + if imu_data.is_empty() { + return Err("IMU CSV is empty"); + } + log::info!("Loaded IMU CSV data: {} rows", imu_data.len()); + IMU_CSV_DATA = Some(imu_data); + } + + if VELOCITY_CSV_DATA.is_none() { + let velocity_data = parse_velocity_csv(VELOCITY_CSV_DATA_STR)?; + if velocity_data.is_empty() { + return Err("Velocity CSV is empty"); + } + log::info!("Loaded velocity CSV data: {} rows", velocity_data.len()); + VELOCITY_CSV_DATA = Some(velocity_data); + } + } + + Ok(()) +} + +fn parse_imu_csv(csv: &str) -> Result, &'static str> { + let mut rows = Vec::new(); + + parse_csv_records(csv, |fields| { + if fields.len() < 12 { + return Err("IMU CSV has insufficient columns"); + } + + let timestamp = parse_timestamp(fields[0], fields[1])?; + let orientation = imu_driver::Quaternion { + x: parse_f64(fields[2])?, + y: parse_f64(fields[3])?, + z: parse_f64(fields[4])?, + w: parse_f64(fields[5])?, + }; + let angular_velocity = common_types::Vector3::new( + parse_f64(fields[6])?, + parse_f64(fields[7])?, + parse_f64(fields[8])?, + ); + let linear_acceleration = common_types::Vector3::new( + parse_f64(fields[9])?, + parse_f64(fields[10])?, + parse_f64(fields[11])?, + ); + + rows.push(ImuCsvRow { + timestamp, + orientation, + angular_velocity, + linear_acceleration, + }); + Ok(()) + })?; + + Ok(rows) +} + +fn parse_velocity_csv(csv: &str) -> Result, &'static str> { + let mut rows = Vec::new(); + + parse_csv_records(csv, |fields| { + if fields.len() < 5 { + return Err("Velocity CSV has insufficient columns"); + } + + let timestamp = parse_timestamp(fields[0], fields[1])?; + let longitudinal_velocity = parse_f64(fields[2])?; + let lateral_velocity = parse_f64(fields[3])?; + let heading_rate = parse_f64(fields[4])?; + + rows.push(VelocityCsvRow { + timestamp, + longitudinal_velocity, + lateral_velocity, + heading_rate, + }); + Ok(()) + })?; + + Ok(rows) +} + +fn parse_csv_records(csv: &str, mut on_record: F) -> Result<(), &'static str> +where + F: FnMut(&[&str]) -> Result<(), &'static str>, +{ + let mut reader = Reader::new(); + let mut input = csv.as_bytes(); + let mut output = vec![0u8; 4096]; + let mut ends = vec![0usize; 32]; + let mut header_skipped = false; + + loop { + let (result, in_read, _out_written, num_fields) = + reader.read_record(input, &mut output, &mut ends); + input = &input[in_read..]; + + if matches!(result, ReadRecordResult::OutputFull) { + return Err("CSV output buffer is too small"); + } + + if num_fields == 0 { + if matches!(result, ReadRecordResult::InputEmpty | ReadRecordResult::End) { + break; + } + continue; + } + + let mut fields: Vec<&str> = Vec::with_capacity(num_fields); + let mut start = 0usize; + for i in 0..num_fields { + let end = ends[i]; + let slice = &output[start..end]; + let field = core::str::from_utf8(slice).map_err(|_| "Failed to decode CSV UTF-8")?; + fields.push(field); + start = end; + } + + if !header_skipped { + header_skipped = true; + } else { + on_record(&fields)?; + } + + if matches!(result, ReadRecordResult::End) { + break; + } + } + + Ok(()) +} + +fn parse_timestamp(sec: &str, nsec: &str) -> Result { + let sec_val = parse_u64(sec)?; + let nsec_val = parse_u64(nsec)?; + let ts = sec_val + .checked_mul(1_000_000_000) + .and_then(|v| v.checked_add(nsec_val)) + .ok_or("Timestamp calculation overflowed")?; + Ok(ts) +} + +fn parse_u64(field: &str) -> Result { + let trimmed = field.trim(); + if trimmed.is_empty() { + return Ok(0); + } + trimmed.parse::().map_err(|_| "Failed to parse u64") +} + +fn parse_f64(field: &str) -> Result { + let trimmed = field.trim(); + if trimmed.is_empty() { + return Ok(0.0); + } + trimmed.parse::().map_err(|_| "Failed to parse f64") +} + +fn get_awkernel_uptime_timestamp() -> u64 { + let uptime_nanos = awkernel_lib::delay::uptime_nano(); + if uptime_nanos > u64::MAX as u128 { + u64::MAX + } else { + uptime_nanos as u64 + } +} + +pub async fn start_periodic_udp_sender() { + awkernel_async_lib::spawn( + "periodic_udp_sender".into(), + periodic_udp_sender_task(), + awkernel_async_lib::scheduler::SchedulerType::GEDF(5), + ) + .await; +} + +async fn periodic_udp_sender_task() { + let socket_result = awkernel_async_lib::net::udp::UdpSocket::bind_on_interface( + INTERFACE_ID, + &Default::default(), + ); + + let mut socket = match socket_result { + Ok(socket) => socket, + Err(e) => { + log::error!( + "Periodic UDP sender task: failed to create UDP socket: {:?}", + e + ); + return; + } + }; + + let dst_addr = IpAddr::new_v4(UDP_TCP_DST_ADDR); + let mut counter = 0; + + loop { + if !JSON_DATA_READY.load(Ordering::Relaxed) { + awkernel_async_lib::sleep(Duration::from_secs(1)).await; + continue; + } + + let json_data = unsafe { LATEST_JSON_DATA.as_ref().map(|s| s.clone()) }; + + if let Some(data) = json_data { + match socket.send(data.as_bytes(), &dst_addr, UDP_DST_PORT).await { + Ok(_) => { + counter += 1; + log::info!( + "Periodic UDP sender task: send success #{} ({} bytes)", + counter, + data.len() + ); + + let mut buf = [0u8; 1024]; + if let Some(Ok((n, src_addr, src_port))) = awkernel_async_lib::timeout( + Duration::from_millis(100), + socket.recv(&mut buf), + ) + .await + { + if let Ok(response) = core::str::from_utf8(&buf[..n]) { + log::debug!( + "Periodic UDP sender task: response received: {}:{} - {}", + src_addr.get_addr(), + src_port, + response + ); + } + } + } + Err(e) => { + log::warn!("Periodic UDP sender task: send failed: {:?}", e); + } + } + } else { + log::warn!("Periodic UDP sender task: failed to get JSON data"); + } + + awkernel_async_lib::sleep(Duration::from_millis(5)).await; + } +} + +fn save_json_data_to_global(json_data: String) { + unsafe { + LATEST_JSON_DATA = Some(json_data.clone()); + } + JSON_DATA_READY.store(true, Ordering::Relaxed); + JSON_DATA_LENGTH.store(json_data.len(), Ordering::Relaxed); +} diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 222256a19..c6fac4eaa 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -79,7 +79,7 @@ path = "../applications/tests/test_voluntary_preemption" optional = true [features] -default = [] +default = ["test_autoware"] perf = ["awkernel_services/perf"] # Evaluation applications From a20c0f32cd47991a2096ab3948b19cca12ac22cc Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 01:56:04 +0900 Subject: [PATCH 02/20] fix: apply cargo fmt 5 Signed-off-by: nokosaaan --- .../test_autoware/gyro_odometer/src/lib.rs | 24 ++++++++++++------- applications/tests/test_autoware/src/lib.rs | 24 ++++++++++++------- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/tests/test_autoware/gyro_odometer/src/lib.rs index 834924786..5084e7b65 100644 --- a/applications/tests/test_autoware/gyro_odometer/src/lib.rs +++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs @@ -2,9 +2,9 @@ extern crate alloc; use alloc::{collections::VecDeque, string::String}; -use core::time::Duration; use core::ptr::null_mut; use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; +use core::time::Duration; pub use imu_corrector::{transform_covariance, ImuWithCovariance, Transform}; pub use imu_driver::{Header, ImuMsg, Quaternion, Vector3}; @@ -395,7 +395,8 @@ mod tests { let odometry = Odometry { velocity: sample_twist.twist.twist.linear.x, }; - let twist = core.convert_vehicle_velocity_to_twist(&odometry, sample_twist.header.timestamp); + let twist = + core.convert_vehicle_velocity_to_twist(&odometry, sample_twist.header.timestamp); assert_eq!(twist.header.frame_id, sample_twist.header.frame_id); assert_eq!(twist.header.timestamp, 123456789); @@ -413,8 +414,17 @@ mod tests { let mut imu_with_cov = ImuWithCovariance::from_imu_msg(&imu_msg); imu_with_cov.angular_velocity_covariance = [0.0009, 0.0, 0.0, 0.0, 0.0009, 0.0, 0.0, 0.0, 0.0009]; - imu_with_cov.linear_acceleration_covariance = - [100000000.0, 0.0, 0.0, 0.0, 100000000.0, 0.0, 0.0, 0.0, 100000000.0]; + imu_with_cov.linear_acceleration_covariance = [ + 100000000.0, + 0.0, + 0.0, + 0.0, + 100000000.0, + 0.0, + 0.0, + 0.0, + 100000000.0, + ]; let result = core.process_imu_with_covariance(imu_with_cov); assert!(result.is_ok()); @@ -448,9 +458,7 @@ pub fn get_or_initialize() -> Result<&'static mut GyroOdometerCore> { AtomicOrdering::Acquire, AtomicOrdering::Relaxed, ) { - Ok(_) => { - Ok(unsafe { &mut *new_ptr }) - } + Ok(_) => Ok(unsafe { &mut *new_ptr }), Err(existing_ptr) => { unsafe { let _ = alloc::boxed::Box::from_raw(new_ptr); @@ -458,4 +466,4 @@ pub fn get_or_initialize() -> Result<&'static mut GyroOdometerCore> { Ok(unsafe { &mut *existing_ptr }) } } -} \ No newline at end of file +} diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/tests/test_autoware/src/lib.rs index ebb9ba161..d27449eb8 100644 --- a/applications/tests/test_autoware/src/lib.rs +++ b/applications/tests/test_autoware/src/lib.rs @@ -9,20 +9,20 @@ use awkernel_async_lib::scheduler::SchedulerType; use awkernel_lib::delay::wait_microsec; use awkernel_lib::sync::mutex::{MCSNode, Mutex}; use core::net::Ipv4Addr; +use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; use core::time::Duration; use csv_core::{ReadRecordResult, Reader}; -use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; pub use common_types::Header; +use ekf_localizer::{ + get_or_initialize_default_module, EKFOdometry, Point3D, Pose, PoseWithCovariance, Quaternion, +}; use imu_corrector::{ImuCorrector, ImuWithCovariance}; use imu_driver::{build_imu_msg_from_csv_row, ImuCsvRow, ImuMsg, TamagawaImuParser}; use vehicle_velocity_converter::{ build_velocity_report_from_csv_row, reactor_helpers, Twist, TwistWithCovariance, TwistWithCovarianceStamped, VehicleVelocityConverter, VelocityCsvRow, }; -use ekf_localizer::{ - get_or_initialize_default_module, EKFOdometry, Point3D, Pose, PoseWithCovariance, Quaternion, -}; const LOG_ENABLE: bool = false; @@ -57,9 +57,7 @@ pub async fn run() { dag.register_periodic_reactor::<_, (i32, i32, i32)>( "start_dummy_data".into(), - move || -> (i32, i32, i32) { - (1, 2, 3) - }, + move || -> (i32, i32, i32) { (1, 2, 3) }, vec![ Cow::from("start_imu"), Cow::from("start_vel"), @@ -282,8 +280,16 @@ pub async fn run() { }, twist: TwistWithCovariance { twist: Twist { - linear: common_types::Vector3::new(ekf_twist.linear.x, ekf_twist.linear.y, ekf_twist.linear.z), - angular: common_types::Vector3::new(ekf_twist.angular.x, ekf_twist.angular.y, ekf_twist.angular.z), + linear: common_types::Vector3::new( + ekf_twist.linear.x, + ekf_twist.linear.y, + ekf_twist.linear.z, + ), + angular: common_types::Vector3::new( + ekf_twist.angular.x, + ekf_twist.angular.y, + ekf_twist.angular.z, + ), }, covariance: twist_covariance, }, From 15c8668f4df57ad978617d20ab0705ca2d93967d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:09:36 +0900 Subject: [PATCH 03/20] fix: fallback when len=0 Signed-off-by: nokosaaan --- applications/tests/test_autoware/build.rs | 31 +++++++++++ applications/tests/test_autoware/src/lib.rs | 57 ++++++++++++++------- 2 files changed, 69 insertions(+), 19 deletions(-) create mode 100644 applications/tests/test_autoware/build.rs diff --git a/applications/tests/test_autoware/build.rs b/applications/tests/test_autoware/build.rs new file mode 100644 index 000000000..7269fdc9f --- /dev/null +++ b/applications/tests/test_autoware/build.rs @@ -0,0 +1,31 @@ +use std::{env, fs, path::PathBuf}; + +fn main() { + let out_dir = PathBuf::from(env::var("OUT_DIR").expect("OUT_DIR is not set")); + let csv_data_path = out_dir.join("csv_data.rs"); + + let imu_csv = read_csv_from_env("IMU_CSV_PATH"); + let velocity_csv = read_csv_from_env("VELOCITY_CSV_PATH"); + + let generated = format!( + "pub const IMU_CSV_DATA_STR: &str = {imu:?};\npub const VELOCITY_CSV_DATA_STR: &str = {velocity:?};\n", + imu = imu_csv, + velocity = velocity_csv, + ); + + fs::write(&csv_data_path, generated).expect("failed to write generated csv constants"); +} + +fn read_csv_from_env(var_name: &str) -> String { + let Ok(path) = env::var(var_name) else { + return String::new(); + }; + + match fs::read_to_string(&path) { + Ok(contents) => contents, + Err(err) => { + println!("cargo:warning=failed to read {var_name} from {path}: {err}"); + String::new() + } + } +} diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/tests/test_autoware/src/lib.rs index d27449eb8..7c1ae9f4e 100644 --- a/applications/tests/test_autoware/src/lib.rs +++ b/applications/tests/test_autoware/src/lib.rs @@ -35,8 +35,7 @@ static mut LATEST_JSON_DATA: Option = None; static JSON_DATA_READY: AtomicBool = AtomicBool::new(false); static JSON_DATA_LENGTH: AtomicUsize = AtomicUsize::new(0); -const IMU_CSV_DATA_STR: &str = include_str!("../imu_raw.csv"); -const VELOCITY_CSV_DATA_STR: &str = include_str!("../velocity_status.csv"); +include!(concat!(env!("OUT_DIR"), "/csv_data.rs")); static mut IMU_CSV_DATA: Option> = None; static mut VELOCITY_CSV_DATA: Option> = None; @@ -75,11 +74,12 @@ pub async fn run() { let mut count_guard = IMU_CSV_COUNT.lock(&mut node); let count = *count_guard; let data = unsafe { IMU_CSV_DATA.as_ref() }; + let awkernel_timestamp = get_awkernel_uptime_timestamp(); let imu_msg = if let Some(csv_data) = data { if csv_data.is_empty() { + // Fallback: generate dummy IMU data let mut parser = TamagawaImuParser::new("imu_link"); - let awkernel_timestamp = get_awkernel_uptime_timestamp(); let static_dummy_data = parser.generate_static_dummy_data(awkernel_timestamp); parser .parse_binary_data(&static_dummy_data, awkernel_timestamp) @@ -87,12 +87,11 @@ pub async fn run() { } else { let idx = count % csv_data.len(); let row = &csv_data[idx]; - let awkernel_timestamp = get_awkernel_uptime_timestamp(); build_imu_msg_from_csv_row(row, "imu_link", awkernel_timestamp) } } else { + // Fallback: generate dummy IMU data if data is not initialized let mut parser = TamagawaImuParser::new("imu_link"); - let awkernel_timestamp = get_awkernel_uptime_timestamp(); let static_dummy_data = parser.generate_static_dummy_data(awkernel_timestamp); parser .parse_binary_data(&static_dummy_data, awkernel_timestamp) @@ -131,13 +130,41 @@ pub async fn run() { let mut count_guard = VELOCITY_CSV_COUNT.lock(&mut node); let count = *count_guard; let data = unsafe { VELOCITY_CSV_DATA.as_ref() }; - - let csv_data = data.expect("VELOCITY_CSV_DATA must be initialized"); - let idx = count % csv_data.len(); - let row = &csv_data[idx]; let awkernel_timestamp = get_awkernel_uptime_timestamp(); - let velocity_report = - build_velocity_report_from_csv_row(row, "base_link", awkernel_timestamp); + + let twist_msg = if let Some(csv_data) = data { + if csv_data.is_empty() { + // Fallback: generate dummy velocity report + let dummy_report = vehicle_velocity_converter::VelocityReport { + header: common_types::Header { + frame_id: "base_link", + timestamp: awkernel_timestamp, + }, + longitudinal_velocity: 1.0, + lateral_velocity: 0.0, + heading_rate: 0.0, + }; + converter.convert_velocity_report(&dummy_report) + } else { + let idx = count % csv_data.len(); + let row = &csv_data[idx]; + let velocity_report = + build_velocity_report_from_csv_row(row, "base_link", awkernel_timestamp); + converter.convert_velocity_report(&velocity_report) + } + } else { + // Fallback: generate dummy velocity report if data is not initialized + let dummy_report = vehicle_velocity_converter::VelocityReport { + header: common_types::Header { + frame_id: "base_link", + timestamp: awkernel_timestamp, + }, + longitudinal_velocity: 1.0, + lateral_velocity: 0.0, + heading_rate: 0.0, + }; + converter.convert_velocity_report(&dummy_report) + }; *count_guard += 1; if *count_guard >= 5700 { @@ -146,8 +173,6 @@ pub async fn run() { loop {} } - let twist_msg = converter.convert_velocity_report(&velocity_report); - if LOG_ENABLE { log::debug!("Vehicle velocity converter: Converted velocity report to twist - linear.x={:.3}, angular.z={:.3}, awkernel_timestamp={}", twist_msg.twist.twist.linear.x, @@ -391,18 +416,12 @@ fn initialize_csv_data() -> Result<(), &'static str> { unsafe { if IMU_CSV_DATA.is_none() { let imu_data = parse_imu_csv(IMU_CSV_DATA_STR)?; - if imu_data.is_empty() { - return Err("IMU CSV is empty"); - } log::info!("Loaded IMU CSV data: {} rows", imu_data.len()); IMU_CSV_DATA = Some(imu_data); } if VELOCITY_CSV_DATA.is_none() { let velocity_data = parse_velocity_csv(VELOCITY_CSV_DATA_STR)?; - if velocity_data.is_empty() { - return Err("Velocity CSV is empty"); - } log::info!("Loaded velocity CSV data: {} rows", velocity_data.len()); VELOCITY_CSV_DATA = Some(velocity_data); } From 3f8bd2216809c7962149dd46428755649dc94ba0 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:15:03 +0900 Subject: [PATCH 04/20] fix: apply clippy error Signed-off-by: nokosaaan --- .../vehicle_velocity_converter/src/lib.rs | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs index 2b523c95f..1c6b864d1 100644 --- a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs @@ -78,6 +78,12 @@ pub struct VehicleVelocityConverter { speed_scale_factor: f64, } +impl Default for VehicleVelocityConverter { + fn default() -> Self { + Self::new("base_link", 0.2, 0.1, 1.0) + } +} + impl VehicleVelocityConverter { pub fn new( frame_id: &'static str, @@ -106,10 +112,6 @@ impl VehicleVelocityConverter { Self::new(frame_id, stddev_vx, stddev_wz, speed_scale_factor) } - pub fn default() -> Self { - Self::new("base_link", 0.2, 0.1, 1.0) - } - pub fn convert_velocity_report(&self, msg: &VelocityReport) -> TwistWithCovarianceStamped { TwistWithCovarianceStamped { header: msg.header.clone(), @@ -133,12 +135,12 @@ impl VehicleVelocityConverter { fn create_covariance_matrix(&self) -> [f64; 36] { let mut covariance = [0.0; 36]; - covariance[0 + 0 * 6] = self.stddev_vx * self.stddev_vx; - covariance[1 + 1 * 6] = 10000.0; - covariance[2 + 2 * 6] = 10000.0; - covariance[3 + 3 * 6] = 10000.0; - covariance[4 + 4 * 6] = 10000.0; - covariance[5 + 5 * 6] = self.stddev_wz * self.stddev_wz; + covariance[0] = self.stddev_vx * self.stddev_vx; + covariance[7] = 10000.0; + covariance[14] = 10000.0; + covariance[21] = 10000.0; + covariance[28] = 10000.0; + covariance[35] = self.stddev_wz * self.stddev_wz; covariance } From 7625192904b72abe14cdabc2b375c2a6e74bece1 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:19:18 +0900 Subject: [PATCH 05/20] fix: apply clippy error 2 Signed-off-by: nokosaaan --- .../tests/test_autoware/imu_corrector/src/lib.rs | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/tests/test_autoware/imu_corrector/src/lib.rs index abf265a33..edeb4c134 100644 --- a/applications/tests/test_autoware/imu_corrector/src/lib.rs +++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs @@ -61,6 +61,12 @@ pub struct MockTransformListener { transforms: alloc::collections::BTreeMap, } +impl Default for MockTransformListener { + fn default() -> Self { + Self::new() + } +} + impl MockTransformListener { pub fn new() -> Self { Self { @@ -153,6 +159,12 @@ pub struct ImuCorrector { transform_listener: T, } +impl Default for ImuCorrector { + fn default() -> Self { + Self::new() + } +} + impl ImuCorrector { pub fn new() -> Self { Self { @@ -247,7 +259,7 @@ impl ImuCorrector { pub fn correct_imu_with_dynamic_tf(&self, imu_msg: &ImuMsg) -> Option { let transform = self .transform_listener - .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?; + .get_latest_transform(imu_msg.header.frame_id, self.config.output_frame)?; let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, Some(&transform)); Some(corrected_with_cov.to_imu_msg()) @@ -259,7 +271,7 @@ impl ImuCorrector { ) -> Option { let transform = self .transform_listener - .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?; + .get_latest_transform(imu_msg.header.frame_id, self.config.output_frame)?; Some(self.correct_imu_with_covariance(imu_msg, Some(&transform))) } From 581a3fd64212fe4aeffc6997a788838cfefd9f94 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:23:53 +0900 Subject: [PATCH 06/20] fix: apply clippy error 3 Signed-off-by: nokosaaan --- .../tests/test_autoware/gyro_odometer/src/lib.rs | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/tests/test_autoware/gyro_odometer/src/lib.rs index 5084e7b65..f5ad032dc 100644 --- a/applications/tests/test_autoware/gyro_odometer/src/lib.rs +++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs @@ -96,7 +96,7 @@ impl GyroOdometerCore { } let tf = self.get_transform( - &self.gyro_queue.front().unwrap().header.frame_id, + self.gyro_queue.front().unwrap().header.frame_id, &self.output_frame, )?; @@ -112,7 +112,7 @@ impl GyroOdometerCore { for vehicle_twist in &self.vehicle_twist_queue { vx_mean += vehicle_twist.twist.twist.linear.x; - vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; + vx_covariance_original += vehicle_twist.twist.covariance[0]; } vx_mean /= self.vehicle_twist_queue.len() as f64; vx_covariance_original /= self.vehicle_twist_queue.len() as f64; @@ -177,7 +177,7 @@ impl GyroOdometerCore { dt.abs() > timeout_sec } pub fn get_transform(&self, from_frame: &str, to_frame: &str) -> Result { - if from_frame == to_frame || from_frame == "" || to_frame == "" { + if from_frame == to_frame || from_frame.is_empty() || to_frame.is_empty() { Ok(Transform::identity()) } else { Ok(Transform::identity()) @@ -240,10 +240,7 @@ impl GyroOdometerCore { &mut self, current_time: u64, ) -> Option { - match self.concat_gyro_and_odometer(current_time) { - Ok(result) => result, - Err(_) => None, - } + self.concat_gyro_and_odometer(current_time).unwrap_or_default() } pub fn get_queue_sizes(&self) -> (usize, usize) { From 57887bce78c151fa46f1d35535079900bdf4ff92 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:26:34 +0900 Subject: [PATCH 07/20] fix: apply cargo fmt 6 Signed-off-by: nokosaaan --- applications/tests/test_autoware/gyro_odometer/src/lib.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/tests/test_autoware/gyro_odometer/src/lib.rs index f5ad032dc..c99e6b186 100644 --- a/applications/tests/test_autoware/gyro_odometer/src/lib.rs +++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs @@ -240,7 +240,8 @@ impl GyroOdometerCore { &mut self, current_time: u64, ) -> Option { - self.concat_gyro_and_odometer(current_time).unwrap_or_default() + self.concat_gyro_and_odometer(current_time) + .unwrap_or_default() } pub fn get_queue_sizes(&self) -> (usize, usize) { From c9261d2f9695077ecbc6c47da79ad76fb11f9f60 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:32:32 +0900 Subject: [PATCH 08/20] fix: apply clippy error 4 Signed-off-by: nokosaaan --- applications/tests/test_autoware/gyro_odometer/src/lib.rs | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/tests/test_autoware/gyro_odometer/src/lib.rs index c99e6b186..6916a16ae 100644 --- a/applications/tests/test_autoware/gyro_odometer/src/lib.rs +++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs @@ -177,11 +177,8 @@ impl GyroOdometerCore { dt.abs() > timeout_sec } pub fn get_transform(&self, from_frame: &str, to_frame: &str) -> Result { - if from_frame == to_frame || from_frame.is_empty() || to_frame.is_empty() { - Ok(Transform::identity()) - } else { - Ok(Transform::identity()) - } + let _ = (from_frame, to_frame); + Ok(Transform::identity()) } pub fn process_result( From 4989f944a0124eb8aece5f540801e31478343f71 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:36:25 +0900 Subject: [PATCH 09/20] fix: apply clippy error 5 Signed-off-by: nokosaaan --- .../tests/test_autoware/ekf_localizer/src/lib.rs | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/applications/tests/test_autoware/ekf_localizer/src/lib.rs b/applications/tests/test_autoware/ekf_localizer/src/lib.rs index 038a2cbf1..e07f1223d 100644 --- a/applications/tests/test_autoware/ekf_localizer/src/lib.rs +++ b/applications/tests/test_autoware/ekf_localizer/src/lib.rs @@ -102,6 +102,12 @@ pub struct Simple1DFilter { proc_var_x_c: f64, } +impl Default for Simple1DFilter { + fn default() -> Self { + Self::new() + } +} + impl Simple1DFilter { pub fn new() -> Self { Self { @@ -125,11 +131,11 @@ impl Simple1DFilter { } let proc_var_x_d = self.proc_var_x_c * dt * dt; - self.var = self.var + proc_var_x_d; + self.var += proc_var_x_d; let kalman_gain = self.var / (self.var + obs_var); - self.x = self.x + kalman_gain * (obs - self.x); - self.var = (1.0 - kalman_gain) * self.var; + self.x += kalman_gain * (obs - self.x); + self.var *= 1.0 - kalman_gain; } pub fn set_proc_var(&mut self, proc_var: f64) { @@ -211,7 +217,7 @@ impl EKFModule { } fn predict_next_state(&self, dt: f64) -> StateVector { - let mut x_next = self.state.clone(); + let mut x_next = self.state; let x = self.state[StateIndex::X as usize]; let y = self.state[StateIndex::Y as usize]; let yaw = self.state[StateIndex::Yaw as usize]; From c77328ec86d1cfd09b88e6e7d9d6137a8fe6ec88 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 9 May 2026 02:40:48 +0900 Subject: [PATCH 10/20] fix: apply clippy error 6 Signed-off-by: nokosaaan --- applications/tests/test_autoware/src/lib.rs | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/tests/test_autoware/src/lib.rs index 7c1ae9f4e..37881b3fb 100644 --- a/applications/tests/test_autoware/src/lib.rs +++ b/applications/tests/test_autoware/src/lib.rs @@ -14,6 +14,7 @@ use core::time::Duration; use csv_core::{ReadRecordResult, Reader}; pub use common_types::Header; +use core::slice; use ekf_localizer::{ get_or_initialize_default_module, EKFOdometry, Point3D, Pose, PoseWithCovariance, Quaternion, }; @@ -102,7 +103,9 @@ pub async fn run() { if *count_guard >= 5700 { *count_guard = 0; log::info!("rust_e2e_app: finish csv for IMU"); - loop {} + loop { + wait_microsec(1000); + } } if LOG_ENABLE { @@ -170,7 +173,9 @@ pub async fn run() { if *count_guard >= 5700 { *count_guard = 0; log::info!("rust_e2e_app: finish csv for Velocity"); - loop {} + loop { + wait_microsec(1000); + } } if LOG_ENABLE { @@ -269,7 +274,7 @@ pub async fn run() { static mut INITIALIZED: bool = false; unsafe { if !INITIALIZED { - ekf.initialize(pose.clone()); + ekf.initialize(pose); INITIALIZED = true; } } @@ -300,7 +305,7 @@ pub async fn run() { }, child_frame_id: "base_link", pose: PoseWithCovariance { - pose: ekf_pose.clone(), + pose: ekf_pose, covariance: pose_covariance, }, twist: TwistWithCovariance { @@ -362,7 +367,7 @@ pub async fn run() { ) .await; - let result = finish_create_dags(&[dag.clone()]).await; + let result = finish_create_dags(slice::from_ref(&dag)).await; match result { Ok(_) => { @@ -521,8 +526,7 @@ where let mut fields: Vec<&str> = Vec::with_capacity(num_fields); let mut start = 0usize; - for i in 0..num_fields { - let end = ends[i]; + for &end in ends.iter().take(num_fields) { let slice = &output[start..end]; let field = core::str::from_utf8(slice).map_err(|_| "Failed to decode CSV UTF-8")?; fields.push(field); @@ -613,7 +617,7 @@ async fn periodic_udp_sender_task() { continue; } - let json_data = unsafe { LATEST_JSON_DATA.as_ref().map(|s| s.clone()) }; + let json_data = unsafe { LATEST_JSON_DATA.clone() }; if let Some(data) = json_data { match socket.send(data.as_bytes(), &dst_addr, UDP_DST_PORT).await { From f88665b83a1958ab61e25b93f0ee1946af7dc907 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:15:22 +0900 Subject: [PATCH 11/20] fix: dependics path 2 Signed-off-by: nokosaaan --- Cargo.toml | 1 + applications/tests/test_autoware/Cargo.toml | 3 +-- userland/Cargo.toml | 10 +++++----- userland/src/lib.rs | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 6f4af7aab..3e46c056e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,6 +13,7 @@ members = [ "applications/awkernel_services", "applications/awkernel_shell", "applications/awkernel_display", + "applications/autoware", "applications/rd_gen_to_dags", "applications/tests/*", "smoltcp", diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/tests/test_autoware/Cargo.toml index 048eec8d5..fea864309 100644 --- a/applications/tests/test_autoware/Cargo.toml +++ b/applications/tests/test_autoware/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "test_autoware" +name = "autoware" version = "0.1.0" edition = "2021" @@ -13,7 +13,6 @@ libm = "0.2" csv-core = "0.1" awkernel_async_lib = { path = "../../../awkernel_async_lib", default-features = false } awkernel_lib = { path = "../../../awkernel_lib", default-features = false } -common_types = { path = "./common_types", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } diff --git a/userland/Cargo.toml b/userland/Cargo.toml index c6fac4eaa..714d817c5 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -14,6 +14,10 @@ path = "../awkernel_async_lib" [dependencies.awkernel_services] path = "../applications/awkernel_services" +[dependencies.autoware] +path = "../applications/autoware" +optional = true + [dependencies.rd_gen_to_dags] path = "../applications/rd_gen_to_dags" optional = true @@ -66,10 +70,6 @@ optional = true path = "../applications/tests/test_dag" optional = true -[dependencies.test_autoware] -path = "../applications/tests/test_autoware" -optional = true - [dependencies.test_dvfs] path = "../applications/tests/test_dvfs" optional = true @@ -84,6 +84,7 @@ perf = ["awkernel_services/perf"] # Evaluation applications rd_gen_to_dags = ["dep:rd_gen_to_dags"] +autoware = ["dep:autoware"] # Test applications test_network = ["dep:test_network"] @@ -97,5 +98,4 @@ test_gedf = ["dep:test_gedf"] test_measure_channel = ["dep:test_measure_channel"] test_measure_channel_heavy = ["dep:test_measure_channel_heavy"] test_dag = ["dep:test_dag"] -test_autoware = ["dep:test_autoware"] test_voluntary_preemption = ["dep:test_voluntary_preemption"] diff --git a/userland/src/lib.rs b/userland/src/lib.rs index 7082e1eba..bdad77b5b 100644 --- a/userland/src/lib.rs +++ b/userland/src/lib.rs @@ -7,6 +7,9 @@ use alloc::borrow::Cow; pub async fn main() -> Result<(), Cow<'static, str>> { awkernel_services::run().await; + #[cfg(feature = "autoware")] + autoware::run().await; // run the autoware application + #[cfg(feature = "rd_gen_to_dags")] rd_gen_to_dags::run().await; // run the rd_gen_to_dags application @@ -46,9 +49,6 @@ pub async fn main() -> Result<(), Cow<'static, str>> { #[cfg(feature = "test_dag")] test_dag::run().await; // test for DAG - #[cfg(feature = "test_autoware")] - test_autoware::run().await; // test for Autoware - #[cfg(feature = "test_dvfs")] test_dvfs::run().await; // test for DVFS From a96a065c685b03083e3e4d0dad812d7b77c41d4a Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:27:13 +0900 Subject: [PATCH 12/20] fix: move autoware position 4 Signed-off-by: nokosaaan --- applications/{tests/test_autoware => autoware}/Cargo.toml | 5 +++-- applications/{tests/test_autoware => autoware}/build.rs | 0 .../test_autoware => autoware}/common_types/Cargo.toml | 0 .../test_autoware => autoware}/common_types/src/lib.rs | 0 .../test_autoware => autoware}/ekf_localizer/Cargo.toml | 0 .../test_autoware => autoware}/ekf_localizer/src/lib.rs | 0 .../test_autoware => autoware}/gyro_odometer/Cargo.toml | 0 .../test_autoware => autoware}/gyro_odometer/src/lib.rs | 0 .../test_autoware => autoware}/imu_corrector/Cargo.toml | 0 .../test_autoware => autoware}/imu_corrector/src/lib.rs | 0 .../{tests/test_autoware => autoware}/imu_driver/Cargo.toml | 0 .../{tests/test_autoware => autoware}/imu_driver/src/lib.rs | 0 applications/{tests/test_autoware => autoware}/src/lib.rs | 0 .../vehicle_velocity_converter/Cargo.toml | 0 .../vehicle_velocity_converter/src/lib.rs | 0 15 files changed, 3 insertions(+), 2 deletions(-) rename applications/{tests/test_autoware => autoware}/Cargo.toml (70%) rename applications/{tests/test_autoware => autoware}/build.rs (100%) rename applications/{tests/test_autoware => autoware}/common_types/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/common_types/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/ekf_localizer/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/ekf_localizer/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/gyro_odometer/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/gyro_odometer/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/imu_corrector/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/imu_corrector/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/imu_driver/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/imu_driver/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/vehicle_velocity_converter/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/vehicle_velocity_converter/src/lib.rs (100%) diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/autoware/Cargo.toml similarity index 70% rename from applications/tests/test_autoware/Cargo.toml rename to applications/autoware/Cargo.toml index fea864309..ff87530c7 100644 --- a/applications/tests/test_autoware/Cargo.toml +++ b/applications/autoware/Cargo.toml @@ -11,8 +11,9 @@ crate-type = ["rlib"] log = "0.4" libm = "0.2" csv-core = "0.1" -awkernel_async_lib = { path = "../../../awkernel_async_lib", default-features = false } -awkernel_lib = { path = "../../../awkernel_lib", default-features = false } +awkernel_async_lib = { path = "../../awkernel_async_lib", default-features = false } +awkernel_lib = { path = "../../awkernel_lib", default-features = false } +common_types = { path = "./common_types", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } diff --git a/applications/tests/test_autoware/build.rs b/applications/autoware/build.rs similarity index 100% rename from applications/tests/test_autoware/build.rs rename to applications/autoware/build.rs diff --git a/applications/tests/test_autoware/common_types/Cargo.toml b/applications/autoware/common_types/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/common_types/Cargo.toml rename to applications/autoware/common_types/Cargo.toml diff --git a/applications/tests/test_autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/common_types/src/lib.rs rename to applications/autoware/common_types/src/lib.rs diff --git a/applications/tests/test_autoware/ekf_localizer/Cargo.toml b/applications/autoware/ekf_localizer/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/ekf_localizer/Cargo.toml rename to applications/autoware/ekf_localizer/Cargo.toml diff --git a/applications/tests/test_autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/ekf_localizer/src/lib.rs rename to applications/autoware/ekf_localizer/src/lib.rs diff --git a/applications/tests/test_autoware/gyro_odometer/Cargo.toml b/applications/autoware/gyro_odometer/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/gyro_odometer/Cargo.toml rename to applications/autoware/gyro_odometer/Cargo.toml diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/gyro_odometer/src/lib.rs rename to applications/autoware/gyro_odometer/src/lib.rs diff --git a/applications/tests/test_autoware/imu_corrector/Cargo.toml b/applications/autoware/imu_corrector/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/imu_corrector/Cargo.toml rename to applications/autoware/imu_corrector/Cargo.toml diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/imu_corrector/src/lib.rs rename to applications/autoware/imu_corrector/src/lib.rs diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/autoware/imu_driver/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/imu_driver/Cargo.toml rename to applications/autoware/imu_driver/Cargo.toml diff --git a/applications/tests/test_autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/imu_driver/src/lib.rs rename to applications/autoware/imu_driver/src/lib.rs diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/autoware/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/src/lib.rs rename to applications/autoware/src/lib.rs diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml b/applications/autoware/vehicle_velocity_converter/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml rename to applications/autoware/vehicle_velocity_converter/Cargo.toml diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs rename to applications/autoware/vehicle_velocity_converter/src/lib.rs From cb25fdc01498e212e2de508a110426db8fec0c30 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:28:30 +0900 Subject: [PATCH 13/20] fix: default feature Signed-off-by: nokosaaan --- userland/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 714d817c5..8d724cb0f 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -79,7 +79,7 @@ path = "../applications/tests/test_voluntary_preemption" optional = true [features] -default = ["test_autoware"] +default = ["autoware"] perf = ["awkernel_services/perf"] # Evaluation applications From e99da019ab80ec035f2b17f47e75eeae4a67c4a0 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:51:15 +0900 Subject: [PATCH 14/20] fix: comment 3 Signed-off-by: nokosaaan --- applications/autoware/gyro_odometer/src/lib.rs | 6 ++++++ applications/autoware/imu_corrector/src/lib.rs | 6 ++++++ applications/autoware/imu_driver/src/lib.rs | 6 ++++++ .../vehicle_velocity_converter/src/lib.rs | 18 +++++------------- 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 6916a16ae..722abb0e3 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index edeb4c134..77058c61a 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// universe/autoware_universe: +// type: git +// url: https://github.com/autowarefoundation/autoware_universe.git +// version: 0.51.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs index 15ef58e24..bdbf28c5a 100644 --- a/applications/autoware/imu_driver/src/lib.rs +++ b/applications/autoware/imu_driver/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// tamagawa_imu_driver +// type: git +// url: https://github.com/tier4/tamagawa_imu_driver +// version: 0.1.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs index 1c6b864d1..42f18c234 100644 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/autoware/vehicle_velocity_converter/src/lib.rs @@ -1,16 +1,8 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 #![no_std] From 9fbaf135473f9e230f82a69103e0194c9ba47791 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:53:04 +0900 Subject: [PATCH 15/20] fix: comment 4 Signed-off-by: nokosaaan --- applications/autoware/ekf_localizer/src/lib.rs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/applications/autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs index e07f1223d..0bc9dab63 100644 --- a/applications/autoware/ekf_localizer/src/lib.rs +++ b/applications/autoware/ekf_localizer/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 + #![no_std] #![allow(non_snake_case)] From d1c287582c27c31bb442c4a6fda5a5e4077120ab Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Wed, 20 May 2026 12:20:27 +0900 Subject: [PATCH 16/20] fix: delete Odometry from vehicle_velocity_converter Signed-off-by: nokosaaan --- .../autoware/gyro_odometer/src/lib.rs | 43 +++---------------- .../vehicle_velocity_converter/src/lib.rs | 5 --- 2 files changed, 6 insertions(+), 42 deletions(-) diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 722abb0e3..64649d37e 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -14,9 +14,7 @@ use core::time::Duration; pub use imu_corrector::{transform_covariance, ImuWithCovariance, Transform}; pub use imu_driver::{Header, ImuMsg, Quaternion, Vector3}; -pub use vehicle_velocity_converter::{ - Odometry, Twist, TwistWithCovariance, TwistWithCovarianceStamped, -}; +pub use vehicle_velocity_converter::{Twist, TwistWithCovariance, TwistWithCovarianceStamped}; static GYRO_ODOMETER_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); @@ -204,26 +202,6 @@ impl GyroOdometerCore { } } - pub fn convert_vehicle_velocity_to_twist( - &self, - odometry: &Odometry, - timestamp: u64, - ) -> TwistWithCovarianceStamped { - TwistWithCovarianceStamped { - header: Header { - frame_id: "base_link", - timestamp, - }, - twist: TwistWithCovariance { - twist: Twist { - linear: Vector3::new(odometry.velocity, 0.0, 0.0), - angular: Vector3::new(0.0, 0.0, 0.0), - }, - covariance: [0.0; 36], - }, - } - } - pub fn add_vehicle_twist(&mut self, twist: TwistWithCovarianceStamped) { self.vehicle_twist_arrived = true; self.vehicle_twist_queue.push_back(twist); @@ -386,24 +364,15 @@ mod tests { #[test] fn test_vehicle_velocity_conversion() { - let config = get_config_with_default_params(); - let core = GyroOdometerCore::new(config).unwrap(); - let sample_twist = generate_sample_velocity(); assert_eq!(sample_twist.header.frame_id, "base_link"); assert_eq!(sample_twist.twist.twist.linear.x, 1.0); - let odometry = Odometry { - velocity: sample_twist.twist.twist.linear.x, - }; - let twist = - core.convert_vehicle_velocity_to_twist(&odometry, sample_twist.header.timestamp); - - assert_eq!(twist.header.frame_id, sample_twist.header.frame_id); - assert_eq!(twist.header.timestamp, 123456789); - assert_eq!(twist.twist.twist.linear.x, 1.0); - assert_eq!(twist.twist.twist.linear.y, 0.0); - assert_eq!(twist.twist.twist.linear.z, 0.0); + // Verify the sample twist structure directly + assert_eq!(sample_twist.header.timestamp, 123456789); + assert_eq!(sample_twist.twist.twist.linear.x, 1.0); + assert_eq!(sample_twist.twist.twist.linear.y, 0.0); + assert_eq!(sample_twist.twist.twist.linear.z, 0.0); } #[test] diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs index 42f18c234..3ea8ddb0a 100644 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/autoware/vehicle_velocity_converter/src/lib.rs @@ -58,11 +58,6 @@ pub struct Twist { pub angular: Vector3, } -#[repr(C)] -pub struct Odometry { - pub velocity: f64, -} - pub struct VehicleVelocityConverter { frame_id: &'static str, stddev_vx: f64, From 2d8d87139d49dd2db4eee59c1f5a0f45d65c6db4 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 4 Jun 2026 23:05:06 +0900 Subject: [PATCH 17/20] fix: resolve merge conflict Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 12 ------------ applications/autoware/src/lib.rs | 7 ------- .../autoware/vehicle_velocity_converter/src/lib.rs | 6 ------ 3 files changed, 25 deletions(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 8f2378798..2953d1cea 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -71,12 +71,6 @@ pub struct MockTransformListener { transforms: alloc::collections::BTreeMap, } -impl Default for MockTransformListener { - fn default() -> Self { - Self::new() - } -} - impl MockTransformListener { pub fn new() -> Self { Self { @@ -177,12 +171,6 @@ pub struct ImuCorrector { transform_listener: T, } -impl Default for ImuCorrector { - fn default() -> Self { - Self::new() - } -} - impl ImuCorrector { pub fn new() -> Self { Self { diff --git a/applications/autoware/src/lib.rs b/applications/autoware/src/lib.rs index 37881b3fb..38928916e 100644 --- a/applications/autoware/src/lib.rs +++ b/applications/autoware/src/lib.rs @@ -444,12 +444,6 @@ fn parse_imu_csv(csv: &str) -> Result, &'static str> { } let timestamp = parse_timestamp(fields[0], fields[1])?; - let orientation = imu_driver::Quaternion { - x: parse_f64(fields[2])?, - y: parse_f64(fields[3])?, - z: parse_f64(fields[4])?, - w: parse_f64(fields[5])?, - }; let angular_velocity = common_types::Vector3::new( parse_f64(fields[6])?, parse_f64(fields[7])?, @@ -463,7 +457,6 @@ fn parse_imu_csv(csv: &str) -> Result, &'static str> { rows.push(ImuCsvRow { timestamp, - orientation, angular_velocity, linear_acceleration, }); diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs index 0c6d6f164..d3d65556b 100644 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/autoware/vehicle_velocity_converter/src/lib.rs @@ -81,12 +81,6 @@ pub struct VehicleVelocityConverter { speed_scale_factor: f64, } -impl Default for VehicleVelocityConverter { - fn default() -> Self { - Self::new("base_link", 0.2, 0.1, 1.0) - } -} - impl VehicleVelocityConverter { pub fn new( frame_id: &'static str, From 428d3e2bf7aba9809c35d722f1d1b722a4fc48c8 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 1 Aug 2026 10:30:45 +0900 Subject: [PATCH 18/20] fix: ignore log file 2 Signed-off-by: nokosaaan --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index b2771824f..e3294ce99 100644 --- a/.gitignore +++ b/.gitignore @@ -42,6 +42,7 @@ *.su *.idb *.pdb +log/ # Kernel Module Compile Results *.mod* From 712f7dd2f758ce8019449cbca2888f4b3dbd622d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 1 Aug 2026 13:48:36 +0900 Subject: [PATCH 19/20] feat: DAG FasS connection with KVM option Signed-off-by: nokosaaan --- Makefile | 15 +- .../autoware/gyro_odometer/src/lib.rs | 14 +- applications/autoware/src/lib.rs | 162 ++++++++++-------- 3 files changed, 116 insertions(+), 75 deletions(-) diff --git a/Makefile b/Makefile index 0f95beb9c..fa4dc1e4a 100644 --- a/Makefile +++ b/Makefile @@ -6,6 +6,15 @@ else OPT = --features debug endif +# KVM=1 to accelerate x86_64 QEMU with KVM (requires /dev/kvm access on the host). +# Off by default since not every environment (CI, nested virtualization without KVM +# passthrough, non-Linux hosts) has it available. +ifeq ($(KVM), 1) + QEMU_KVM_ARGS = -enable-kvm -cpu host +else + QEMU_KVM_ARGS = +endif + # 2MiB Stack STACKSIZE = 1024 * 1024 * 2 @@ -143,7 +152,7 @@ check_x86_64: $(X86ASM) cargo +$(RUSTV) check_x86 kernel-x86_64.elf: $(X86ASM) FORCE - RUSTFLAGS="$(RUSTC_MISC_ARGS)" cargo +$(RUSTV) x86 $(OPT) + RUSTFLAGS="$(RUSTC_MISC_ARGS)" IMU_CSV_PATH=$(IMU_CSV_PATH) VELOCITY_CSV_PATH=$(VELOCITY_CSV_PATH) cargo +$(RUSTV) x86 $(OPT) python3 scripts/embed_debug_info.py $@ x86_64_boot.img: kernel-x86_64.elf @@ -157,10 +166,14 @@ $(X86ASM): FORCE OVMF_PATH := $(shell cat ${HOME}/.ovmfpath) +IMU_CSV_PATH ?= $(CURDIR)/../awkernel_script/sensor_inputs/imu_raw.csv +VELOCITY_CSV_PATH ?= $(CURDIR)/../awkernel_script/sensor_inputs/velocity_status.csv + QEMU_X86_ARGS= -drive if=pflash,format=raw,readonly=on,file=${OVMF_PATH}/code.fd QEMU_X86_ARGS+= -drive if=pflash,format=raw,file=${OVMF_PATH}/vars_qemu.fd QEMU_X86_ARGS+= -drive format=raw,file=x86_64_uefi.img QEMU_X86_ARGS+= -machine q35 +QEMU_X86_ARGS+= $(QEMU_KVM_ARGS) QEMU_X86_ARGS+= -serial stdio -smp 4 -monitor telnet::5556,server,nowait QEMU_X86_ARGS+= -m 4G -smp cpus=16 QEMU_X86_ARGS+= -object memory-backend-ram,size=1G,id=m0 diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 99ef549ea..95a70669e 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -45,7 +45,7 @@ const COV_IDX_XYZRPY_PITCH_PITCH: usize = 28; const COV_IDX_XYZRPY_YAW_YAW: usize = 35; pub struct GyroOdometerCore { - pub output_frame: String, + pub output_frame: &'static str, pub message_timeout_sec: f64, pub vehicle_twist_arrived: bool, pub imu_arrived: bool, @@ -59,7 +59,7 @@ pub struct GyroOdometerCore { impl GyroOdometerCore { pub fn new(config: GyroOdometerConfig) -> Result { let queue_size = config.queue_size; - let output_frame = config.output_frame.clone(); + let output_frame = config.output_frame; let message_timeout_sec = config.message_timeout_sec; Ok(Self { @@ -119,7 +119,7 @@ impl GyroOdometerCore { let tf = self.get_transform( self.gyro_queue.front().unwrap().header.frame_id, - &self.output_frame, + self.output_frame, )?; // In the original C++ implementation, angular_velocity_covariance is also transformed @@ -168,7 +168,7 @@ impl GyroOdometerCore { let mut result = TwistWithCovarianceStamped { header: Header { - frame_id: self.gyro_queue.front().unwrap().header.frame_id, + frame_id: self.output_frame, timestamp: result_timestamp, }, twist: TwistWithCovariance { @@ -301,7 +301,7 @@ type Result = core::result::Result; #[derive(Debug, Clone)] pub struct GyroOdometerConfig { - pub output_frame: String, + pub output_frame: &'static str, pub message_timeout_sec: f64, pub queue_size: usize, pub transform_timeout: Duration, @@ -312,7 +312,7 @@ pub struct GyroOdometerConfig { impl Default for GyroOdometerConfig { fn default() -> Self { Self { - output_frame: String::from("base_link"), + output_frame: "base_link", message_timeout_sec: 1.0, queue_size: 100, transform_timeout: Duration::from_secs(1), @@ -356,7 +356,7 @@ mod tests { fn get_config_with_default_params() -> GyroOdometerConfig { GyroOdometerConfig { - output_frame: String::from("base_link"), + output_frame: "base_link", message_timeout_sec: 1e12, ..GyroOdometerConfig::default() } diff --git a/applications/autoware/src/lib.rs b/applications/autoware/src/lib.rs index 38928916e..71b825724 100644 --- a/applications/autoware/src/lib.rs +++ b/applications/autoware/src/lib.rs @@ -3,22 +3,24 @@ extern crate alloc; use alloc::{borrow::Cow, format, string::String, vec, vec::Vec}; +use awkernel_async_lib::channel::bounded; use awkernel_async_lib::dag::{create_dag, finish_create_dags}; use awkernel_async_lib::net::IpAddr; +use awkernel_async_lib::pubsub::Lifespan; use awkernel_async_lib::scheduler::SchedulerType; use awkernel_lib::delay::wait_microsec; use awkernel_lib::sync::mutex::{MCSNode, Mutex}; use core::net::Ipv4Addr; -use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; use core::time::Duration; use csv_core::{ReadRecordResult, Reader}; pub use common_types::Header; use core::slice; use ekf_localizer::{ - get_or_initialize_default_module, EKFOdometry, Point3D, Pose, PoseWithCovariance, Quaternion, + apply_twist_observability_gate, get_or_initialize_default_module, EKFOdometry, EKFParameters, + Point3D, Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion, }; -use imu_corrector::{ImuCorrector, ImuWithCovariance}; +use imu_corrector::{ImuCorrector, ImuWithCovariance, Transform}; use imu_driver::{build_imu_msg_from_csv_row, ImuCsvRow, ImuMsg, TamagawaImuParser}; use vehicle_velocity_converter::{ build_velocity_report_from_csv_row, reactor_helpers, Twist, TwistWithCovariance, @@ -32,10 +34,6 @@ const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 64); const UDP_TCP_DST_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 2); const UDP_DST_PORT: u16 = 26099; -static mut LATEST_JSON_DATA: Option = None; -static JSON_DATA_READY: AtomicBool = AtomicBool::new(false); -static JSON_DATA_LENGTH: AtomicUsize = AtomicUsize::new(0); - include!(concat!(env!("OUT_DIR"), "/csv_data.rs")); static mut IMU_CSV_DATA: Option> = None; @@ -52,6 +50,18 @@ pub async fn run() { log::info!("Starting Autoware test application with simplified TCP networking"); + // Latest-value handoff from the (synchronous) sink reactor to the independent async UDP + // sender task. `queue_size: 1` + `flow_control: false` makes `try_send` overwrite the + // previous (not-yet-sent) entry instead of blocking or erroring -- matching the "only the + // newest odometry matters, dropping intermediate ticks is fine" semantics this bridge needs. + // This replaces the earlier unsynchronized `static mut` handoff (a genuine data race, since + // the sink reactor and the UDP task can run concurrently on different cores). + let (json_sender, json_receiver) = bounded::new::(bounded::Attribute { + queue_size: 1, + flow_control: false, + lifespan: Lifespan::Permanent, + }); + let dag = create_dag(); let _dag_id = dag.get_id(); @@ -268,35 +278,74 @@ pub async fn run() { dag.register_reactor::<_, (Pose, TwistWithCovarianceStamped), (Pose, EKFOdometry)>( "ekf_localizer".into(), - |(pose, twist): (Pose, TwistWithCovarianceStamped)| -> (Pose, EKFOdometry) { + |(pose, mut twist): (Pose, TwistWithCovarianceStamped)| -> (Pose, EKFOdometry) { let ekf = get_or_initialize_default_module(); + let current_time = get_awkernel_uptime_timestamp(); + + // `pose_dummy_generator` has no real localization source behind it yet (an NDT + // scan matcher port is out of scope here), so this covariance is a small fixed + // placeholder, not a calibrated sensor value. + let mut pose_covariance = [0.0; 36]; + pose_covariance[0] = 0.01; // X_X + pose_covariance[7] = 0.01; // Y_Y + pose_covariance[14] = 0.01; // Z_Z + pose_covariance[21] = 0.01; // ROLL_ROLL + pose_covariance[28] = 0.01; // PITCH_PITCH + pose_covariance[35] = 0.01; // YAW_YAW + + let pose_stamped = PoseWithCovarianceStamped { + header: common_types::Header { + frame_id: "map", + timestamp: current_time, + }, + pose: PoseWithCovariance { + pose, + covariance: pose_covariance, + }, + }; + // Seeded once from the first pose, matching upstream's node-layer `initialize()` + // call; every later pose feeds `measurement_update_pose` instead. No TF listener + // is wired in yet, so the transform is the identity (see + // `EKFModule::initialize`'s doc comment). static mut INITIALIZED: bool = false; - unsafe { - if !INITIALIZED { - ekf.initialize(pose); + let just_initialized = unsafe { + if INITIALIZED { + false + } else { + ekf.initialize(&pose_stamped, &Transform::identity()); INITIALIZED = true; + true } - } + }; - // Use a fixed 50ms timestep to match the Autoware publisher cadence. + // Fixed 50ms timestep to match the periodic "start_dummy_data" trigger cadence. + // Both calls are required every predict tick -- `predict_with_delay` does not + // age the delay-time buffer itself (see its doc comment). const FIXED_DT: f64 = 0.05; - let dt = FIXED_DT; + ekf.accumulate_delay_time(FIXED_DT); + ekf.predict_with_delay(FIXED_DT); - if dt > 0.0 { - ekf.predict(dt); + if !just_initialized { + ekf.measurement_update_pose(&pose_stamped, current_time); } - let vx = twist.twist.twist.linear.x; - let wz = twist.twist.twist.angular.z; - ekf.update_velocity(vx, wz); + // Below `threshold_observable_velocity_mps` (0.0 by default, matching upstream's + // shipped config), vx is inflated as unreliable before fusing; wiring the gate + // unconditionally here matches upstream's `callback_twist_with_covariance`, even + // though the default threshold makes it a no-op today. + apply_twist_observability_gate( + &mut twist, + EKFParameters::default().threshold_observable_velocity_mps, + ); + ekf.measurement_update_twist(&twist, current_time); - let ekf_pose = ekf.get_current_pose(false); + let ekf_pose = ekf.get_current_pose(false, current_time); let pose_covariance = ekf.get_current_pose_covariance(); let twist_covariance = ekf.get_current_twist_covariance(); - let ekf_twist = ekf.get_current_twist(); + let ekf_twist = ekf.get_current_twist(current_time); let odometry = EKFOdometry { header: common_types::Header { @@ -305,27 +354,27 @@ pub async fn run() { }, child_frame_id: "base_link", pose: PoseWithCovariance { - pose: ekf_pose, + pose: ekf_pose.pose, covariance: pose_covariance, }, twist: TwistWithCovariance { twist: Twist { linear: common_types::Vector3::new( - ekf_twist.linear.x, - ekf_twist.linear.y, - ekf_twist.linear.z, + ekf_twist.twist.linear.x, + ekf_twist.twist.linear.y, + ekf_twist.twist.linear.z, ), angular: common_types::Vector3::new( - ekf_twist.angular.x, - ekf_twist.angular.y, - ekf_twist.angular.z, + ekf_twist.twist.angular.x, + ekf_twist.twist.angular.y, + ekf_twist.twist.angular.z, ), }, covariance: twist_covariance, }, }; - (ekf_pose, odometry) + (ekf_pose.pose, odometry) }, vec![Cow::from("dummy_pose"), Cow::from("twist_with_covariance")], vec![Cow::from("estimated_pose"), Cow::from("ekf_odometry")], @@ -359,7 +408,9 @@ pub async fn run() { ekf_odom.twist.covariance.iter().map(|&x| format!("{:.6}", x)).collect::>().join(",") ); - save_json_data_to_global(json_data); + if let Err(e) = json_sender.try_send(json_data) { + log::warn!("ekf_sink: failed to hand off JSON data to the UDP sender: {e:?}"); + } }, vec![Cow::from("estimated_pose"), Cow::from("ekf_odometry")], SchedulerType::GEDF(5), @@ -398,21 +449,7 @@ pub async fn run() { awkernel_async_lib::sleep(Duration::from_secs(2)).await; log::info!("Starting periodic UDP sender task"); - start_periodic_udp_sender().await; - - log::info!("Waiting for JSON data to become ready..."); - let mut wait_count = 0; - const MAX_WAIT_COUNT: u32 = 3; - - while !JSON_DATA_READY.load(Ordering::Relaxed) && wait_count < MAX_WAIT_COUNT { - awkernel_async_lib::sleep(Duration::from_secs(1)).await; - wait_count += 1; - } - - if JSON_DATA_READY.load(Ordering::Relaxed) { - } else { - log::warn!("JSON data was not ready. Periodic UDP sender task remains waiting"); - } + start_periodic_udp_sender(json_receiver).await; log::info!("Autoware test application completed"); } @@ -575,16 +612,16 @@ fn get_awkernel_uptime_timestamp() -> u64 { } } -pub async fn start_periodic_udp_sender() { +pub async fn start_periodic_udp_sender(receiver: bounded::Receiver) { awkernel_async_lib::spawn( "periodic_udp_sender".into(), - periodic_udp_sender_task(), + periodic_udp_sender_task(receiver), awkernel_async_lib::scheduler::SchedulerType::GEDF(5), ) .await; } -async fn periodic_udp_sender_task() { +async fn periodic_udp_sender_task(receiver: bounded::Receiver) { let socket_result = awkernel_async_lib::net::udp::UdpSocket::bind_on_interface( INTERFACE_ID, &Default::default(), @@ -605,15 +642,8 @@ async fn periodic_udp_sender_task() { let mut counter = 0; loop { - if !JSON_DATA_READY.load(Ordering::Relaxed) { - awkernel_async_lib::sleep(Duration::from_secs(1)).await; - continue; - } - - let json_data = unsafe { LATEST_JSON_DATA.clone() }; - - if let Some(data) = json_data { - match socket.send(data.as_bytes(), &dst_addr, UDP_DST_PORT).await { + match receiver.try_recv() { + Ok(data) => match socket.send(data.as_bytes(), &dst_addr, UDP_DST_PORT).await { Ok(_) => { counter += 1; log::info!( @@ -642,19 +672,17 @@ async fn periodic_udp_sender_task() { Err(e) => { log::warn!("Periodic UDP sender task: send failed: {:?}", e); } + }, + // Normal in steady state: `ekf_sink` only ticks every ~50ms, so the channel is + // frequently empty between sends. Fall through to the same 5ms poll cadence as the + // success path instead of a long sleep, so a fresh value is picked up promptly. + Err(bounded::RecvErr::NoData) => {} + Err(bounded::RecvErr::ChannelClosed) => { + log::warn!("Periodic UDP sender task: channel closed, stopping"); + return; } - } else { - log::warn!("Periodic UDP sender task: failed to get JSON data"); } awkernel_async_lib::sleep(Duration::from_millis(5)).await; } } - -fn save_json_data_to_global(json_data: String) { - unsafe { - LATEST_JSON_DATA = Some(json_data.clone()); - } - JSON_DATA_READY.store(true, Ordering::Relaxed); - JSON_DATA_LENGTH.store(json_data.len(), Ordering::Relaxed); -} From 3620756804255b51b4528f092005f4848059640a Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Sat, 1 Aug 2026 14:04:58 +0900 Subject: [PATCH 20/20] fix: cargo clippy_aarch64_virt Signed-off-by: nokosaaan --- .../autoware/ekf_localizer/src/lib.rs | 2 +- .../autoware/gyro_odometer/src/lib.rs | 23 ++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/applications/autoware/ekf_localizer/src/lib.rs b/applications/autoware/ekf_localizer/src/lib.rs index 1dd223a1d..80a7d2478 100644 --- a/applications/autoware/ekf_localizer/src/lib.rs +++ b/applications/autoware/ekf_localizer/src/lib.rs @@ -246,7 +246,7 @@ impl Simple1DFilter { let kalman_gain = self.var / (self.var + obs_var); self.x += kalman_gain * (obs - self.x); - self.var = (1.0 - kalman_gain) * self.var; + self.var *= 1.0 - kalman_gain; } pub fn set_proc_var(&mut self, proc_var: f64) { diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 95a70669e..97085a422 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -201,15 +201,22 @@ impl GyroOdometerCore { let dt = (current_timestamp as f64 - last_timestamp as f64) / 1_000_000_000.0; dt.abs() > timeout_sec } - pub fn get_transform(&self, from_frame: &str, to_frame: &str) -> Result { + pub fn get_transform(&self, _from_frame: &str, _to_frame: &str) -> Result { + // This exists to rotate the raw gyro's angular velocity out of the IMU's own mounting + // orientation (`from_frame`, e.g. `imu_link`) and into the vehicle body frame + // (`to_frame`, `output_frame`/"base_link"). If the IMU isn't mounted perfectly aligned + // with the vehicle body, its raw yaw-axis reading is not exactly the vehicle's own yaw + // rate -- some of it leaks in from roll/pitch, and vice versa. `measurement_update_twist` + // downstream assumes `wz` already IS the vehicle's base_link yaw rate, so any + // uncorrected mounting misalignment biases the fused estimate. + // // In the original implementation, a TF lookup failure should clear the queues and - // terminate processing early. This port currently returns identity because the - // evaluation setup uses a fixed identity transform. - if from_frame == to_frame || from_frame == "" || to_frame == "" { - Ok(Transform::identity()) - } else { - Ok(Transform::identity()) - } + // terminate processing early. This port currently always returns identity because the + // evaluation setup uses a fixed identity transform (IMU assumed perfectly aligned with + // base_link); there is no real TF lookup yet -- a real one would query calibrated sensor + // extrinsics (typically a fixed static offset for an IMU-to-base_link mount) instead of + // hardcoding identity here. + Ok(Transform::identity()) } // The original C++ node publishes four topics: raw TwistStamped, raw TwistWithCovarianceStamped,