From 82589e6e7c211398a4ea9bede90dcb188161b69e Mon Sep 17 00:00:00 2001 From: XiaoPengYouCode <120481176+XiaoPengYouCode@users.noreply.github.com> Date: Tue, 16 Jun 2026 11:32:49 +0800 Subject: [PATCH] Add offline energy mechanism observability --- app/auto_aim_async/src/main.rs | 2 + app/auto_aim_async/src/rbt_threads.rs | 169 +++++++++++++++++- cfg/rbt_cfg.toml | 3 +- lib/src/rbt_infra/rbt_cfg.rs | 24 +++ .../rbt_mod/rbt_energy_mechanism/detected.rs | 46 ++++- 5 files changed, 230 insertions(+), 14 deletions(-) diff --git a/app/auto_aim_async/src/main.rs b/app/auto_aim_async/src/main.rs index 4c5ce6a..9c76baf 100644 --- a/app/auto_aim_async/src/main.rs +++ b/app/auto_aim_async/src/main.rs @@ -123,6 +123,7 @@ async fn main() -> RbtResult<()> { let pre_task_handler = pre_process( pre_infer_queue.clone(), energy_pre_infer_queue.clone(), + feedback_queue.clone(), cfg.detector_cfg.clone(), runtime_router.clone(), runtime_completion.clone(), @@ -159,6 +160,7 @@ async fn main() -> RbtResult<()> { let energy_estimate_task_handler = energy_mechanism_estimate_process( energy_solved_queue.clone(), energy_track_queue.clone(), + rec.clone(), runtime_router.clone(), runtime_completion.clone(), ); diff --git a/app/auto_aim_async/src/rbt_threads.rs b/app/auto_aim_async/src/rbt_threads.rs index 1d05602..22e0a4d 100644 --- a/app/auto_aim_async/src/rbt_threads.rs +++ b/app/auto_aim_async/src/rbt_threads.rs @@ -52,7 +52,7 @@ use lib::{ }, }; -const DEFAULT_VIDEO_FILE: &str = "offline_capture_bundle_outpost_rot180.avi"; +const DEFAULT_VIDEO_FILE: &str = "offline_capture_bundle.avi"; const RAW_RGB_CHANNELS: usize = 3; const FIRE_CONTROL_SNAPSHOT_STALE_MS: f64 = 180.0; const CONTROL_STATUS_LOG_PERIOD_TICKS: u64 = 50; @@ -64,6 +64,9 @@ const RERUN_FILTER_FILTERED_CENTER_COLOR: u32 = 0x14DC78FF; const RERUN_FILTER_FILTERED_ARMOR_COLOR: u32 = 0x288CFFFF; const RERUN_FILTER_VELOCITY_COLOR: u32 = 0xFFFFFFFF; const RERUN_FILTER_VELOCITY_ARROW_SCALE_S: f64 = 0.2; +const RERUN_ENERGY_CENTER_COLOR: u32 = 0xF59E0BFF; +const RERUN_ENERGY_TARGET_CENTER_COLOR: u32 = 0x10B981FF; +const RERUN_ENERGY_PREDICT_COLOR: u32 = 0x38BDF8FF; #[derive(Debug, Clone)] pub struct PlannerTrackSnapshot { @@ -461,6 +464,109 @@ fn log_rerun_filter_snapshot( Ok(()) } +fn energy_position_mm(x: f64, y: f64, z: f64) -> [f32; 3] { + [ + (x * 1_000.0) as f32, + (y * 1_000.0) as f32, + (z * 1_000.0) as f32, + ] +} + +fn log_rerun_energy_mechanism_snapshot( + rec: &rr::RecordingStream, + seq: u64, + target: Option, + center_visible: &mut bool, + blade_center_visible: &mut bool, + horizon_visible: &mut bool, +) -> Result<(), rr::RecordingStreamError> { + if !rec.is_enabled() { + return Ok(()); + } + + rec.set_time_sequence("energy_estimate_seq", seq as i64); + + if let Some(snapshot) = target { + let center = energy_position_mm( + snapshot.rune_center_world_m.x, + snapshot.rune_center_world_m.y, + snapshot.rune_center_world_m.z, + ); + let target_center = energy_position_mm( + snapshot.target_center_world_m.x, + snapshot.target_center_world_m.y, + snapshot.target_center_world_m.z, + ); + let predict_horizon = snapshot + .predict_target_horizon(&[0.02, 0.04, 0.06, 0.08, 0.10]) + .into_iter() + .map(|point| energy_position_mm(point.x, point.y, point.z)) + .collect::>(); + + rec.log( + "world/energy_mechanism/center", + &rr::Points3D::new([center]) + .with_colors([RERUN_ENERGY_CENTER_COLOR]) + .with_radii([26.0]), + )?; + rec.log( + "world/energy_mechanism/blade_center", + &rr::Points3D::new([target_center]) + .with_colors([RERUN_ENERGY_TARGET_CENTER_COLOR]) + .with_radii([22.0]), + )?; + rec.log( + "world/energy_mechanism/predict_horizon", + &rr::LineStrips3D::new([predict_horizon]).with_colors([RERUN_ENERGY_PREDICT_COLOR]), + )?; + + rec.log( + "time/energy_mechanism/roll_rad", + &rr::Scalars::new([snapshot.roll_rad]), + )?; + rec.log( + "time/energy_mechanism/roll_rate_rad_s", + &rr::Scalars::new([snapshot.roll_rate_rad_s]), + )?; + rec.log( + "time/energy_mechanism/track_valid", + &rr::Scalars::new([if snapshot.track_valid { 1.0 } else { 0.0 }]), + )?; + rec.log( + "time/energy_mechanism/lost", + &rr::Scalars::new([if snapshot.lost { 1.0 } else { 0.0 }]), + )?; + + *center_visible = true; + *blade_center_visible = true; + *horizon_visible = true; + } else { + if *center_visible { + rec.log( + "world/energy_mechanism/center", + &rr::Points3D::new([] as [[f32; 3]; 0]), + )?; + *center_visible = false; + } + if *blade_center_visible { + rec.log( + "world/energy_mechanism/blade_center", + &rr::Points3D::new([] as [[f32; 3]; 0]), + )?; + *blade_center_visible = false; + } + if *horizon_visible { + rec.log( + "world/energy_mechanism/predict_horizon", + &rr::LineStrips3D::new([] as [Vec<[f32; 3]>; 0]), + )?; + *horizon_visible = false; + } + } + + Ok(()) +} + pub fn video_input_path() -> PathBuf { Path::new(env!("CARGO_MANIFEST_DIR")) .join("..") @@ -469,8 +575,7 @@ pub fn video_input_path() -> PathBuf { .join(DEFAULT_VIDEO_FILE) } -fn default_feedback(bullet_speed_mps: f64) -> SensData { - let task_mode = TaskMode::AutoShot; +fn default_feedback(bullet_speed_mps: f64, task_mode: TaskMode) -> SensData { SensData { task_mode, self_fraction: SelfFraction::Blue, @@ -493,6 +598,7 @@ fn default_feedback(bullet_speed_mps: f64) -> SensData { pub fn pre_process( queue: Arc>, energy_queue: Arc>, + feedback_queue: Arc>, detector_cfg: DetectorCfg, runtime_router: RuntimeRouter, completion: RuntimePipelineCompletion, @@ -500,7 +606,13 @@ pub fn pre_process( tokio::spawn(async move { let completion_guard = completion.clone(); let result = tokio::task::spawn_blocking(move || { - run_video_preprocess_loop(queue, energy_queue, detector_cfg, runtime_router) + run_video_preprocess_loop( + queue, + energy_queue, + feedback_queue, + detector_cfg, + runtime_router, + ) }) .await; @@ -531,6 +643,7 @@ pub fn pre_process( fn run_video_preprocess_loop( queue: Arc>, energy_queue: Arc>, + feedback_queue: Arc>, _detector_cfg: DetectorCfg, runtime_router: RuntimeRouter, ) -> Result { @@ -547,6 +660,13 @@ fn run_video_preprocess_loop( reader.height ); + let cfg = GENERIC_RBT_CFG.read().unwrap().clone(); + let offline_feedback = default_feedback( + cfg.general_cfg.bullet_speed, + cfg.general_cfg.offline_task_mode(), + ); + feedback_queue.push_latest(offline_feedback); + loop { if !IS_RUNNING.load(Ordering::SeqCst) { info!("pre_process: stopping video source as IS_RUNNING is false"); @@ -558,6 +678,8 @@ fn run_video_preprocess_loop( break; }; summary.read_total += read_started.elapsed(); + feedback_queue.push_latest(offline_feedback); + let frame_img = frame_img.rotate180(); let route_state = runtime_router.state(); if route_state.armor_pipeline_active() { @@ -1194,6 +1316,7 @@ pub fn energy_mechanism_post_process( pub fn energy_mechanism_estimate_process( solved_queue: Arc>, track_queue: Arc>, + rec: rr::RecordingStream, runtime_router: RuntimeRouter, completion: RuntimePipelineCompletion, ) -> JoinHandle<()> { @@ -1208,6 +1331,9 @@ pub fn energy_mechanism_estimate_process( let mut tracker = EnergyMechanismTracker::from_tracker_cfg(EnergyMechanismMode::Small, &tracker_cfg); let mut snapshot_seq = 0_u64; + let mut center_visible = false; + let mut blade_center_visible = false; + let mut horizon_visible = false; let mut last_transition_seq = runtime_router.state().transition_seq; loop { ticker.tick().await; @@ -1239,6 +1365,16 @@ pub fn energy_mechanism_estimate_process( }); snapshot_seq = snapshot_seq.wrapping_add(1); let target = tracker.update(mode, solved.target.as_ref()); + if let Err(err) = log_rerun_energy_mechanism_snapshot( + &rec, + snapshot_seq, + target, + &mut center_visible, + &mut blade_center_visible, + &mut horizon_visible, + ) { + warn!("energy_mechanism_estimate_process: failed to log rerun snapshot: {err}"); + } track_queue.push_latest(EnergyMechanismTrackPacket { seq: snapshot_seq, target, @@ -1386,9 +1522,17 @@ pub fn control_loop_250hz( latest_feedback .as_ref() .map(|(feedback, _)| *feedback) - .unwrap_or_else(|| default_feedback(cfg.general_cfg.bullet_speed)) + .unwrap_or_else(|| { + default_feedback( + cfg.general_cfg.bullet_speed, + cfg.general_cfg.offline_task_mode(), + ) + }) } else { - default_feedback(cfg.general_cfg.bullet_speed) + default_feedback( + cfg.general_cfg.bullet_speed, + cfg.general_cfg.offline_task_mode(), + ) }; if route_state.energy_mechanism_active() { @@ -1642,7 +1786,7 @@ mod tests { #[test] fn default_feedback_disables_mcu_fire_permit() { - let feedback = default_feedback(24.0); + let feedback = default_feedback(24.0, TaskMode::AutoShot); assert_eq!(feedback.gimbal_yaw, 0.0); assert_eq!(feedback.gimbal_pitch, 0.0); @@ -1650,9 +1794,18 @@ mod tests { assert_eq!(feedback.task_mode, TaskMode::AutoShot); } + #[test] + fn default_feedback_can_override_task_mode_for_offline_simulation() { + let feedback = default_feedback(24.0, TaskMode::HitBigBuff); + + assert_eq!(feedback.task_mode, TaskMode::HitBigBuff); + assert_eq!(feedback.raw_task_mode, TaskMode::HitBigBuff as u8); + assert_eq!(feedback.mapped_task_mode, TaskMode::HitBigBuff); + } + #[test] fn route_disabled_control_keeps_gimbal_and_stops_fire() { - let mut feedback = default_feedback(24.0); + let mut feedback = default_feedback(24.0, TaskMode::AutoShot); feedback.gimbal_yaw = 12.5; feedback.gimbal_pitch = -3.0; diff --git a/cfg/rbt_cfg.toml b/cfg/rbt_cfg.toml index b1d8b2c..eb3ec43 100644 --- a/cfg/rbt_cfg.toml +++ b/cfg/rbt_cfg.toml @@ -17,7 +17,8 @@ file_log_enable = true img_dbg = false bullet_speed = 24.0 can_interface = "can0" -can_enabled = true +can_enabled = false +offline_task_mode = "HitBigBuff" [detector_cfg] # 神经网络模型分别放在 model/armor 和 model/engine_mechanism 目录下 diff --git a/lib/src/rbt_infra/rbt_cfg.rs b/lib/src/rbt_infra/rbt_cfg.rs index 4a285a0..c04c4f4 100644 --- a/lib/src/rbt_infra/rbt_cfg.rs +++ b/lib/src/rbt_infra/rbt_cfg.rs @@ -3,6 +3,7 @@ use std::path::Path; use crate::rbt_bail_error; use crate::rbt_infra::rbt_err::{RbtError, RbtResult}; +use crate::rbt_mod::rbt_comm::rbt_comm_frame::TaskMode; use crate::rbt_mod::rbt_estimator::rbt_enemy_dynamic_model::EnemyFaction; #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] @@ -54,6 +55,8 @@ pub struct GeneralCfg { pub can_interface: String, #[serde(default = "default_can_enabled")] pub can_enabled: bool, + #[serde(default = "default_offline_task_mode")] + pub offline_task_mode: String, } fn default_can_interface() -> String { @@ -64,6 +67,27 @@ fn default_can_enabled() -> bool { true } +fn default_offline_task_mode() -> String { + "AutoShot".to_string() +} + +impl GeneralCfg { + pub fn offline_task_mode(&self) -> TaskMode { + match self.offline_task_mode.trim() { + "AutoShot" => TaskMode::AutoShot, + "HitBigBuff" => TaskMode::HitBigBuff, + "HitSmallBuff" => TaskMode::HitSmallBuff, + "HitOutpost" => TaskMode::HitOutpost, + invalid => { + eprintln!( + "请检查 general_cfg/offline_task_mode 设置，当前值 `{invalid}` 无效，回退到 AutoShot" + ); + TaskMode::AutoShot + } + } + } +} + // 检测器相关配置 #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] pub struct DetectorCfg { diff --git a/lib/src/rbt_mod/rbt_energy_mechanism/detected.rs b/lib/src/rbt_mod/rbt_energy_mechanism/detected.rs index aaffff8..8216551 100644 --- a/lib/src/rbt_mod/rbt_energy_mechanism/detected.rs +++ b/lib/src/rbt_mod/rbt_energy_mechanism/detected.rs @@ -41,8 +41,8 @@ pub enum EnergyMechanismClass { impl EnergyMechanismClass { fn from_index(index: usize, class_count: usize) -> Option { match (class_count, index) { - (2, 0) => Some(Self::Target), - (2, 1) => Some(Self::CenterMarker), + (2, 0) => Some(Self::RedTarget), + (2, 1) => Some(Self::BlueTarget), (4, 0) => Some(Self::RedTarget), (4, 1) => Some(Self::RedHit), (4, 2) => Some(Self::BlueTarget), @@ -477,8 +477,8 @@ mod tests { } #[test] - fn decode_keeps_two_class_target_with_keypoint_dim_three() { - let mut output = nd::Array2::::zeros((21, 2)); + fn decode_keeps_two_class_color_target_with_keypoint_dim_three() { + let mut output = nd::Array2::::zeros((21, 1)); output[[0, 0]] = 320.0; output[[1, 0]] = 320.0; output[[2, 0]] = 100.0; @@ -503,7 +503,7 @@ mod tests { ); assert_eq!(objects.len(), 1); - assert_eq!(objects[0].class, EnergyMechanismClass::Target); + assert_eq!(objects[0].class, EnergyMechanismClass::RedTarget); assert_eq!(stats.nms_kept, 1); } @@ -540,4 +540,40 @@ mod tests { assert_eq!(stats.confidence_pass, 2); assert_eq!(stats.target_pass, 1); } + + #[test] + fn decode_two_class_target_respects_self_color_filter() { + let mut output = nd::Array2::::zeros((21, 2)); + for anchor in 0..2 { + output[[0, anchor]] = 320.0; + output[[1, anchor]] = 320.0; + output[[2, anchor]] = 100.0; + output[[3, anchor]] = 80.0; + for idx in 0..ENERGY_MECHANISM_KEYPOINTS { + let base = 6 + idx * 3; + output[[base, anchor]] = 300.0 + idx as f32; + output[[base + 1, anchor]] = 310.0 + idx as f32; + output[[base + 2, anchor]] = 1.0; + } + } + output[[4, 0]] = 0.92; // Red target, should be filtered when self is red. + output[[5, 1]] = 0.95; // Blue target, should be kept. + let cfg = EnergyMechanismYoloPostprocessCfg { + confidence_threshold: 0.5, + nms_iou_threshold: 0.4, + self_fraction: Some(EnemyFaction::R), + }; + + let (objects, stats) = decode_energy_mechanism_output_with_stats( + &output.view(), + LetterboxTransform::default(), + &cfg, + ); + + assert_eq!(objects.len(), 1); + assert_eq!(objects[0].class, EnergyMechanismClass::BlueTarget); + assert_eq!(stats.confidence_pass, 2); + assert_eq!(stats.target_pass, 2); + assert_eq!(stats.self_color_pass, 1); + } }