Fix field mark association detected objects type (#2786)

* Fix field mark association detected objects type

* FIx TimeWrapper in robot viewer

* Decomplexify type

Author: alexschmander

crates/nodes/field_mark_association/src/frame_processing.rs

@@ -21,6 +21,9 @@ use crate::{
 
 const MAX_CAMERA_MATRIX_TIME_DISTANCE: Duration = Duration::from_millis(100);
 
+type DetectedObjects = TimeWrapper<Vec<Object<RobocupObjectLabel>>>;
+type DetectedObjectsItem<'a> = FutureItem<'a, (Option<DetectedObjects>,)>;
+
 pub(crate) struct DetectionProcessingContext<'a> {
     pub(crate) parameters: &'a NodeParameters<FieldMarkAssociationParameters>,
     pub(crate) camera_matrix_cache: &'a Cache<TimeWrapper<CameraMatrix>>,
@@ -50,7 +53,7 @@ struct ProcessedDetectionFrame {
 }
 
 pub(crate) async fn process_detected_objects(
-    item: FutureItem<'_, (Option<Vec<Object<RobocupObjectLabel>>>,)>,
+    item: DetectedObjectsItem<'_>,
     ctx: DetectionProcessingContext<'_>,
 ) -> Result<()> {
     for (image_time, (objects,)) in item.persistent {
@@ -98,7 +101,7 @@ pub(crate) async fn process_detected_objects(
 
 fn prepare_detection_frame(
     image_time: Time,
-    objects: Option<Vec<Object<RobocupObjectLabel>>>,
+    objects: Option<DetectedObjects>,
     ctx: &DetectionProcessingContext<'_>,
 ) -> Option<PreparedDetectionFrame> {
     let camera_matrix = ctx.camera_matrix_cache.get_nearest(image_time)?;
@@ -113,7 +116,7 @@ fn prepare_detection_frame(
 
     Some(PreparedDetectionFrame {
         image_time,
-        objects: objects.unwrap_or_default(),
+        objects: objects.map(|item| item.inner).unwrap_or_default(),
         robot_to_camera: robot_to_camera(&camera_matrix),
         camera_matrix,
         field_dimensions: *field_dimensions.as_ref(),

crates/nodes/field_mark_association/src/node.rs

@@ -83,7 +83,7 @@ pub async fn run(ctx: Arc<Context>) -> Result<()> {
 
     let mut detected_objects = node
         .create_future_map_builder()
-        .create_future_subscriber::<Vec<Object<RobocupObjectLabel>>>(
+        .create_future_subscriber::<TimeWrapper<Vec<Object<RobocupObjectLabel>>>>(
             "detected_objects",
             DETECTED_OBJECTS_SAFETY_LAG,
         )

tools/robot_viewer/src/state/alignment.rs

@@ -98,7 +98,10 @@ mod tests {
     use linear_algebra::Isometry3;
     use projection::camera_matrix::CameraMatrix;
     use ros_z::time::Time;
-    use types::visual_localization::VisualLocalizationFrame as FieldMarkAssociations;
+    use types::{
+        object_detection::{Object, RobocupObjectLabel},
+        visual_localization::VisualLocalizationFrame as FieldMarkAssociations,
+    };
 
     use super::*;
     use crate::state::{MAX_NEAREST_SAMPLE_DISTANCE, ViewerState};
@@ -111,6 +114,13 @@ mod tests {
         }
     }
 
+    fn empty_detected_objects(time: Time) -> TimeWrapper<Vec<Object<RobocupObjectLabel>>> {
+        TimeWrapper {
+            time,
+            inner: Vec::new(),
+        }
+    }
+
     #[test]
     fn high_rate_alignment_streams_retain_delayed_render_samples() {
         let mut state = ViewerState::default();
@@ -149,7 +159,7 @@ mod tests {
         state.push_camera_frame(association_time, CameraFrame::default());
         state.push_camera_frame(detection_time, CameraFrame::default());
         state.push_field_mark_associations(association_time, empty_associations());
-        state.push_detected_objects(detection_time, Vec::new());
+        state.push_detected_objects(empty_detected_objects(detection_time));
 
         let aligned = state.aligned_snapshot();
 
@@ -235,7 +245,7 @@ mod tests {
         state.push_camera_frame(displayed_time, CameraFrame::default());
         assert_eq!(state.aligned_snapshot().anchor_time, Some(displayed_time));
 
-        state.push_detected_objects(displayed_time, Vec::new());
+        state.push_detected_objects(empty_detected_objects(displayed_time));
         state.push_camera_frame(latest_camera_time, CameraFrame::default());
         let aligned = state.aligned_snapshot();
 
@@ -255,7 +265,7 @@ mod tests {
 
         state.objects_status.update_publishers(1);
         state.push_camera_frame(first_time, CameraFrame::default());
-        state.push_detected_objects(first_time, Vec::new());
+        state.push_detected_objects(empty_detected_objects(first_time));
         assert_eq!(state.aligned_snapshot().anchor_time, Some(first_time));
 
         state.push_camera_frame(second_time, CameraFrame::default());
@@ -264,7 +274,7 @@ mod tests {
         assert_eq!(aligned.anchor_time, Some(first_time));
         assert!(aligned.detected_objects.is_some());
 
-        state.push_detected_objects(second_time, Vec::new());
+        state.push_detected_objects(empty_detected_objects(second_time));
         let aligned = state.aligned_snapshot();
 
         assert_eq!(aligned.anchor_time, Some(second_time));

tools/robot_viewer/src/state/mod.rs

@@ -101,10 +101,9 @@ impl ViewerState {
 
     pub(crate) fn push_detected_objects(
         &mut self,
-        time: Time,
-        value: Vec<Object<RobocupObjectLabel>>,
+        value: TimeWrapper<Vec<Object<RobocupObjectLabel>>>,
     ) {
-        self.detected_objects.insert(time, value);
+        self.detected_objects.insert(value.time, value.inner);
     }
 
     pub(crate) fn push_field_mark_associations(

tools/robot_viewer/src/subscriptions/mod.rs

@@ -7,7 +7,10 @@ use ros_z_debug::{TopicObserver, TopicObserverOptions};
 use ros_z_streams::{CreateFutureQueue, QueueEvent};
 use ros2::sensor_msgs::image::Image as RosImage;
 use tokio::runtime::Runtime;
-use types::object_detection::{Object, RobocupObjectLabel};
+use types::{
+    object_detection::{Object, RobocupObjectLabel},
+    time_wrapper::TimeWrapper,
+};
 
 use crate::{
     cli::Arguments,
@@ -75,7 +78,7 @@ async fn run(arguments: Arguments, state: SharedState, egui_context: EguiContext
         .build()
         .await?;
     let mut objects = node
-        .create_future_subscriber::<Vec<Object<RobocupObjectLabel>>>(
+        .create_future_subscriber::<TimeWrapper<Vec<Object<RobocupObjectLabel>>>>(
             DETECTED_OBJECTS_TOPIC,
             DETECTED_OBJECTS_SAFETY_LAG,
         )
@@ -119,8 +122,8 @@ async fn run(arguments: Arguments, state: SharedState, egui_context: EguiContext
                 }),
             },
             message = objects.recv() => match message {
-                Ok(QueueEvent::Data(time, message)) => update_state(&state, &egui_context, |state| {
-                    state.push_detected_objects(time, message);
+                Ok(QueueEvent::Data(_, message)) => update_state(&state, &egui_context, |state| {
+                    state.push_detected_objects(message);
                     state.objects_status.mark_live(objects.publisher_count());
                 }),
                 Ok(QueueEvent::Announcement) => {}