Various tweaks and hacks for the pool test

src/main.rs

@@ -166,9 +166,9 @@ async fn run_mission(mission: &str) -> Result<()> {
             Ok(())
         }
         "gate_run" => {
-            let cam = Camera::jetson_new("/dev/video1", "front", Path::new("/tmp/feed.mp4"))?;
-            let _cam_extra =
-                Camera::jetson_new("/dev/video0", "front", Path::new("/tmp/feed_extra.mp4"))?;
+            let cam = Camera::jetson_new("/dev/video0", "front", Path::new("/tmp/feed.mp4"))?;
+            //let _cam_extra =
+            //Camera::jetson_new("/dev/video0", "front", Path::new("/tmp/feed_extra.mp4"))?;
             println!("Opened camera");
             let _ = gate_run(&FullActionContext::new(
                 control_board().await,

src/missions/basic.rs

@@ -1,6 +1,6 @@
 use crate::{
     video_source::MatSource,
-    vision::{buoy::Buoy, nn_cv2::OnnxModel},
+    vision::{buoy::Buoy, gate::Gate, gate_poles::GatePoles, nn_cv2::OnnxModel},
 };
 
 use super::{
@@ -85,13 +85,13 @@ pub fn gate_run<
     context: &T,
 ) -> impl ActionExec + '_ {
     let depth: f32 = -1.0;
-    let model = Buoy::default();
+    let model = GatePoles::default();
 
     ActionSequence::new(
         ActionConcurrent::new(descend_and_go_forward(context), StartBno055::new(context)),
         ActionWhile::new(ActionSequence::new(
             ActionChain::new(
-                VisionNormOffset::<T, Buoy<OnnxModel>, f64>::new(context, model),
+                VisionNormOffset::<T, GatePoles<OnnxModel>, f64>::new(context, model),
                 AdjustMovement::new(context, depth),
             ),
             AlwaysTrue::new(),

src/missions/movement.rs

@@ -1,5 +1,6 @@
 use anyhow::Result;
 use async_trait::async_trait;
+use core::fmt::Debug;
 use tokio::io::WriteHalf;
 use tokio_serial::SerialStream;
 
@@ -152,11 +153,18 @@ impl<T> ActionMod<f32> for AdjustMovement<'_, T> {
 
 impl<T, V> ActionMod<Result<V>> for AdjustMovement<'_, T>
 where
-    V: RelPos<Number = f64> + Sync + Send,
+    V: RelPos<Number = f64> + Sync + Send + Debug,
 {
     fn modify(&mut self, input: Result<V>) {
         if let Ok(input) = input {
-            self.x = *input.offset().x() as f32;
+            println!("Modify value: {:?}", input);
+            if !input.offset().x().is_nan() || !input.offset().y().is_nan() {
+                self.x = *input.offset().x() as f32;
+            } else {
+                self.x = 0.0;
+            }
+        } else {
+            self.x = 0.0;
         }
     }
 }
@@ -167,7 +175,7 @@ impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec for AdjustMovement<
     async fn execute(&mut self) -> Self::Output {
         self.context
             .get_control_board()
-            .stability_2_speed_set_initial_yaw(0.5, self.x, 0.0, 0.0, self.target_depth)
+            .stability_2_speed_set_initial_yaw(self.x, 0.5, 0.0, 0.0, self.target_depth)
             .await
     }
 }

src/missions/vision.rs

@@ -2,7 +2,7 @@ use std::marker::PhantomData;
 
 use super::action::{Action, ActionExec};
 use crate::video_source::MatSource;
-use crate::vision::{Offset2D, RelPos, VisualDetector};
+use crate::vision::{Draw, Offset2D, RelPos, VisualDetector};
 use anyhow::Result;
 use async_trait::async_trait;
 use num_traits::{FromPrimitive, Num};
@@ -40,30 +40,33 @@ impl<T, U, V> Action for VisionNormOffset<'_, T, U, V> {}
 impl<T: MatSource, V: Num + FromPrimitive + Send + Sync, U: VisualDetector<V> + Send + Sync>
     ActionExec for VisionNormOffset<'_, T, U, V>
 where
-    U::Position: RelPos<Number = V>,
+    U::Position: RelPos<Number = V> + Draw,
 {
     type Output = Result<Offset2D<V>>;
     async fn execute(&mut self) -> Self::Output {
         #[cfg(feature = "logging")]
         {
             println!("Running detection...");
         }
-        let mat = self.context.get_mat().await;
-        let detections = self.model.detect_unique(&mat);
+        let mut mat = self.context.get_mat().await;
+        let detections = self.model.detect(&mat);
+        #[cfg(feature = "logging")]
+        println!("Detect attempt: {}", detections.is_ok());
+        let detections = detections?;
         #[cfg(feature = "logging")]
         {
+            detections
+                .iter()
+                .for_each(|x| x.position().draw(&mut mat).unwrap());
+            println!("Number of detects: {}", detections.len());
             create_dir_all("/tmp/detect").unwrap();
-            println!("Detect status: {}", detections.is_ok());
             imwrite(
                 &("/tmp/detect/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"),
                 &mat,
                 &Vector::default(),
             )
             .unwrap();
         }
-        let detections = detections?;
-        #[cfg(feature = "logging")]
-        println!("Number of detects: {}", detections.len());
 
         let positions: Vec<_> = detections
             .iter()

src/vision/yolo_model.rs

@@ -57,8 +57,8 @@ where
         const IMAGE_HEIGHT: f64 = 600.0;
         Self::Position {
             inner: Rect2d::new(
-                ((pos.inner.x / IMAGE_WIDTH) + 0.5) * 2.0,
-                ((pos.inner.y / IMAGE_HEIGHT) + 0.5) * 2.0,
+                ((pos.inner.x / IMAGE_WIDTH) - 0.5) * 2.0,
+                ((pos.inner.y / IMAGE_HEIGHT) - 0.5) * 2.0,
                 pos.inner.width / IMAGE_WIDTH,
                 pos.inner.height / IMAGE_HEIGHT,
             ),

util/push

@@ -8,5 +8,5 @@ cd "$parent"
 
 util/crossbuild "$@"
 
-ssh -i ./util/.sw8_ssh_identity 'sw8@192.168.2.5' 'sudo mount -o remount,rw /; mkdir sw8s'
+ssh -i ./util/.sw8_ssh_identity 'sw8@192.168.2.5' 'sudo mount -o remount,rw /; mkdir -p sw8s'
 scp -i ./util/.sw8_ssh_identity jetson_target/debug/sw8s_rust 'sw8@192.168.2.5:sw8s/'