From 9e596306b1dca379c5b3d54712058fd4be2e1213 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Wed, 19 Mar 2025 00:58:41 -0400 Subject: [PATCH 01/21] path_cv in progress --- .envrc | 1 + .gitignore | 1 + flake.lock | 48 +++++ flake.nix | 57 ++++++ src/missions/path_align.rs | 1 + src/vision/mod.rs | 1 + src/vision/path_cv.rs | 371 +++++++++++++++++++++++++++++++++++++ 7 files changed, 480 insertions(+) create mode 100644 .envrc create mode 100644 flake.lock create mode 100644 flake.nix create mode 100644 src/vision/path_cv.rs diff --git a/.envrc b/.envrc new file mode 100644 index 00000000000..3550a30f2de --- /dev/null +++ b/.envrc @@ -0,0 +1 @@ +use flake diff --git a/.gitignore b/.gitignore index 3875fd529aa..b925db05b40 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,7 @@ graphs sysroot-jetson target-jetson console/ +.direnv/ # Ignore all log files generated logging/ diff --git a/flake.lock b/flake.lock new file mode 100644 index 00000000000..3f56904de95 --- /dev/null +++ b/flake.lock @@ -0,0 +1,48 @@ +{ + "nodes": { + "nixpkgs": { + "locked": { + "lastModified": 1725910328, + "narHash": "sha256-n9pCtzGZ0httmTwMuEbi5E78UQ4ZbQMr1pzi5N0LAG8=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "5775c2583f1801df7b790bf7f7d710a19bac66f4", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixpkgs-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "nixpkgs": "nixpkgs", + "rust-overlay": "rust-overlay" + } + }, + "rust-overlay": { + "inputs": { + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1721355572, + "narHash": "sha256-I4TQ2guV9jTmZsXeWt5HMojcaqNZHII4zu0xIKZEovM=", + "owner": "oxalica", + "repo": "rust-overlay", + "rev": "d5bc7b1b21cf937fb8ff108ae006f6776bdb163d", + "type": "github" + }, + "original": { + "owner": "oxalica", + "repo": "rust-overlay", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/flake.nix b/flake.nix new file mode 100644 index 00000000000..655c11ceab7 --- /dev/null +++ b/flake.nix @@ -0,0 +1,57 @@ +{ + description = "A tool for building sysroots for cross compilation"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable"; + rust-overlay = { + url = "github:oxalica/rust-overlay"; + inputs.nixpkgs.follows = "nixpkgs"; + }; + }; + + outputs = { self, nixpkgs, rust-overlay }: + let + supportedSystems = [ "x86_64-linux" "aarch64-linux" "x86_64-darwin" "aarch64-darwin" ]; + forEachSupportedSystem = f: nixpkgs.lib.genAttrs supportedSystems (system: f { + pkgs = import nixpkgs { + inherit system; + overlays = [ rust-overlay.overlays.default self.overlays.default ]; + }; + }); + in + { + overlays.default = final: prev: { + rustToolchain = + let + rust = prev.rust-bin; + in + if builtins.pathExists ./rust-toolchain.toml then + rust.fromRustupToolchainFile ./rust-toolchain.toml + else if builtins.pathExists ./rust-toolchain then + rust.fromRustupToolchainFile ./rust-toolchain + else + rust.stable.latest.default.override { + extensions = [ "rust-src" "rustfmt" ]; + }; + }; + + devShells = forEachSupportedSystem ({ pkgs }: { + default = pkgs.mkShell { + packages = with pkgs; [ + rustToolchain + openssl + pkg-config + cargo-deny + cargo-edit + cargo-watch + rust-analyzer + ]; + + env = { + # Required by rust-analyzer + RUST_SRC_PATH = "${pkgs.rustToolchain}/lib/rustlib/src/rust/library"; + }; + }; + }); + }; +} diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 77ea658181f..99be36844bc 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -17,6 +17,7 @@ use crate::{ vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, }, vision::path::Path, + vision::path_cv::PathCV, }; use super::{ diff --git a/src/vision/mod.rs b/src/vision/mod.rs index dc14d541cfc..67bea5aabcc 100644 --- a/src/vision/mod.rs +++ b/src/vision/mod.rs @@ -22,6 +22,7 @@ pub mod image_prep; pub mod nn_cv2; pub mod octagon; pub mod path; +pub mod path_cv; pub mod pca; pub mod yolo_model; diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs new file mode 100644 index 00000000000..f6e5fd3affe --- /dev/null +++ b/src/vision/path_cv.rs @@ -0,0 +1,371 @@ +use std::{fs::create_dir_all, ops::RangeInclusive}; + +use itertools::Itertools; +use opencv::{ + core::{in_range, Size, VecN, Vector}, + imgcodecs::imwrite, + imgproc::{cvt_color, COLOR_RGB2YUV, COLOR_YUV2RGB}, + prelude::{Mat, MatTraitConst, MatTraitConstManual}, +}; +use uuid::Uuid; + +use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; + +use super::{ + image_prep::{kmeans, resize}, + pca::PosVector, + MatWrapper, VisualDetection, VisualDetector, +}; + +static FORWARD: (f64, f64) = (0.0, -1.0); + +#[derive(Debug, PartialEq)] +pub struct Yuv { + pub y: u8, + pub u: u8, + pub v: u8, +} + +impl From<&VecN> for Yuv { + fn from(value: &VecN) -> Self { + Self { + y: value[0], + u: value[1], + v: value[2], + } + } +} + +impl From<&Yuv> for VecN { + fn from(val: &Yuv) -> Self { + VecN::from_array([val.y, val.u, val.v]) + } +} + +impl Yuv { + fn in_range(&self, range: &RangeInclusive) -> bool { + self.y >= range.start().y + && self.u >= range.start().u + && self.v >= range.start().v + && self.y <= range.end().y + && self.u <= range.end().u + && self.v <= range.end().v + } +} + +#[derive(Debug)] +pub struct PathCV { + color_bounds: RangeInclusive, + width_bounds: RangeInclusive, + num_regions: i32, + size: Size, + attempts: i32, + image: MatWrapper, +} + +impl PathCV { + pub fn image(&self) -> Mat { + (*self.image).clone() + } +} + +impl PathCV { + pub fn new( + color_bounds: RangeInclusive, + width_bounds: RangeInclusive, + num_regions: i32, + size: Size, + attempts: i32, + ) -> Self { + Self { + color_bounds, + width_bounds, + num_regions, + size, + attempts, + image: Mat::default().into(), + } + } +} + +impl Default for PathCV { + fn default() -> Self { + PathCV::new( + (Yuv { y: 0, u: 0, v: 127 })..=(Yuv { + y: 255, + u: 127, + v: 255, + }), + 20.0..=800.0, + 4, + Size::from((400, 300)), + 3, + ) + } +} + +fn compute_angle(v1: (f64, f64), v2: (f64, f64)) -> f64 { + let dot = (v1.0 * v2.0) + (v1.1 * v2.1); + let norm = |vec: (f64, f64)| ((vec.0 * vec.0) + (vec.1 * vec.1)).sqrt(); + let norm_combined = norm(v1) * norm(v2); + (dot / norm_combined).acos() +} + +impl VisualDetector for PathCV { + type ClassEnum = bool; + type Position = PosVector; + + fn detect( + &mut self, + input_image: &Mat, + ) -> anyhow::Result>> { + self.image = resize(input_image, &self.size)?.into(); + let mut yuv_image = Mat::default(); + + cvt_color(&self.image.0, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); + yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); + let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); + + cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); + + yuv_image + .iter::>() + .unwrap() + .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) + .dedup_by(|(_, val), (_, n_val)| val == n_val) + .map(|(_, val)| { + + // let mut bin_image = Mat::default(); + // in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); + // let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); + // let pca_output = binary_pca(&on_points, 0).unwrap(); + + // let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() + // > pca_output.pca_value().get(0).unwrap() + // { + // (1, 0) + // } else { + // (0, 1) + // }; + // // width bounds have a temp fix -- not sure why output is so large + // let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; + // let length = pca_output.pca_value().get(length_idx).unwrap(); + // let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); + + // logln!("Testing for valid..."); + // logln!("\tself.width_bounds = {:?}", self.width_bounds); + // logln!("\tself.width = {:?}", width); + // logln!( + // "\tcontained_width = {:?}", + // self.width_bounds.contains(&width) + // ); + // logln!(); + // logln!("\tYUV range = {:?}", self.color_bounds); + // logln!("\tYUV val = {:?}", Yuv::from(&val)); + // logln!( + // "\tcontained_color = {:?}", + // Yuv::from(&val).in_range(&self.color_bounds) + // ); + // logln!(); + + // let valid = self.width_bounds.contains(&width) + // && Yuv::from(&val).in_range(&self.color_bounds); + + // let p_vec = PosVector::new( + // ((pca_output.mean().get(0).unwrap()) - image_center.0) + // + (self.image.size().unwrap().width as f64) / 2.0, + // (pca_output.mean().get(1).unwrap()) - image_center.1 + // + (self.image.size().unwrap().height as f64) / 2.0, + // compute_angle( + // ( + // pca_output.pca_vector().get(length_idx).unwrap(), + // pca_output.pca_vector().get(length_idx + 1).unwrap(), + // ), + // FORWARD, + // ), + // width, + // length / 300.0, + // length_2, + // ); + + // Ok(VisualDetection { + // class: valid, + // position: p_vec, + // }) + }) + .collect() + } + + fn normalize(&mut self, pos: &Self::Position) -> Self::Position { + let img_size = self.image.size().unwrap(); + Self::Position::new( + ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, + ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + *pos.angle(), + *pos.width() / (img_size.width as f64), + *pos.length() / (img_size.height as f64), + *pos.length_2() / (img_size.height as f64), + ) + } +} + +impl VisualDetector for PathCV { + type ClassEnum = bool; + type Position = PosVector; + + fn detect( + &mut self, + input_image: &Mat, + ) -> anyhow::Result>> { + let image = resize(input_image, &self.size)?; + let mut yuv_image = Mat::default(); + + cvt_color(&image, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); + yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); + let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); + + cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); + + #[cfg(feature = "logging")] + { + create_dir_all("/tmp/path_images").unwrap(); + imwrite( + &("/tmp/path_images/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"), + &self.image.0, + &Vector::default(), + ) + .unwrap(); + } + + yuv_image + .iter::>() + .unwrap() + .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) + .dedup_by(|(_, val), (_, n_val)| val == n_val) + .map(|(_, val)| { + let mut bin_image = Mat::default(); + in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); + let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); + let pca_output = binary_pca(&on_points, 0).unwrap(); + + let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() + > pca_output.pca_value().get(0).unwrap() + { + (1, 0) + } else { + (0, 1) + }; + // width bounds have a temp fix -- not sure why output is so large + let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; + let length = pca_output.pca_value().get(length_idx).unwrap(); + let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); + + logln!("Testing for valid..."); + logln!("\tself.width_bounds = {:?}", self.width_bounds); + logln!("\tself.width = {:?}", width); + logln!( + "\tcontained_width = {:?}", + self.width_bounds.contains(&width) + ); + logln!(); + logln!("\tYUV range = {:?}", self.color_bounds); + logln!("\tYUV val = {:?}", Yuv::from(&val)); + logln!( + "\tcontained_color = {:?}", + Yuv::from(&val).in_range(&self.color_bounds) + ); + logln!(); + + let valid = self.width_bounds.contains(&width) + && Yuv::from(&val).in_range(&self.color_bounds); + + if valid { + logln!("\tself.width_bounds = {:?}", self.width_bounds); + logln!("\tself.width = {:?}", width); + logln!( + "\tcontained_width = {:?}", + self.width_bounds.contains(&width) + ); + logln!(); + logln!("\tYUV range = {:?}", self.color_bounds); + logln!("\tYUV val = {:?}", Yuv::from(&val)); + logln!( + "\tcontained_color = {:?}", + Yuv::from(&val).in_range(&self.color_bounds) + ); + }; + + let p_vec = PosVector::new( + ((pca_output.mean().get(0).unwrap()) - image_center.0) + + (self.image.size().unwrap().width as f64) / 2.0, + (pca_output.mean().get(1).unwrap()) - image_center.1 + + (self.image.size().unwrap().height as f64) / 2.0, + compute_angle( + ( + pca_output.pca_vector().get(length_idx).unwrap(), + pca_output.pca_vector().get(length_idx + 1).unwrap(), + ), + FORWARD, + ), + width, + length / 300.0, + length_2, + ); + + Ok(VisualDetection { + class: valid, + position: p_vec, + }) + }) + .collect() + } + + fn normalize(&mut self, pos: &Self::Position) -> Self::Position { + let img_size = self.image.size().unwrap(); + Self::Position::new( + ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, + ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + *pos.angle(), + *pos.width() / (img_size.width as f64), + *pos.length() / (img_size.height as f64), + *pos.length_2() / (img_size.height as f64), + ) + } +}: + +#[cfg(test)] +mod tests { + use std::fs::create_dir_all; + + use opencv::{ + core::Vector, + imgcodecs::{imread, imwrite, IMREAD_COLOR}, + }; + + use crate::{logln, vision::Draw}; + + use super::*; + + #[test] + fn detect_single() { + let image = imread("tests/vision/resources/path_images/1.jpeg", IMREAD_COLOR).unwrap(); + let mut path = PathCV::default(); + let detections = >::detect(&mut path, &image).unwrap(); + let mut shrunk_image = path.image().clone(); + + detections.iter().for_each(|result| { + as Draw>::draw(result, &mut shrunk_image).unwrap() + }); + + logln!("Detections: {:#?}", detections); + + create_dir_all("tests/vision/output/path_images").unwrap(); + imwrite( + "tests/vision/output/path_images/1.jpeg", + &shrunk_image, + &Vector::default(), + ) + .unwrap(); + } +} + From 9a25b34fd1074e8fc2b1110fd34d0a8d11558c05 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:19:06 -0400 Subject: [PATCH 02/21] Complete pathCV implementation (path detection using pure cv and contour detection). Compiles for jetson and ready to test for pool test 3/22/25. --- src/missions/path_align.rs | 2 +- src/vision/path_cv.rs | 412 +++++++++++++++++++++---------------- 2 files changed, 231 insertions(+), 183 deletions(-) diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 99be36844bc..729df686a4d 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -42,7 +42,7 @@ pub fn path_align< ZeroMovement::new(context, DEPTH), DelayAction::new(2.0), ActionWhile::new(ActionChain::new( - VisionNormBottom::::new(context, Path::default()), + VisionNormBottom::::new(context, PathCV::default()), TupleSecond::new(ActionConcurrent::new( act_nest!( ActionChain::new, diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index f6e5fd3affe..5f63bce1aea 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -1,24 +1,105 @@ -use std::{fs::create_dir_all, ops::RangeInclusive}; +use std::{fs::create_dir_all, ops::Mul, ops::RangeInclusive}; +use derive_getters::Getters; use itertools::Itertools; use opencv::{ - core::{in_range, Size, VecN, Vector}, + core::{in_range, Point, Scalar, Size, VecN, Vector}, imgcodecs::imwrite, - imgproc::{cvt_color, COLOR_RGB2YUV, COLOR_YUV2RGB}, + imgproc::{ + self, box_points, circle, contour_area_def, cvt_color_def, find_contours_def, + min_area_rect, CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, COLOR_YUV2BGR, LINE_8, RETR_EXTERNAL, + }, prelude::{Mat, MatTraitConst, MatTraitConstManual}, }; use uuid::Uuid; use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; +use crate::vision::{Angle2D, Draw, Offset2D, RelPosAngle}; use super::{ image_prep::{kmeans, resize}, - pca::PosVector, MatWrapper, VisualDetection, VisualDetector, }; static FORWARD: (f64, f64) = (0.0, -1.0); +#[derive(Debug, Clone, Getters, PartialEq)] +pub struct PosVector { + x: f64, + y: f64, + z: f64, + angle: f64, +} + +impl PosVector { + fn new(x: f64, y: f64, z: f64, angle: f64) -> Self { + Self { x, y, z, angle } + } +} + +impl RelPosAngle for PosVector { + type Number = f64; + + fn offset_angle(&self) -> Angle2D { + Angle2D { + x: self.x, + y: self.y, + angle: self.angle, + } + } +} + +impl Mul<&Mat> for PosVector { + type Output = Self; + + fn mul(self, rhs: &Mat) -> Self::Output { + let size = rhs.size().unwrap(); + Self { + x: (self.x + 0.5) * (size.width as f64), + y: (self.y + 0.5) * (size.height as f64), + z: 0., + angle: self.angle, + } + } +} + +impl Draw for VisualDetection { + fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> { + let color = if self.class { + logln!("Drawing true: {:#?}", self.position()); + Scalar::from((0.0, 255.0, 0.0)) + } else { + Scalar::from((0.0, 0.0, 255.0)) + }; + + imgproc::circle( + canvas, + Point::new(*self.position.x() as i32, *self.position.y() as i32), + 10, + color, + 2, + LINE_8, + 0, + )?; + + // imgproc::arrowed_line( + // canvas, + // Point::new(*self.position.x() as i32, *self.position.y() as i32), + // Point::new( + // (self.position.x() + 0.02 * self.position.length() * self.position.length()) as i32, + // (self.position.y() + 0.4 * self.position.length_2() * self.position.length()) + // as i32, + // ), + // color, + // 2, + // LINE_8, + // 0, + // 0.1, + // )?; + Ok(()) + } +} + #[derive(Debug, PartialEq)] pub struct Yuv { pub y: u8, @@ -91,7 +172,7 @@ impl PathCV { impl Default for PathCV { fn default() -> Self { PathCV::new( - (Yuv { y: 0, u: 0, v: 127 })..=(Yuv { + (Yuv { y: 0, u: 0, v: 175 })..=(Yuv { y: 255, u: 127, v: 255, @@ -122,78 +203,77 @@ impl VisualDetector for PathCV { self.image = resize(input_image, &self.size)?.into(); let mut yuv_image = Mat::default(); - cvt_color(&self.image.0, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); - yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); - let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); - - cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); - - yuv_image - .iter::>() - .unwrap() - .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) - .dedup_by(|(_, val), (_, n_val)| val == n_val) - .map(|(_, val)| { - - // let mut bin_image = Mat::default(); - // in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); - // let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); - // let pca_output = binary_pca(&on_points, 0).unwrap(); - - // let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() - // > pca_output.pca_value().get(0).unwrap() - // { - // (1, 0) - // } else { - // (0, 1) - // }; - // // width bounds have a temp fix -- not sure why output is so large - // let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; - // let length = pca_output.pca_value().get(length_idx).unwrap(); - // let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); - - // logln!("Testing for valid..."); - // logln!("\tself.width_bounds = {:?}", self.width_bounds); - // logln!("\tself.width = {:?}", width); - // logln!( - // "\tcontained_width = {:?}", - // self.width_bounds.contains(&width) - // ); - // logln!(); - // logln!("\tYUV range = {:?}", self.color_bounds); - // logln!("\tYUV val = {:?}", Yuv::from(&val)); - // logln!( - // "\tcontained_color = {:?}", - // Yuv::from(&val).in_range(&self.color_bounds) - // ); - // logln!(); - - // let valid = self.width_bounds.contains(&width) - // && Yuv::from(&val).in_range(&self.color_bounds); - - // let p_vec = PosVector::new( - // ((pca_output.mean().get(0).unwrap()) - image_center.0) - // + (self.image.size().unwrap().width as f64) / 2.0, - // (pca_output.mean().get(1).unwrap()) - image_center.1 - // + (self.image.size().unwrap().height as f64) / 2.0, - // compute_angle( - // ( - // pca_output.pca_vector().get(length_idx).unwrap(), - // pca_output.pca_vector().get(length_idx + 1).unwrap(), - // ), - // FORWARD, - // ), - // width, - // length / 300.0, - // length_2, - // ); - - // Ok(VisualDetection { - // class: valid, - // position: p_vec, - // }) - }) - .collect() + cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?; + + let color_start = self.color_bounds.start(); + let color_end = self.color_bounds.end(); + let lower_orange = Scalar::new( + color_start.y as f64, + color_start.u as f64, + color_start.v as f64, + 0., + ); + let upper_orange = Scalar::new( + color_end.y as f64, + color_end.u as f64, + color_end.v as f64, + 0., + ); + + let mut mask = Mat::default(); + in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); + + let mut contours = Vector::>::new(); + find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; + + let max_contour = contours.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); + + if let Some(contour) = max_contour { + let area = contour_area_def(&contour)?; + if area > 1000.0 { + let rect = min_area_rect(&contour)?; + let mut angle = rect.angle as f64; + let width = rect.size.width; + let height = rect.size.height; + if width > height { + angle = rect.angle as f64; + } else { + angle = (rect.angle as f64) + 90.0; + } + angle -= 90.0; + let mut box_rect = Mat::default(); + box_points(rect, &mut box_rect)?; + + println!("{:?}", angle); + // DRAW STUFF + // SEND TO RTSP SERVER + + let center_adjusted_x = + (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + let center_adjusted_y = + ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new(center_adjusted_x, center_adjusted_y, 0., angle), + }]) + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } } fn normalize(&mut self, pos: &Self::Position) -> Self::Position { @@ -201,10 +281,8 @@ impl VisualDetector for PathCV { Self::Position::new( ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + 0., *pos.angle(), - *pos.width() / (img_size.width as f64), - *pos.length() / (img_size.height as f64), - *pos.length_2() / (img_size.height as f64), ) } } @@ -217,107 +295,80 @@ impl VisualDetector for PathCV { &mut self, input_image: &Mat, ) -> anyhow::Result>> { - let image = resize(input_image, &self.size)?; + self.image = resize(input_image, &self.size)?.into(); let mut yuv_image = Mat::default(); - cvt_color(&image, &mut yuv_image, COLOR_RGB2YUV, 0).unwrap(); - yuv_image = kmeans(&yuv_image, self.num_regions, self.attempts); - let image_center = ((yuv_image.cols() / 2) as f64, (yuv_image.rows() / 2) as f64); - - cvt_color(&yuv_image, &mut self.image.0, COLOR_YUV2RGB, 0).unwrap(); - - #[cfg(feature = "logging")] - { - create_dir_all("/tmp/path_images").unwrap(); - imwrite( - &("/tmp/path_images/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"), - &self.image.0, - &Vector::default(), - ) - .unwrap(); - } + cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?; + + let color_start = self.color_bounds.start(); + let color_end = self.color_bounds.end(); + let lower_orange = Scalar::new( + color_start.y as f64, + color_start.u as f64, + color_start.v as f64, + 0., + ); + let upper_orange = Scalar::new( + color_end.y as f64, + color_end.u as f64, + color_end.v as f64, + 0., + ); + + let mut mask = Mat::default(); + in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); + + let mut contours = Vector::>::new(); + find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; + + let max_contour = contours.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); - yuv_image - .iter::>() - .unwrap() - .sorted_by(|(_, val), (_, n_val)| Ord::cmp(val.as_slice(), n_val.as_slice())) - .dedup_by(|(_, val), (_, n_val)| val == n_val) - .map(|(_, val)| { - let mut bin_image = Mat::default(); - in_range(&yuv_image, &val, &val, &mut bin_image).unwrap(); - let on_points = cvt_binary_to_points(&bin_image.try_into_typed().unwrap()); - let pca_output = binary_pca(&on_points, 0).unwrap(); - - let (length_idx, width_idx) = if pca_output.pca_value().get(1).unwrap() - > pca_output.pca_value().get(0).unwrap() - { - (1, 0) + if let Some(contour) = max_contour { + let area = contour_area_def(&contour)?; + if area > 1000.0 { + let rect = min_area_rect(&contour)?; + let mut angle = rect.angle as f64; + let width = rect.size.width; + let height = rect.size.height; + if width > height { + angle = rect.angle as f64; } else { - (0, 1) - }; - // width bounds have a temp fix -- not sure why output is so large - let width = pca_output.pca_value().get(width_idx).unwrap() / 100.0; - let length = pca_output.pca_value().get(length_idx).unwrap(); - let length_2 = pca_output.pca_vector().get(length_idx + 1).unwrap(); - - logln!("Testing for valid..."); - logln!("\tself.width_bounds = {:?}", self.width_bounds); - logln!("\tself.width = {:?}", width); - logln!( - "\tcontained_width = {:?}", - self.width_bounds.contains(&width) - ); - logln!(); - logln!("\tYUV range = {:?}", self.color_bounds); - logln!("\tYUV val = {:?}", Yuv::from(&val)); - logln!( - "\tcontained_color = {:?}", - Yuv::from(&val).in_range(&self.color_bounds) - ); - logln!(); - - let valid = self.width_bounds.contains(&width) - && Yuv::from(&val).in_range(&self.color_bounds); - - if valid { - logln!("\tself.width_bounds = {:?}", self.width_bounds); - logln!("\tself.width = {:?}", width); - logln!( - "\tcontained_width = {:?}", - self.width_bounds.contains(&width) - ); - logln!(); - logln!("\tYUV range = {:?}", self.color_bounds); - logln!("\tYUV val = {:?}", Yuv::from(&val)); - logln!( - "\tcontained_color = {:?}", - Yuv::from(&val).in_range(&self.color_bounds) - ); - }; - - let p_vec = PosVector::new( - ((pca_output.mean().get(0).unwrap()) - image_center.0) - + (self.image.size().unwrap().width as f64) / 2.0, - (pca_output.mean().get(1).unwrap()) - image_center.1 - + (self.image.size().unwrap().height as f64) / 2.0, - compute_angle( - ( - pca_output.pca_vector().get(length_idx).unwrap(), - pca_output.pca_vector().get(length_idx + 1).unwrap(), - ), - FORWARD, - ), - width, - length / 300.0, - length_2, - ); - - Ok(VisualDetection { - class: valid, - position: p_vec, - }) - }) - .collect() + angle = (rect.angle as f64) + 90.0; + } + angle -= 90.0; + let mut box_rect = Mat::default(); + box_points(rect, &mut box_rect)?; + + println!("{:?}", angle); + // DRAW STUFF + // SEND TO RTSP SERVER + + let center_adjusted_x = + (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + let center_adjusted_y = + ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new(center_adjusted_x, center_adjusted_y, 0., angle), + }]) + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } } fn normalize(&mut self, pos: &Self::Position) -> Self::Position { @@ -325,13 +376,11 @@ impl VisualDetector for PathCV { Self::Position::new( ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + 0., *pos.angle(), - *pos.width() / (img_size.width as f64), - *pos.length() / (img_size.height as f64), - *pos.length_2() / (img_size.height as f64), ) } -}: +} #[cfg(test)] mod tests { @@ -368,4 +417,3 @@ mod tests { .unwrap(); } } - From d8cb1fd9415de0358f1a429a32999d88a92824b3 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:24:16 -0400 Subject: [PATCH 03/21] fix: remove nix flakes --- .envrc | 1 - flake.lock | 48 --------------------------------------------- flake.nix | 57 ------------------------------------------------------ 3 files changed, 106 deletions(-) delete mode 100644 .envrc delete mode 100644 flake.lock delete mode 100644 flake.nix diff --git a/.envrc b/.envrc deleted file mode 100644 index 3550a30f2de..00000000000 --- a/.envrc +++ /dev/null @@ -1 +0,0 @@ -use flake diff --git a/flake.lock b/flake.lock deleted file mode 100644 index 3f56904de95..00000000000 --- a/flake.lock +++ /dev/null @@ -1,48 +0,0 @@ -{ - "nodes": { - "nixpkgs": { - "locked": { - "lastModified": 1725910328, - "narHash": "sha256-n9pCtzGZ0httmTwMuEbi5E78UQ4ZbQMr1pzi5N0LAG8=", - "owner": "NixOS", - "repo": "nixpkgs", - "rev": "5775c2583f1801df7b790bf7f7d710a19bac66f4", - "type": "github" - }, - "original": { - "owner": "NixOS", - "ref": "nixpkgs-unstable", - "repo": "nixpkgs", - "type": "github" - } - }, - "root": { - "inputs": { - "nixpkgs": "nixpkgs", - "rust-overlay": "rust-overlay" - } - }, - "rust-overlay": { - "inputs": { - "nixpkgs": [ - "nixpkgs" - ] - }, - "locked": { - "lastModified": 1721355572, - "narHash": "sha256-I4TQ2guV9jTmZsXeWt5HMojcaqNZHII4zu0xIKZEovM=", - "owner": "oxalica", - "repo": "rust-overlay", - "rev": "d5bc7b1b21cf937fb8ff108ae006f6776bdb163d", - "type": "github" - }, - "original": { - "owner": "oxalica", - "repo": "rust-overlay", - "type": "github" - } - } - }, - "root": "root", - "version": 7 -} diff --git a/flake.nix b/flake.nix deleted file mode 100644 index 655c11ceab7..00000000000 --- a/flake.nix +++ /dev/null @@ -1,57 +0,0 @@ -{ - description = "A tool for building sysroots for cross compilation"; - - inputs = { - nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable"; - rust-overlay = { - url = "github:oxalica/rust-overlay"; - inputs.nixpkgs.follows = "nixpkgs"; - }; - }; - - outputs = { self, nixpkgs, rust-overlay }: - let - supportedSystems = [ "x86_64-linux" "aarch64-linux" "x86_64-darwin" "aarch64-darwin" ]; - forEachSupportedSystem = f: nixpkgs.lib.genAttrs supportedSystems (system: f { - pkgs = import nixpkgs { - inherit system; - overlays = [ rust-overlay.overlays.default self.overlays.default ]; - }; - }); - in - { - overlays.default = final: prev: { - rustToolchain = - let - rust = prev.rust-bin; - in - if builtins.pathExists ./rust-toolchain.toml then - rust.fromRustupToolchainFile ./rust-toolchain.toml - else if builtins.pathExists ./rust-toolchain then - rust.fromRustupToolchainFile ./rust-toolchain - else - rust.stable.latest.default.override { - extensions = [ "rust-src" "rustfmt" ]; - }; - }; - - devShells = forEachSupportedSystem ({ pkgs }: { - default = pkgs.mkShell { - packages = with pkgs; [ - rustToolchain - openssl - pkg-config - cargo-deny - cargo-edit - cargo-watch - rust-analyzer - ]; - - env = { - # Required by rust-analyzer - RUST_SRC_PATH = "${pkgs.rustToolchain}/lib/rustlib/src/rust/library"; - }; - }; - }); - }; -} From 68fc221afdf0cde55f7261431168008fc1b7693a Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 22 Mar 2025 00:25:35 -0400 Subject: [PATCH 04/21] fix: update gitignore --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index b925db05b40..3875fd529aa 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ graphs sysroot-jetson target-jetson console/ -.direnv/ # Ignore all log files generated logging/ From 343b26ffa239093df4e5875f3ae99dd5a2aa5eca Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sun, 30 Mar 2025 14:26:17 -0400 Subject: [PATCH 05/21] feat: changes from March 29th, 2025 pool test - Tune depths for dive well - Replace path align PCA implementation with CV implementation --- src/missions/coinflip.rs | 4 +- src/missions/gate.rs | 2 +- src/missions/path_align.rs | 106 ++++++++++++++++++++++++++++++++++--- src/vision/path_cv.rs | 24 +++++---- 4 files changed, 116 insertions(+), 20 deletions(-) diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index 276d96eab46..e8fcfb122a5 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -40,10 +40,10 @@ pub fn coinflip< const TRUE_COUNT: u32 = 4; const DELAY_TIME: f32 = 3.0; - const DEPTH: f32 = -1.15; + const DEPTH: f32 = -1.75; const ALIGN_X_SPEED: f32 = 0.0; const ALIGN_Y_SPEED: f32 = 0.0; - const ALIGN_YAW_SPEED: f32 = -6.0; + const ALIGN_YAW_SPEED: f32 = -3.0; const ALIGN_YAW_CORRECTION_SPEED: f32 = 0.0; act_nest!( diff --git a/src/missions/gate.rs b/src/missions/gate.rs index 93c46ba5bb7..329a9514e76 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -121,7 +121,7 @@ pub fn gate_run_coinflip< ) -> impl ActionExec> + '_ { const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.0; + let depth: f32 = -1.6; act_nest!( ActionSequence::new, diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 729df686a4d..298c3e04000 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -9,10 +9,10 @@ use crate::{ TupleSecond, }, basic::DelayAction, - extra::{CountTrue, OutputType, Terminal, ToVec}, + extra::{CountFalse, CountTrue, OutputType, Terminal, ToVec}, movement::{ - LinearYawFromX, OffsetToPose, Stability2Adjust, Stability2Movement, Stability2Pos, - ZeroMovement, + AdjustType, ClampX, FlipX, LinearYawFromX, OffsetToPose, SetY, Stability2Adjust, + Stability2Movement, Stability2Pos, ZeroMovement, }, vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, }, @@ -34,13 +34,48 @@ pub fn path_align< >( context: &Con, ) -> impl ActionExec<()> + '_ { - const DEPTH: f32 = -1.25; + const DEPTH: f32 = -1.5; const PATH_ALIGN_SPEED: f32 = 0.3; act_nest!( ActionSequence::new, ZeroMovement::new(context, DEPTH), DelayAction::new(2.0), + ActionWhile::new(ActionChain::new( + VisionNormBottom::::new(context, PathCV::default()), + TupleSecond::new(ActionConcurrent::new( + act_nest!( + ActionChain::new, + ActionDataConditional::new( + DetectTarget::new(true), + act_nest!( + ActionChain::new, + ExtractPosition::new(), + MidPoint::new(), + OffsetToPose::default(), + // LinearYawFromX::::new(0.0), + ClampX::new(1.0), + // SetY::::new(AdjustType::Adjust(0.02)), + // FlipX::default(), + Stability2Movement::new( + context, + Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH), + ), + ), + act_nest!( + ActionSequence::new, + Terminal::new(), + Stability2Movement::new( + context, + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), + ), + ), + ), + OutputType::<()>::new(), + ), + ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), + )), + )), ActionWhile::new(ActionChain::new( VisionNormBottom::::new(context, PathCV::default()), TupleSecond::new(ActionConcurrent::new( @@ -56,7 +91,7 @@ pub fn path_align< LinearYawFromX::::default(), Stability2Movement::new( context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 30.0, 0.0, None, DEPTH), + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), ), ), act_nest!( @@ -64,13 +99,70 @@ pub fn path_align< Terminal::new(), Stability2Movement::new( context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 30.0, 0.0, None, DEPTH), + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), ), ), ), OutputType::<()>::new(), ), - ActionChain::new(DetectTarget::new(true), CountTrue::new(5)), + ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), + )), + )), + ActionWhile::new(ActionChain::new( + VisionNormBottom::::new(context, PathCV::default()), + TupleSecond::new(ActionConcurrent::new( + act_nest!( + ActionChain::new, + // act_nest!( + // ActionChain::new, + // ActionDataConditional::new( + // DetectTarget::new(true), + // act_nest!( + // ActionChain::new, + // ExtractPosition::new(), + // MidPoint::new(), + // OffsetToPose::default(), + // LinearYawFromX::::default(), + // Stability2Movement::new( + // context, + // Stability2Pos::new( + // 0.0, + // PATH_ALIGN_SPEED, + // 0.0, + // 0.0, + // None, + // DEPTH + // ), + // ), + // ), + // act_nest!( + // ActionSequence::new, + // Terminal::new(), + // Stability2Movement::new( + // context, + // Stability2Pos::new( + // 0.0, + // PATH_ALIGN_SPEED, + // 0.0, + // 0.0, + // None, + // DEPTH + // ), + // ), + // ), + // OutputType::<()>::new(), + // ), + act_nest!( + ActionSequence::new, + Terminal::new(), + Stability2Movement::new( + context, + Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), + ), + ), + OutputType::<()>::new(), + ), + ActionChain::new(DetectTarget::new(true), CountFalse::new(5)), )), )), Terminal::new(), diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index 5f63bce1aea..e89d4fb58bc 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -235,7 +235,7 @@ impl VisualDetector for PathCV { if let Some(contour) = max_contour { let area = contour_area_def(&contour)?; - if area > 1000.0 { + if area > 5000.0 { let rect = min_area_rect(&contour)?; let mut angle = rect.angle as f64; let width = rect.size.width; @@ -253,10 +253,12 @@ impl VisualDetector for PathCV { // DRAW STUFF // SEND TO RTSP SERVER - let center_adjusted_x = - (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); - let center_adjusted_y = - ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + // let center_adjusted_x = + // (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + // let center_adjusted_y = + // ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; Ok(vec![VisualDetection { class: true, @@ -330,7 +332,7 @@ impl VisualDetector for PathCV { if let Some(contour) = max_contour { let area = contour_area_def(&contour)?; - if area > 1000.0 { + if area > 500.0 { let rect = min_area_rect(&contour)?; let mut angle = rect.angle as f64; let width = rect.size.width; @@ -348,10 +350,12 @@ impl VisualDetector for PathCV { // DRAW STUFF // SEND TO RTSP SERVER - let center_adjusted_x = - (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); - let center_adjusted_y = - ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + // let center_adjusted_x = + // (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); + // let center_adjusted_y = + // ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; Ok(vec![VisualDetection { class: true, From 634edccd474e554bf158aff5a56e8176b403da36 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sat, 5 Apr 2025 15:56:26 -0400 Subject: [PATCH 06/21] feat: path adjustments from pooltest - Detects path well - Continuously rotates counter clockwise while waiting for false detections - Does not move forward while waiting for false count - Flipped Y direction during position corrections --- src/missions/movement.rs | 67 +++++++++++++++++++ src/missions/path_align.rs | 134 +++++++++++++++++++------------------ 2 files changed, 135 insertions(+), 66 deletions(-) diff --git a/src/missions/movement.rs b/src/missions/movement.rs index a5148b868d0..8243bcb7cd7 100644 --- a/src/missions/movement.rs +++ b/src/missions/movement.rs @@ -1076,6 +1076,73 @@ impl ActionExec for FlipX { } } +#[derive(Debug)] +pub struct FlipY { + pose: T, +} + +impl Action for FlipY {} + +impl FlipY<&Stability2Adjust> { + const DEFAULT_POSE: Stability2Adjust = Stability2Adjust::const_default(); + pub const fn new() -> Self { + Self { + pose: &Self::DEFAULT_POSE, + } + } +} + +impl FlipY<&Stability1Adjust> { + const DEFAULT_POSE: Stability1Adjust = Stability1Adjust::const_default(); + pub const fn new() -> Self { + Self { + pose: &Self::DEFAULT_POSE, + } + } +} + +impl FlipY { + pub fn new() -> Self { + Self { pose: T::default() } + } +} + +impl Default for FlipY { + fn default() -> Self { + Self::new() + } +} + +impl ActionMod for FlipY { + fn modify(&mut self, input: &T) { + self.pose = input.clone(); + } +} + +impl ActionExec for FlipY { + async fn execute(&mut self) -> Stability2Adjust { + if let Some(ref mut y) = self.pose.y { + *y = match *y { + AdjustType::Adjust(y) => AdjustType::Adjust(-y), + AdjustType::Replace(y) => AdjustType::Replace(-y), + }; + } + self.pose.clone() + } +} + +impl ActionExec for FlipY { + async fn execute(&mut self) -> Stability1Adjust { + if let Some(ref mut y) = self.pose.y { + *y = match *y { + AdjustType::Adjust(y) => AdjustType::Adjust(-y), + AdjustType::Replace(y) => AdjustType::Replace(-y), + }; + } + self.pose.clone() + } +} + #[derive(Debug)] pub struct StripX { pose: T, diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 298c3e04000..f8aecec5e14 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -11,7 +11,7 @@ use crate::{ basic::DelayAction, extra::{CountFalse, CountTrue, OutputType, Terminal, ToVec}, movement::{ - AdjustType, ClampX, FlipX, LinearYawFromX, OffsetToPose, SetY, Stability2Adjust, + AdjustType, ClampX, FlipX, FlipY, LinearYawFromX, OffsetToPose, SetY, Stability2Adjust, Stability2Movement, Stability2Pos, ZeroMovement, }, vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, @@ -34,7 +34,7 @@ pub fn path_align< >( context: &Con, ) -> impl ActionExec<()> + '_ { - const DEPTH: f32 = -1.5; + const DEPTH: f32 = -1.25; const PATH_ALIGN_SPEED: f32 = 0.3; act_nest!( @@ -53,13 +53,12 @@ pub fn path_align< ExtractPosition::new(), MidPoint::new(), OffsetToPose::default(), - // LinearYawFromX::::new(0.0), - ClampX::new(1.0), - // SetY::::new(AdjustType::Adjust(0.02)), - // FlipX::default(), + LinearYawFromX::::new(1.0), + ClampX::::new(0.3), + FlipY::default(), Stability2Movement::new( context, - Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH), + Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), ), ), act_nest!( @@ -88,11 +87,14 @@ pub fn path_align< ExtractPosition::new(), MidPoint::new(), OffsetToPose::default(), - LinearYawFromX::::default(), + LinearYawFromX::::new(1.0), + ClampX::::new(0.3), + FlipY::default(), Stability2Movement::new( context, Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), ), + // SetY::::new(AdjustType::Replace(PATH_ALIGN_SPEED)), ), act_nest!( ActionSequence::new, @@ -105,66 +107,66 @@ pub fn path_align< ), OutputType::<()>::new(), ), - ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), - )), - )), - ActionWhile::new(ActionChain::new( - VisionNormBottom::::new(context, PathCV::default()), - TupleSecond::new(ActionConcurrent::new( - act_nest!( - ActionChain::new, - // act_nest!( - // ActionChain::new, - // ActionDataConditional::new( - // DetectTarget::new(true), - // act_nest!( - // ActionChain::new, - // ExtractPosition::new(), - // MidPoint::new(), - // OffsetToPose::default(), - // LinearYawFromX::::default(), - // Stability2Movement::new( - // context, - // Stability2Pos::new( - // 0.0, - // PATH_ALIGN_SPEED, - // 0.0, - // 0.0, - // None, - // DEPTH - // ), - // ), - // ), - // act_nest!( - // ActionSequence::new, - // Terminal::new(), - // Stability2Movement::new( - // context, - // Stability2Pos::new( - // 0.0, - // PATH_ALIGN_SPEED, - // 0.0, - // 0.0, - // None, - // DEPTH - // ), - // ), - // ), - // OutputType::<()>::new(), - // ), - act_nest!( - ActionSequence::new, - Terminal::new(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), - ), - ), - OutputType::<()>::new(), - ), - ActionChain::new(DetectTarget::new(true), CountFalse::new(5)), + ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), )), )), + // ActionWhile::new(ActionChain::new( + // VisionNormBottom::::new(context, PathCV::default()), + // TupleSecond::new(ActionConcurrent::new( + // act_nest!( + // ActionChain::new, + // act_nest!( + // ActionChain::new, + // ActionDataConditional::new( + // DetectTarget::new(true), + // act_nest!( + // ActionChain::new, + // ExtractPosition::new(), + // MidPoint::new(), + // OffsetToPose::default(), + // LinearYawFromX::::default(), + // Stability2Movement::new( + // context, + // Stability2Pos::new( + // 0.0, + // PATH_ALIGN_SPEED / 2.0, + // 0.0, + // 0.0, + // None, + // DEPTH + // ), + // ), + // ), + // act_nest!( + // ActionSequence::new, + // Terminal::new(), + // Stability2Movement::new( + // context, + // Stability2Pos::new( + // 0.0, + // PATH_ALIGN_SPEED / 2.0, + // 0.0, + // 0.0, + // None, + // DEPTH + // ), + // ), + // ), + // OutputType::<()>::new(), + // ), + // act_nest!( + // ActionSequence::new, + // Terminal::new(), + // Stability2Movement::new( + // context, + // Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), + // ), + // ), + // OutputType::<()>::new(), + // ), + // ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), + // )), + // )), Terminal::new(), ) } From 0a74eaa410a15a4a53b78f6ad580684d786e3620 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 10 Apr 2025 01:25:14 -0400 Subject: [PATCH 07/21] feat(WIP): add feature to publish RTSP stream annotated with cv detections --- Cargo.toml | 1 + jetson/src/main.rs | 2 +- src/missions/action_context.rs | 8 +++++ src/missions/vision.rs | 1 + src/video_source/appsink.rs | 57 ++++++++++++++++++++++++++++++++-- src/vision/path_cv.rs | 12 ------- 6 files changed, 65 insertions(+), 16 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 735f5e4b8ad..c094a979575 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -23,6 +23,7 @@ required-features = ["graphing"] [features] default = [] logging = [] +annotated_streams = [] #unblocked_logging = ["logging"] cuda = ["dep:cc"] cuda_f16 = ["cuda"] diff --git a/jetson/src/main.rs b/jetson/src/main.rs index 66997b26605..aa2b940429a 100644 --- a/jetson/src/main.rs +++ b/jetson/src/main.rs @@ -38,7 +38,7 @@ async fn main() -> Result<()> { "build".to_string(), "--release".to_string(), "--features".to_string(), - "logging".to_string(), + "logging,annotated_streams".to_string(), ]; } diff --git a/src/missions/action_context.rs b/src/missions/action_context.rs index ebe1b78c654..f9a13df3198 100644 --- a/src/missions/action_context.rs +++ b/src/missions/action_context.rs @@ -1,5 +1,6 @@ use core::fmt::Debug; use opencv::core::Mat; +use opencv::mod_prelude::ToInputArray; use tokio::io::{AsyncWriteExt, WriteHalf}; use tokio::sync::RwLock; use tokio_serial::SerialStream; @@ -40,6 +41,7 @@ pub trait GetFrontCamMat { #[allow(async_fn_in_trait)] pub trait GetBottomCamMat { async fn get_bottom_camera_mat(&self) -> Mat; + async fn annotate_bottom_camera(&self, image: &impl ToInputArray); } /* @@ -111,6 +113,9 @@ impl GetBottomCamMat for FullActionContext<'_, async fn get_bottom_camera_mat(&self) -> Mat { self.bottom_cam.get_mat().await } + async fn annotate_bottom_camera(&self, image: &impl ToInputArray) { + self.bottom_cam.push_annotated_frame(image); + } } impl GetControlBoard> for EmptyActionContext { @@ -141,4 +146,7 @@ impl GetBottomCamMat for EmptyActionContext { async fn get_bottom_camera_mat(&self) -> Mat { todo!() } + async fn annotate_bottom_camera(&self, image: &impl ToInputArray) { + todo!(); + } } diff --git a/src/missions/vision.rs b/src/missions/vision.rs index 7ef8f32dc02..7029772d2b6 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -322,6 +322,7 @@ where &Vector::default(), ) .unwrap(); + self.context.annotate_bottom_camera(&mat); } Ok(detections diff --git a/src/video_source/appsink.rs b/src/video_source/appsink.rs index fec4e1f3b81..b078a203ead 100644 --- a/src/video_source/appsink.rs +++ b/src/video_source/appsink.rs @@ -1,10 +1,16 @@ use anyhow::{anyhow, Result}; +use opencv::core::Size; +use opencv::mod_prelude::ToInputArray; use opencv::prelude::Mat; -use opencv::videoio::VideoCapture; use opencv::videoio::VideoCaptureAPIs; -use opencv::videoio::VideoCaptureTrait; +use opencv::videoio::{ + VideoCapture, VideoWriter, CAP_GSTREAMER, CAP_PROP_FPS, CAP_PROP_FRAME_HEIGHT, + CAP_PROP_FRAME_WIDTH, +}; +use opencv::videoio::{VideoCaptureTrait, VideoCaptureTraitConst, VideoWriterTrait}; use std::fs::create_dir_all; use std::path::Path; +use std::sync; use std::sync::Arc; use std::thread::spawn; use tokio::sync::Mutex; @@ -16,6 +22,8 @@ use super::MatSource; #[derive(Debug)] pub struct Camera { frame: Arc>>, + #[cfg(feature = "annotated_streams")] + output: Arc>, } impl Camera { @@ -49,15 +57,48 @@ impl Camera { + camera_name + ".mp4\" "; + #[cfg(feature = "annotated_streams")] + let output_string = "appsrc ! videoconvert ! x264enc key-int-max=30 insert-vui=1 tune=zerolatency ! mpegtsmux ! rtspclientsink location=rtspt://127.0.0.1:8554/".to_string() + + camera_name + "_annotated.mp4 "; + let frame: Arc>> = Arc::default(); let frame_copy = frame.clone(); + #[cfg(feature = "annotated_streams")] + let output: Arc> = + Arc::new(sync::Mutex::new(VideoWriter::default().unwrap())); + #[cfg(feature = "annotated_streams")] + let output_copy = output.clone(); + #[cfg(feature = "logging")] logln!("Capture string: {capture_string}"); spawn(move || { let mut capture = VideoCapture::from_file(&capture_string, VideoCaptureAPIs::CAP_GSTREAMER as i32) .unwrap(); + + #[cfg(feature = "annotated_streams")] + { + let width = capture + .get(CAP_PROP_FRAME_WIDTH) + .expect("Failed to get capture frame width") as i32; + let height = capture + .get(CAP_PROP_FRAME_HEIGHT) + .expect("Failed to get capture frame height") + as i32; + let fps = capture + .get(CAP_PROP_FPS) + .expect("Failed to get capture FPS"); + + *output_copy.lock().unwrap() = VideoWriter::new_with_backend_def( + &output_string, + CAP_GSTREAMER, + VideoWriter::fourcc('X', '2', '6', '4').unwrap(), + fps, + Size::new(width, height), + ) + .unwrap(); + } loop { let mut mat = Mat::default(); if capture.read(&mut mat).unwrap() { @@ -66,12 +107,22 @@ impl Camera { } }); - Ok(Self { frame }) + Ok(Self { + frame, + #[cfg(feature = "annotated_streams")] + output, + }) } pub fn jetson_new(camera_path: &str, camera_name: &str, filesink_dir: &Path) -> Result { Camera::new(camera_path, camera_name, filesink_dir, (640, 480), true) } + + pub fn push_annotated_frame(&self, image: &impl ToInputArray) { + let writer = self.output.clone(); + let mut writer = writer.lock().unwrap(); + writer.write(image); + } } impl MatSource for Camera { diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index e89d4fb58bc..9f22b850980 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -250,13 +250,7 @@ impl VisualDetector for PathCV { box_points(rect, &mut box_rect)?; println!("{:?}", angle); - // DRAW STUFF - // SEND TO RTSP SERVER - // let center_adjusted_x = - // (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); - // let center_adjusted_y = - // ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); let center_adjusted_x = rect.center.x as f64; let center_adjusted_y = rect.center.y as f64; @@ -347,13 +341,7 @@ impl VisualDetector for PathCV { box_points(rect, &mut box_rect)?; println!("{:?}", angle); - // DRAW STUFF - // SEND TO RTSP SERVER - // let center_adjusted_x = - // (rect.center.x as f64) - ((self.image.size()?.width as f64) / 2.0); - // let center_adjusted_y = - // ((self.image.size()?.height as f64) / 2.0) - (rect.center.y as f64); let center_adjusted_x = rect.center.x as f64; let center_adjusted_y = rect.center.y as f64; From 0c3e237ae02669769cf4f121dbdea2d13d48e45f Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 10 Apr 2025 14:22:01 -0400 Subject: [PATCH 08/21] fix(WIP): await bottom camera annotation --- src/missions/vision.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/missions/vision.rs b/src/missions/vision.rs index 7029772d2b6..d5d49b3c960 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -322,7 +322,7 @@ where &Vector::default(), ) .unwrap(); - self.context.annotate_bottom_camera(&mat); + self.context.annotate_bottom_camera(&mat).await; } Ok(detections From 2279eeafcf4f0457bb3f952e2f47ddde29764983 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 10 Apr 2025 14:38:03 -0400 Subject: [PATCH 09/21] feat: implement detection annotated streams for all detectors --- src/missions/action_context.rs | 13 +++++++++++++ src/missions/vision.rs | 7 +++++++ 2 files changed, 20 insertions(+) diff --git a/src/missions/action_context.rs b/src/missions/action_context.rs index f9a13df3198..d2f9df10386 100644 --- a/src/missions/action_context.rs +++ b/src/missions/action_context.rs @@ -31,6 +31,8 @@ pub trait GetMainElectronicsBoard: Send + Sync { #[allow(async_fn_in_trait)] pub trait GetFrontCamMat { fn get_front_camera_mat(&self) -> impl std::future::Future + Send; + #[cfg(feature = "annotated_streams")] + async fn annotate_front_camera(&self, image: &impl ToInputArray); async fn get_desired_buoy_gate(&self) -> Target; async fn set_desired_buoy_gate(&mut self, value: Target) -> &Self; } @@ -41,6 +43,7 @@ pub trait GetFrontCamMat { #[allow(async_fn_in_trait)] pub trait GetBottomCamMat { async fn get_bottom_camera_mat(&self) -> Mat; + #[cfg(feature = "annotated_streams")] async fn annotate_bottom_camera(&self, image: &impl ToInputArray); } @@ -99,6 +102,10 @@ impl GetFrontCamMat for FullActionContext<'_, T async fn get_front_camera_mat(&self) -> Mat { self.front_cam.get_mat().await } + #[cfg(feature = "annotated_streams")] + async fn annotate_front_camera(&self, image: &impl ToInputArray) { + self.front_cam.push_annotated_frame(image); + } async fn get_desired_buoy_gate(&self) -> Target { let res = self.desired_buoy_target.read().await; (*res).clone() @@ -113,6 +120,7 @@ impl GetBottomCamMat for FullActionContext<'_, async fn get_bottom_camera_mat(&self) -> Mat { self.bottom_cam.get_mat().await } + #[cfg(feature = "annotated_streams")] async fn annotate_bottom_camera(&self, image: &impl ToInputArray) { self.bottom_cam.push_annotated_frame(image); } @@ -134,6 +142,10 @@ impl GetFrontCamMat for EmptyActionContext { async fn get_front_camera_mat(&self) -> Mat { todo!() } + #[cfg(feature = "annotated_streams")] + async fn annotate_front_camera(&self, image: &impl ToInputArray) { + todo!(); + } async fn get_desired_buoy_gate(&self) -> Target { todo!() } @@ -146,6 +158,7 @@ impl GetBottomCamMat for EmptyActionContext { async fn get_bottom_camera_mat(&self) -> Mat { todo!() } + #[cfg(feature = "annotated_streams")] async fn annotate_bottom_camera(&self, image: &impl ToInputArray) { todo!(); } diff --git a/src/missions/vision.rs b/src/missions/vision.rs index d5d49b3c960..341a75540a9 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -86,6 +86,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_front_camera(&mat).await; } let positions: Vec<_> = detections @@ -165,6 +167,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_bottom_camera(&mat).await; } let positions: Vec<_> = detections @@ -246,6 +250,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_front_camera(&mat).await; } Ok(detections @@ -322,6 +328,7 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] self.context.annotate_bottom_camera(&mat).await; } From bec06674ac2bc82c7367c15cc7ba6dd55c3ac9d2 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 10 Apr 2025 14:48:24 -0400 Subject: [PATCH 10/21] refactor: rename Get*CamMat to *CamIO to reflect addition of annotate_*_camera functions --- src/missions/action_context.rs | 12 ++++++------ src/missions/align_buoy.rs | 6 +++--- src/missions/buoy_hit.rs | 8 ++++---- src/missions/circle_buoy.rs | 8 ++++---- src/missions/coinflip.rs | 4 ++-- src/missions/fancy_octagon.rs | 4 ++-- src/missions/gate.rs | 12 ++++++------ src/missions/octagon.rs | 4 ++-- src/missions/path_align.rs | 8 ++------ src/missions/spin.rs | 8 ++------ src/missions/vision.rs | 16 ++++++++-------- 11 files changed, 41 insertions(+), 49 deletions(-) diff --git a/src/missions/action_context.rs b/src/missions/action_context.rs index d2f9df10386..51f4ad73ec5 100644 --- a/src/missions/action_context.rs +++ b/src/missions/action_context.rs @@ -29,7 +29,7 @@ pub trait GetMainElectronicsBoard: Send + Sync { * Inherit this trait if you have a front camera */ #[allow(async_fn_in_trait)] -pub trait GetFrontCamMat { +pub trait FrontCamIO { fn get_front_camera_mat(&self) -> impl std::future::Future + Send; #[cfg(feature = "annotated_streams")] async fn annotate_front_camera(&self, image: &impl ToInputArray); @@ -41,7 +41,7 @@ pub trait GetFrontCamMat { * Inherit this trait if you have a bottom camera */ #[allow(async_fn_in_trait)] -pub trait GetBottomCamMat { +pub trait BottomCamIO { async fn get_bottom_camera_mat(&self) -> Mat; #[cfg(feature = "annotated_streams")] async fn annotate_bottom_camera(&self, image: &impl ToInputArray); @@ -98,7 +98,7 @@ impl GetMainElectronicsBoard for FullActionContext<'_, WriteHalf> } } -impl GetFrontCamMat for FullActionContext<'_, T> { +impl FrontCamIO for FullActionContext<'_, T> { async fn get_front_camera_mat(&self) -> Mat { self.front_cam.get_mat().await } @@ -116,7 +116,7 @@ impl GetFrontCamMat for FullActionContext<'_, T } } -impl GetBottomCamMat for FullActionContext<'_, T> { +impl BottomCamIO for FullActionContext<'_, T> { async fn get_bottom_camera_mat(&self) -> Mat { self.bottom_cam.get_mat().await } @@ -138,7 +138,7 @@ impl GetMainElectronicsBoard for EmptyActionContext { } } -impl GetFrontCamMat for EmptyActionContext { +impl FrontCamIO for EmptyActionContext { async fn get_front_camera_mat(&self) -> Mat { todo!() } @@ -154,7 +154,7 @@ impl GetFrontCamMat for EmptyActionContext { } } -impl GetBottomCamMat for EmptyActionContext { +impl BottomCamIO for EmptyActionContext { async fn get_bottom_camera_mat(&self) -> Mat { todo!() } diff --git a/src/missions/align_buoy.rs b/src/missions/align_buoy.rs index 3e501aa937f..82e95598cd8 100644 --- a/src/missions/align_buoy.rs +++ b/src/missions/align_buoy.rs @@ -30,7 +30,7 @@ use crate::{ use super::{ action::ActionExec, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, }; pub fn buoy_align< @@ -38,7 +38,7 @@ pub fn buoy_align< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &'static Con, @@ -149,7 +149,7 @@ pub fn buoy_align_shot< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &'static Con, diff --git a/src/missions/buoy_hit.rs b/src/missions/buoy_hit.rs index 490ff329689..32d521f90d4 100644 --- a/src/missions/buoy_hit.rs +++ b/src/missions/buoy_hit.rs @@ -1,6 +1,6 @@ use super::{ action::{Action, ActionExec, ActionSequence, ActionWhile}, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, basic::DelayAction, movement::{StraightMovement, ZeroMovement}, }; @@ -54,7 +54,7 @@ impl Action for FindBuoy<'_, T> {} impl ActionExec> for FindBuoy<'_, T> where - T: GetControlBoard> + GetFrontCamMat + Sync + Unpin, + T: GetControlBoard> + FrontCamIO + Sync + Unpin, { async fn execute(&mut self) -> Result<()> { let camera_aquisition = self.context.get_front_camera_mat(); @@ -75,7 +75,7 @@ where } impl ActionExec> for DriveToBuoyVision<'_, T> where - T: GetControlBoard> + GetFrontCamMat + Sync + Unpin, + T: GetControlBoard> + FrontCamIO + Sync + Unpin, { async fn execute(&mut self) -> Result<()> { let camera_aquisition = self.context.get_front_camera_mat(); @@ -121,7 +121,7 @@ pub fn buoy_collision_sequence< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, T: Send + Sync, >( diff --git a/src/missions/circle_buoy.rs b/src/missions/circle_buoy.rs index d1332961f4e..0a7c7d8a4e0 100644 --- a/src/missions/circle_buoy.rs +++ b/src/missions/circle_buoy.rs @@ -21,7 +21,7 @@ use crate::{ use super::{ action::{ActionExec, ActionSequence}, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, basic::DelayAction, movement::ZeroMovement, }; @@ -35,7 +35,7 @@ pub fn buoy_circle_sequence< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &Con, @@ -95,7 +95,7 @@ pub fn buoy_circle_sequence_model< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &'static Con, @@ -144,7 +144,7 @@ pub fn buoy_circle_sequence_blind< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &'static Con, diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index e8fcfb122a5..66985338771 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -20,7 +20,7 @@ use super::{ wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionSequence, ActionWhile, FirstValid, }, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, basic::DelayAction, comms::StartBno055, extra::{CountTrue, OutputType}, @@ -33,7 +33,7 @@ pub fn coinflip< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat, + + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { diff --git a/src/missions/fancy_octagon.rs b/src/missions/fancy_octagon.rs index ec2cf96fc0c..9f350485034 100644 --- a/src/missions/fancy_octagon.rs +++ b/src/missions/fancy_octagon.rs @@ -29,7 +29,7 @@ use crate::{ use super::{ action::ActionExec, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, }; pub fn octagon_path_model() -> Path { @@ -55,7 +55,7 @@ pub fn fancy_octagon< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &'static Con, diff --git a/src/missions/gate.rs b/src/missions/gate.rs index 329a9514e76..ba5bf9b7bcf 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -24,7 +24,7 @@ use super::{ wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionMod, ActionSequence, ActionWhile, FirstValid, TupleSecond, }, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, basic::{descend_and_go_forward, DelayAction}, comms::StartBno055, extra::{CountFalse, CountTrue, OutputType}, @@ -40,7 +40,7 @@ pub fn gate_run_naive< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat, + + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { @@ -78,7 +78,7 @@ pub fn gate_run_complex< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat, + + FrontCamIO, >( context: &Con, ) -> impl ActionExec> + '_ { @@ -115,7 +115,7 @@ pub fn gate_run_coinflip< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat, + + FrontCamIO, >( context: &Con, ) -> impl ActionExec> + '_ { @@ -177,7 +177,7 @@ pub fn adjust_logic< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat, + + FrontCamIO, X: 'a + ActionMod + ActionExec>, >( context: &'a Con, @@ -279,7 +279,7 @@ pub fn gate_run_testing< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat, + + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { diff --git a/src/missions/octagon.rs b/src/missions/octagon.rs index 230cbfcc99a..3409a7fee93 100644 --- a/src/missions/octagon.rs +++ b/src/missions/octagon.rs @@ -26,7 +26,7 @@ use crate::{ use super::{ action::ActionExec, - action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard}, + action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, }; pub fn octagon_path_model() -> Octagon { @@ -38,7 +38,7 @@ pub fn octagon< + Sync + GetControlBoard> + GetMainElectronicsBoard - + GetFrontCamMat + + FrontCamIO + Unpin, >( context: &'static Con, diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index f8aecec5e14..ee691e6f59b 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -22,15 +22,11 @@ use crate::{ use super::{ action::ActionExec, - action_context::{GetBottomCamMat, GetControlBoard, GetMainElectronicsBoard}, + action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}, }; pub fn path_align< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + GetBottomCamMat, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { diff --git a/src/missions/spin.rs b/src/missions/spin.rs index 0b1449c3fa8..2700cd051f0 100644 --- a/src/missions/spin.rs +++ b/src/missions/spin.rs @@ -16,15 +16,11 @@ use crate::{ use super::{ action::{Action, ActionExec}, - action_context::{GetBottomCamMat, GetControlBoard, GetMainElectronicsBoard}, + action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}, }; pub fn spin< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + GetBottomCamMat, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { diff --git a/src/missions/vision.rs b/src/missions/vision.rs index 341a75540a9..e6f8c9924e3 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -5,7 +5,7 @@ use std::sync::RwLock; use std::{iter::Sum, marker::PhantomData}; use super::action::{Action, ActionExec, ActionMod}; -use super::action_context::GetBottomCamMat; +use super::action_context::BottomCamIO; use super::graph::DotString; use crate::logln; use crate::vision::nn_cv2::VisionModel; @@ -16,7 +16,7 @@ use num_traits::{Float, FromPrimitive, Num}; use opencv::core::{Mat, Rect2d}; use uuid::Uuid; -use crate::missions::action_context::GetFrontCamMat; +use crate::missions::action_context::FrontCamIO; #[cfg(feature = "logging")] use opencv::{core::Vector, imgcodecs::imwrite}; #[cfg(feature = "logging")] @@ -49,7 +49,7 @@ impl<'a, T, U, V> VisionNormOffset<'a, T, U, V> { impl Action for VisionNormOffset<'_, T, U, V> {} impl< - T: GetFrontCamMat + Send + Sync, + T: FrontCamIO + Send + Sync, V: Num + Float + FromPrimitive + Send + Sync, U: VisualDetector + Send + Sync, > ActionExec>> for VisionNormOffset<'_, T, U, V> @@ -130,7 +130,7 @@ impl<'a, T, U, V> VisionNormOffsetBottom<'a, T, U, V> { impl Action for VisionNormOffsetBottom<'_, T, U, V> {} impl< - T: GetBottomCamMat + Send + Sync, + T: BottomCamIO + Send + Sync, V: Num + Float + FromPrimitive + Send + Sync, U: VisualDetector + Send + Sync, > ActionExec>> for VisionNormOffsetBottom<'_, T, U, V> @@ -212,7 +212,7 @@ impl<'a, T, U, V> VisionNorm<'a, T, U, V> { impl Action for VisionNorm<'_, T, U, V> {} impl< - T: GetFrontCamMat + Send + Sync, + T: FrontCamIO + Send + Sync, V: Num + Float + FromPrimitive + Send + Sync, U: VisualDetector + Send + Sync, > ActionExec>>>> @@ -290,7 +290,7 @@ impl<'a, T, U, V> VisionNormBottom<'a, T, U, V> { impl Action for VisionNormBottom<'_, T, U, V> {} impl< - T: GetBottomCamMat + Send + Sync, + T: BottomCamIO + Send + Sync, V: Num + Float + FromPrimitive + Send + Sync, U: VisualDetector + Send + Sync, > ActionExec>>>> @@ -414,7 +414,7 @@ impl<'a, T, U, V> Vision<'a, T, U, V> { impl Action for Vision<'_, T, U, V> {} impl< - T: GetFrontCamMat + Send + Sync, + T: FrontCamIO + Send + Sync, V: Num + Float + FromPrimitive + Send + Sync, U: VisualDetector + Send + Sync, > ActionExec>>> for Vision<'_, T, U, V> @@ -457,7 +457,7 @@ impl<'a, T, U, V> VisionSizeLock<'a, T, U, V> { impl Action for VisionSizeLock<'_, T, U, V> {} impl< - T: GetFrontCamMat + Send + Sync, + T: FrontCamIO + Send + Sync, V: Num + Float + FromPrimitive + Send + Sync, U: VisualDetector + Send + Sync, > ActionExec>>> From eb7dab784ef22407fc284d0c6e1f770959d909ac Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 10 Apr 2025 16:06:08 -0400 Subject: [PATCH 11/21] feat: use hardware encoding for annotated streams --- src/video_source/appsink.rs | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/video_source/appsink.rs b/src/video_source/appsink.rs index b078a203ead..50005f3936a 100644 --- a/src/video_source/appsink.rs +++ b/src/video_source/appsink.rs @@ -58,8 +58,11 @@ impl Camera { + ".mp4\" "; #[cfg(feature = "annotated_streams")] - let output_string = "appsrc ! videoconvert ! x264enc key-int-max=30 insert-vui=1 tune=zerolatency ! mpegtsmux ! rtspclientsink location=rtspt://127.0.0.1:8554/".to_string() - + camera_name + "_annotated.mp4 "; + let output_string = "appsrc ! videoconvert ! ".to_string() + + &h264_enc_pipeline(2048000) + + " ! mpegtsmux ! rtspclientsink location=rtspt://127.0.0.1:8554/" + + camera_name + + "_annotated.mp4 "; let frame: Arc>> = Arc::default(); let frame_copy = frame.clone(); From df84e3271c710223dc958772aa4e5d6cd5c22d2d Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Mon, 14 Apr 2025 20:23:10 -0400 Subject: [PATCH 12/21] feat: changes from April 12th, 2025 pool test - Tune yaw PID - Start procedural rewrite of path align - Use Angle2D for path align - Was previously ignoring cv generated angle --- src/comms/control_board/mod.rs | 9 +- src/comms/control_board/response.rs | 4 +- src/main.rs | 19 +- src/missions/example.rs | 48 ++++- src/missions/movement.rs | 12 ++ src/missions/path_align.rs | 316 ++++++++++++++++------------ src/missions/vision.rs | 141 ++++++++++++- src/vision/mod.rs | 8 +- src/vision/path_cv.rs | 29 ++- 9 files changed, 426 insertions(+), 160 deletions(-) diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs index d6359177c56..7f792e08e03 100644 --- a/src/comms/control_board/mod.rs +++ b/src/comms/control_board/mod.rs @@ -43,7 +43,7 @@ fn stab_2_drift() -> f32 { loop { { let mut drift_val_inner = drift_val_clone.lock().unwrap(); - *drift_val_inner += 0.015; + // *drift_val_inner += 0.015; } sleep(Duration::from_secs(1)).await } @@ -157,7 +157,7 @@ impl ControlBoard { .await?; self.stability_assist_pid_tune('Y', 0.15, 0.0, 0.0, 0.1, false) .await?; - self.stability_assist_pid_tune('Z', 1.6, 1e-6, 0.0, 0.8, false) + self.stability_assist_pid_tune('Z', 22.0, 1.0e-18, 250.0, 1.0, false) .await?; self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false) .await @@ -341,7 +341,10 @@ impl ControlBoard { y, target_pitch, target_roll, - (target_yaw + stab_2_drift()), + ( + target_yaw + // + stab_2_drift() + ), target_depth, ] .iter() diff --git a/src/comms/control_board/response.rs b/src/comms/control_board/response.rs index 84953215b93..2764c3f29ad 100644 --- a/src/comms/control_board/response.rs +++ b/src/comms/control_board/response.rs @@ -130,7 +130,7 @@ impl ResponseMap { } else if message_body.get(0..7) == Some(&BNO055D) { static mut PREV_YAW_PRINT: SystemTime = SystemTime::UNIX_EPOCH; let new_status = message_body[7..].try_into().unwrap(); - /* + let now = SystemTime::now(); unsafe { if now.duration_since(PREV_YAW_PRINT).unwrap() > Duration::from_secs(1) { @@ -140,7 +140,7 @@ impl ResponseMap { PREV_YAW_PRINT = SystemTime::now(); } } - */ + *bno055_status.write().await = Some(new_status); } else if message_body.get(0..7) == Some(&MS5837D) { diff --git a/src/main.rs b/src/main.rs index 11759d0ac13..57efea00fb3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -19,13 +19,13 @@ use sw8s_rust_lib::{ buoy_circle_sequence, buoy_circle_sequence_blind, buoy_circle_sequence_model, }, coinflip::coinflip, - example::initial_descent, + example::{initial_descent, pid_test}, fancy_octagon::fancy_octagon, fire_torpedo::{FireLeftTorpedo, FireRightTorpedo}, gate::{gate_run_coinflip, gate_run_complex, gate_run_naive, gate_run_testing}, meb::WaitArm, octagon::octagon, - path_align::path_align, + path_align::path_align_procedural, reset_torpedo::ResetTorpedo, spin::spin, vision::PIPELINE_KILL, @@ -374,14 +374,13 @@ async fn run_mission(mission: &str) -> Result<()> { Ok(()) } "path_align" => { - let _ = path_align(&FullActionContext::new( + let _ = path_align_procedural(&FullActionContext::new( control_board().await, meb().await, front_cam().await, bottom_cam().await, gate_target().await, )) - .execute() .await; Ok(()) } @@ -411,6 +410,18 @@ async fn run_mission(mission: &str) -> Result<()> { .await; Ok(()) } + "pid_test" => { + let _ = pid_test(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute() + .await; + Ok(()) + } "octagon" => { let _ = octagon(static_context().await).execute().await; Ok(()) diff --git a/src/missions/example.rs b/src/missions/example.rs index d21a4e32142..0e1f332bce0 100644 --- a/src/missions/example.rs +++ b/src/missions/example.rs @@ -4,12 +4,16 @@ use tokio_serial::SerialStream; use crate::act_nest; use super::{ - action::{Action, ActionConcurrent, ActionConditional, ActionExec, ActionSequence, RaceAction}, - action_context::{GetControlBoard, GetMainElectronicsBoard}, + action::{ + Action, ActionChain, ActionConcurrent, ActionConditional, ActionExec, ActionSequence, + RaceAction, + }, + action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, basic::DelayAction, - extra::{AlwaysTrue, UnwrapAction}, + comms::StartBno055, + extra::{AlwaysTrue, OutputType, UnwrapAction}, meb::WaitArm, - movement::Descend, + movement::{Descend, Stability2Movement, Stability2Pos, ZeroMovement}, }; /// Example function for Action system @@ -32,6 +36,42 @@ where ) } +pub fn pid_test< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, +>( + context: &Con, +) -> impl ActionExec<()> + '_ { + const TIMEOUT: f32 = 30.0; + + let depth: f32 = -1.6; + + act_nest!( + ActionSequence::new, + ActionConcurrent::new( + ActionChain::new( + Stability2Movement::new( + context, + Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, depth), + ), + OutputType::<()>::default() + ), + StartBno055::new(context), + ), + act_nest!( + ActionSequence::new, + ActionChain::new(DelayAction::new(5.0), OutputType::<()>::default(),), + ActionChain::new( + Stability2Movement::new( + context, + Stability2Pos::new(0.0, 0.0, 0.0, 0.0, Some(45.0), depth), + ), + OutputType::<()>::default() + ), + DelayAction::new(10.0), + ), + ) +} + /// Example function for Action system /// /// Runs two nested actions in order: Waiting for arm and descending in diff --git a/src/missions/movement.rs b/src/missions/movement.rs index 8243bcb7cd7..e30485062ae 100644 --- a/src/missions/movement.rs +++ b/src/missions/movement.rs @@ -1,6 +1,7 @@ use crate::comms::control_board::ControlBoard; use crate::comms::control_board::LAST_YAW; use crate::logln; +use crate::vision::Angle2D; use crate::vision::DrawRect2d; use crate::vision::Offset2D; use crate::vision::RelPos; @@ -1369,6 +1370,17 @@ impl ActionExec for OffsetToPose> { } } +// Messes up type inference for all missions using offset2d +// impl ActionExec for OffsetToPose> { +// async fn execute(&mut self) -> Stability2Adjust { +// let mut adjust = Stability2Adjust::default(); +// adjust.set_x(AdjustType::Replace(*self.offset.x() as f32)); +// adjust.set_y(AdjustType::Replace(*self.offset.y() as f32)); +// adjust.set_target_yaw(AdjustType::Adjust(*self.offset.angle() as f32)); +// adjust +// } +// } + #[derive(Debug)] pub struct BoxToPose { input: T, diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index ee691e6f59b..24ca6e11915 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -11,13 +11,14 @@ use crate::{ basic::DelayAction, extra::{CountFalse, CountTrue, OutputType, Terminal, ToVec}, movement::{ - AdjustType, ClampX, FlipX, FlipY, LinearYawFromX, OffsetToPose, SetY, Stability2Adjust, + AdjustType, ClampX, FlipX, FlipY, OffsetToPose, SetY, Stability2Adjust, Stability2Movement, Stability2Pos, ZeroMovement, }, - vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, + vision::{ + DetectTarget, ExtractPosition, MidPoint, VisionNormBottom, VisionNormBottomAngle, + }, }, - vision::path::Path, - vision::path_cv::PathCV, + vision::{path::Path, path_cv::PathCV}, }; use super::{ @@ -25,144 +26,183 @@ use super::{ action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}, }; -pub fn path_align< +// pub fn path_align< +// Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, +// >( +// context: &Con, +// ) -> impl ActionExec<()> + '_ { +// const DEPTH: f32 = -1.25; +// const PATH_ALIGN_SPEED: f32 = 0.3; + +// act_nest!( +// ActionSequence::new, +// ZeroMovement::new(context, DEPTH), +// DelayAction::new(2.0), +// ActionWhile::new(ActionChain::new( +// VisionNormBottomAngle::::new(context, PathCV::default()), +// TupleSecond::new(ActionConcurrent::new( +// act_nest!( +// ActionChain::new, +// ActionDataConditional::new( +// DetectTarget::new(true), +// act_nest!( +// ActionChain::new, +// ExtractPosition::new(), +// MidPoint::new(), +// OffsetToPose::default(), +// ClampX::::new(0.3), +// FlipY::default(), +// Stability2Movement::new( +// context, +// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), +// ), +// ), +// act_nest!( +// ActionSequence::new, +// Terminal::new(), +// Stability2Movement::new( +// context, +// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), +// ), +// ), +// ), +// OutputType::<()>::new(), +// ), +// ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), +// )), +// )), +// ActionWhile::new(ActionChain::new( +// VisionNormBottom::::new(context, PathCV::default()), +// TupleSecond::new(ActionConcurrent::new( +// act_nest!( +// ActionChain::new, +// ActionDataConditional::new( +// DetectTarget::new(true), +// act_nest!( +// ActionChain::new, +// ExtractPosition::new(), +// MidPoint::new(), +// OffsetToPose::default(), +// ClampX::::new(0.3), +// FlipY::default(), +// Stability2Movement::new( +// context, +// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), +// ), +// ), +// act_nest!( +// ActionSequence::new, +// Terminal::new(), +// Stability2Movement::new( +// context, +// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), +// ), +// ), +// ), +// OutputType::<()>::new(), +// ), +// ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), +// )), +// )), +// // ActionWhile::new(ActionChain::new( +// // VisionNormBottom::::new(context, PathCV::default()), +// // TupleSecond::new(ActionConcurrent::new( +// // act_nest!( +// // ActionChain::new, +// // act_nest!( +// // ActionChain::new, +// // ActionDataConditional::new( +// // DetectTarget::new(true), +// // act_nest!( +// // ActionChain::new, +// // ExtractPosition::new(), +// // MidPoint::new(), +// // OffsetToPose::default(), +// // LinearYawFromX::::default(), +// // Stability2Movement::new( +// // context, +// // Stability2Pos::new( +// // 0.0, +// // PATH_ALIGN_SPEED / 2.0, +// // 0.0, +// // 0.0, +// // None, +// // DEPTH +// // ), +// // ), +// // ), +// // act_nest!( +// // ActionSequence::new, +// // Terminal::new(), +// // Stability2Movement::new( +// // context, +// // Stability2Pos::new( +// // 0.0, +// // PATH_ALIGN_SPEED / 2.0, +// // 0.0, +// // 0.0, +// // None, +// // DEPTH +// // ), +// // ), +// // ), +// // OutputType::<()>::new(), +// // ), +// // act_nest!( +// // ActionSequence::new, +// // Terminal::new(), +// // Stability2Movement::new( +// // context, +// // Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), +// // ), +// // ), +// // OutputType::<()>::new(), +// // ), +// // ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), +// // )), +// // )), +// Terminal::new(), +// ) +// } + +pub async fn path_align_procedural< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, -) -> impl ActionExec<()> + '_ { +) { const DEPTH: f32 = -1.25; const PATH_ALIGN_SPEED: f32 = 0.3; - act_nest!( - ActionSequence::new, - ZeroMovement::new(context, DEPTH), - DelayAction::new(2.0), - ActionWhile::new(ActionChain::new( - VisionNormBottom::::new(context, PathCV::default()), - TupleSecond::new(ActionConcurrent::new( - act_nest!( - ActionChain::new, - ActionDataConditional::new( - DetectTarget::new(true), - act_nest!( - ActionChain::new, - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::default(), - LinearYawFromX::::new(1.0), - ClampX::::new(0.3), - FlipY::default(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), - ), - ), - act_nest!( - ActionSequence::new, - Terminal::new(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), - ), - ), - ), - OutputType::<()>::new(), - ), - ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), - )), - )), - ActionWhile::new(ActionChain::new( - VisionNormBottom::::new(context, PathCV::default()), - TupleSecond::new(ActionConcurrent::new( - act_nest!( - ActionChain::new, - ActionDataConditional::new( - DetectTarget::new(true), - act_nest!( - ActionChain::new, - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::default(), - LinearYawFromX::::new(1.0), - ClampX::::new(0.3), - FlipY::default(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), - ), - // SetY::::new(AdjustType::Replace(PATH_ALIGN_SPEED)), - ), - act_nest!( - ActionSequence::new, - Terminal::new(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), - ), - ), - ), - OutputType::<()>::new(), - ), - ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), - )), - )), - // ActionWhile::new(ActionChain::new( - // VisionNormBottom::::new(context, PathCV::default()), - // TupleSecond::new(ActionConcurrent::new( - // act_nest!( - // ActionChain::new, - // act_nest!( - // ActionChain::new, - // ActionDataConditional::new( - // DetectTarget::new(true), - // act_nest!( - // ActionChain::new, - // ExtractPosition::new(), - // MidPoint::new(), - // OffsetToPose::default(), - // LinearYawFromX::::default(), - // Stability2Movement::new( - // context, - // Stability2Pos::new( - // 0.0, - // PATH_ALIGN_SPEED / 2.0, - // 0.0, - // 0.0, - // None, - // DEPTH - // ), - // ), - // ), - // act_nest!( - // ActionSequence::new, - // Terminal::new(), - // Stability2Movement::new( - // context, - // Stability2Pos::new( - // 0.0, - // PATH_ALIGN_SPEED / 2.0, - // 0.0, - // 0.0, - // None, - // DEPTH - // ), - // ), - // ), - // OutputType::<()>::new(), - // ), - // act_nest!( - // ActionSequence::new, - // Terminal::new(), - // Stability2Movement::new( - // context, - // Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), - // ), - // ), - // OutputType::<()>::new(), - // ), - // ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), - // )), - // )), - Terminal::new(), - ) + let mut visionNormBottom = + VisionNormBottomAngle::::new(context, PathCV::default()); + loop { + let detections = visionNormBottom.execute().await.unwrap(); + if let Some(detection) = detections + .into_iter() + .filter_map(|d| { + if *d.class() { + Some(d.position().clone()) + } else { + None + } + }) + .next() + { + let x = *detection.x() as f32; + let y = (*detection.y() as f32) * -1.0; + let angle = detection.angle(); + let cb = context.get_control_board(); + if let Some(current_angle) = cb.responses().get_angles().await { + cb.stability_2_speed_set( + x, + y, + 0.0, + 0.0, + (*current_angle.yaw() as f32) + *angle as f32, + DEPTH, + ) + .await; + } + } + } } diff --git a/src/missions/vision.rs b/src/missions/vision.rs index e6f8c9924e3..eb146ed3fd8 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -9,7 +9,9 @@ use super::action_context::BottomCamIO; use super::graph::DotString; use crate::logln; use crate::vision::nn_cv2::VisionModel; -use crate::vision::{Draw, DrawRect2d, Offset2D, RelPos, VisualDetection, VisualDetector}; +use crate::vision::{ + Angle2D, Draw, DrawRect2d, Offset2D, RelPos, RelPosAngle, VisualDetection, VisualDetector, +}; use anyhow::{anyhow, Result}; use num_traits::{Float, FromPrimitive, Num}; @@ -344,6 +346,84 @@ where } } +/// Runs a vision routine to obtain object positions +/// +/// The relative positions are normalized to [-1, 1] on both axes. +/// The values are returned with an angle. +#[derive(Debug)] +pub struct VisionNormBottomAngle<'a, T, U, V> { + context: &'a T, + model: U, + _num: PhantomData, +} + +impl<'a, T, U, V> VisionNormBottomAngle<'a, T, U, V> { + pub const fn new(context: &'a T, model: U) -> Self { + Self { + context, + model, + _num: PhantomData, + } + } +} + +impl Action for VisionNormBottomAngle<'_, T, U, V> {} + +impl< + T: BottomCamIO + Send + Sync, + V: Num + Float + FromPrimitive + Send + Sync, + U: VisualDetector + Send + Sync, + > ActionExec>>>> + for VisionNormBottomAngle<'_, T, U, V> +where + U::Position: RelPosAngle + Debug + for<'a> Mul<&'a Mat, Output = U::Position>, + VisualDetection: Draw, + U::ClassEnum: Send + Sync + Debug, +{ + async fn execute(&mut self) -> Result>>> { + #[cfg(feature = "logging")] + { + logln!("Running detection..."); + } + + #[allow(unused_mut)] + let mut mat = self.context.get_bottom_camera_mat().await.clone(); + let detections = self.model.detect(&mat); + #[cfg(feature = "logging")] + logln!("Detect attempt: {:#?}", detections); + let detections = detections?; + #[cfg(feature = "logging")] + { + detections.iter().for_each(|x| { + let x = VisualDetection::new( + x.class().clone(), + self.model.normalize(x.position()) * &mat, + ); + x.draw(&mut mat).unwrap() + }); + create_dir_all("/tmp/detect").unwrap(); + imwrite( + &("/tmp/detect/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"), + &mat, + &Vector::default(), + ) + .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_bottom_camera(&mat).await; + } + + Ok(detections + .into_iter() + .map(|detect| { + VisualDetection::new( + detect.class().clone(), + self.model.normalize(detect.position()).offset_angle(), + ) + }) + .collect()) + } +} + /// Normalizes vision output. /// /// The relative positions are normalized to [-1, 1] on both axes. @@ -772,6 +852,65 @@ impl ActionExec>> for MidPoint> { } } +impl ActionExec>> for MidPoint> { + async fn execute(&mut self) -> Option> { + if self.values.is_empty() { + None + } else { + let min_x = self + .values + .iter() + .map(|val| val.x()) + .cloned() + .reduce(f64::min) + .unwrap(); + let max_x = self + .values + .iter() + .map(|val| val.x()) + .cloned() + .reduce(f64::max) + .unwrap(); + let min_y = self + .values + .iter() + .map(|val| val.y()) + .cloned() + .reduce(f64::min) + .unwrap(); + let max_y = self + .values + .iter() + .map(|val| val.y()) + .cloned() + .reduce(f64::max) + .unwrap(); + let min_angle = self + .values + .iter() + .map(|val| val.angle()) + .cloned() + .reduce(f64::min) + .unwrap(); + let max_angle = self + .values + .iter() + .map(|val| val.angle()) + .cloned() + .reduce(f64::max) + .unwrap(); + + let val = Some(Angle2D::new( + (max_x + min_x) / 2.0, + (max_y + min_y) / 2.0, + (max_angle + min_angle) / 2.0, + )); + logln!("Processed this: {:#?}", val); + val + } + } +} + impl ActionMod> for MidPoint { fn modify(&mut self, input: &Vec) { self.values.clone_from(input); diff --git a/src/vision/mod.rs b/src/vision/mod.rs index 67bea5aabcc..c74b4de5ef5 100644 --- a/src/vision/mod.rs +++ b/src/vision/mod.rs @@ -110,13 +110,19 @@ impl> Draw for Offset2D { } /// Holds x, y, and angle offset of object in frame -#[derive(Debug, Getters)] +#[derive(Debug, Getters, Clone)] pub struct Angle2D { x: T, y: T, angle: T, } +impl Angle2D { + pub fn new(x: T, y: T, angle: T) -> Self { + Self { x, y, angle } + } +} + impl Add for Angle2D { type Output = Self; diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index 9f22b850980..6f144ddbc25 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -1,4 +1,7 @@ -use std::{fs::create_dir_all, ops::Mul, ops::RangeInclusive}; +use std::{ + fs::create_dir_all, + ops::{Mul, RangeInclusive}, +}; use derive_getters::Getters; use itertools::Itertools; @@ -72,6 +75,16 @@ impl Draw for VisualDetection { Scalar::from((0.0, 0.0, 255.0)) }; + let angle_rad = (*self.position.angle() as f32) * (3.14152965 / 180.0); + let b = (angle_rad.cos() * 640.0) / 2.0; + let a = (angle_rad.sin() * 480.0) / 2.0; + + let start = Point::new(*self.position.x() as i32, *self.position.y() as i32); + let end = Point::new( + *self.position.x() as i32 + a as i32, + *self.position.y() as i32 + b as i32, + ); + imgproc::circle( canvas, Point::new(*self.position.x() as i32, *self.position.y() as i32), @@ -82,6 +95,8 @@ impl Draw for VisualDetection { 0, )?; + imgproc::line(canvas, start, end, color, 2, LINE_8, 0)?; + // imgproc::arrowed_line( // canvas, // Point::new(*self.position.x() as i32, *self.position.y() as i32), @@ -331,12 +346,12 @@ impl VisualDetector for PathCV { let mut angle = rect.angle as f64; let width = rect.size.width; let height = rect.size.height; - if width > height { - angle = rect.angle as f64; - } else { - angle = (rect.angle as f64) + 90.0; - } - angle -= 90.0; + // if width > height { + // angle = rect.angle as f64; + // } else { + // angle = (rect.angle as f64) + 90.0; + // } + // angle -= 90.0; let mut box_rect = Mat::default(); box_points(rect, &mut box_rect)?; From a705f100124eb42680701a26a8f90d4cc67c900b Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 8 May 2025 04:40:32 -0400 Subject: [PATCH 13/21] feat: procedural path align implementation - Terminates based on the number of consecutive positive path detections - We should consider replacing this with an average angle error ceiling --- src/missions/path_align.rs | 93 +++++++++++++++++++++++++++----------- 1 file changed, 66 insertions(+), 27 deletions(-) diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 24ca6e11915..23ea633cbc2 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -172,37 +172,76 @@ pub async fn path_align_procedural< ) { const DEPTH: f32 = -1.25; const PATH_ALIGN_SPEED: f32 = 0.3; + const DETECTIONS: u8 = 10; - let mut visionNormBottom = + #[cfg(feature = "logging")] + logln!("Starting path align"); + + let cb = context.get_control_board(); + let mut vision_norm_bottom = VisionNormBottomAngle::::new(context, PathCV::default()); + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw() as f32; + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + cb.stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, initial_yaw, DEPTH) + .await; + + let mut last_set_yaw = initial_yaw; + let mut consec_detections = 0; + + #[cfg(feature = "logging")] + logln!("Starting path detection"); + loop { - let detections = visionNormBottom.execute().await.unwrap(); - if let Some(detection) = detections - .into_iter() - .filter_map(|d| { - if *d.class() { - Some(d.position().clone()) - } else { - None - } - }) - .next() - { - let x = *detection.x() as f32; - let y = (*detection.y() as f32) * -1.0; - let angle = detection.angle(); - let cb = context.get_control_board(); - if let Some(current_angle) = cb.responses().get_angles().await { - cb.stability_2_speed_set( - x, - y, - 0.0, - 0.0, - (*current_angle.yaw() as f32) + *angle as f32, - DEPTH, - ) - .await; + if consec_detections >= DETECTIONS { + #[cfg(feature = "logging")] + logln!("Finished path align"); + + break; + } + + if let Some(current_angle) = cb.responses().get_angles().await { + let current_yaw = *current_angle.yaw() as f32; + + // For the opencv impl of path detection, the returned vector is guaranteed to contain 1 item + let detections = vision_norm_bottom.execute().await.unwrap_or_else(|e| { + #[cfg(feature = "logging")] + logln!( + "Getting path detection resulted in error: `{e}`\n\tUsing empty detection vec" + ); + vec![] + }); + + let mut positions = detections + .into_iter() + .filter_map(|d| d.class().then_some(d.position().clone())); + + if let Some(position) = positions.next() { + let x = *position.x() as f32; + let y = (*position.y() as f32) * -1.0; + let angle = position.angle(); + last_set_yaw = current_yaw + *angle as f32; + cb.stability_2_speed_set(x, y, 0.0, 0.0, last_set_yaw, DEPTH) + .await; + consec_detections += 1; + } else { + cb.stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, last_set_yaw, DEPTH) + .await; + consec_detections = 0; } + } else { + #[cfg(feature = "logging")] + logln!("Failed to get current angle"); } + + #[cfg(feature = "logging")] + logln!("Positive detection count: {consec_detections}"); } } From 058e532eafd5e394dc3163000ecc1fbaafa57f93 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 8 May 2025 05:02:14 -0400 Subject: [PATCH 14/21] refactor: clean up path align --- src/missions/path_align.rs | 159 +------------------------------------ 1 file changed, 1 insertion(+), 158 deletions(-) diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 23ea633cbc2..03b2ba3762c 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -1,170 +1,13 @@ use tokio::io::WriteHalf; use tokio_serial::SerialStream; -use crate::{ - act_nest, - missions::{ - action::{ - ActionChain, ActionConcurrent, ActionDataConditional, ActionSequence, ActionWhile, - TupleSecond, - }, - basic::DelayAction, - extra::{CountFalse, CountTrue, OutputType, Terminal, ToVec}, - movement::{ - AdjustType, ClampX, FlipX, FlipY, OffsetToPose, SetY, Stability2Adjust, - Stability2Movement, Stability2Pos, ZeroMovement, - }, - vision::{ - DetectTarget, ExtractPosition, MidPoint, VisionNormBottom, VisionNormBottomAngle, - }, - }, - vision::{path::Path, path_cv::PathCV}, -}; +use crate::{act_nest, missions::vision::VisionNormBottomAngle, vision::path_cv::PathCV}; use super::{ action::ActionExec, action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}, }; -// pub fn path_align< -// Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, -// >( -// context: &Con, -// ) -> impl ActionExec<()> + '_ { -// const DEPTH: f32 = -1.25; -// const PATH_ALIGN_SPEED: f32 = 0.3; - -// act_nest!( -// ActionSequence::new, -// ZeroMovement::new(context, DEPTH), -// DelayAction::new(2.0), -// ActionWhile::new(ActionChain::new( -// VisionNormBottomAngle::::new(context, PathCV::default()), -// TupleSecond::new(ActionConcurrent::new( -// act_nest!( -// ActionChain::new, -// ActionDataConditional::new( -// DetectTarget::new(true), -// act_nest!( -// ActionChain::new, -// ExtractPosition::new(), -// MidPoint::new(), -// OffsetToPose::default(), -// ClampX::::new(0.3), -// FlipY::default(), -// Stability2Movement::new( -// context, -// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), -// ), -// ), -// act_nest!( -// ActionSequence::new, -// Terminal::new(), -// Stability2Movement::new( -// context, -// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), -// ), -// ), -// ), -// OutputType::<()>::new(), -// ), -// ActionChain::new(DetectTarget::new(true), CountTrue::new(10)), -// )), -// )), -// ActionWhile::new(ActionChain::new( -// VisionNormBottom::::new(context, PathCV::default()), -// TupleSecond::new(ActionConcurrent::new( -// act_nest!( -// ActionChain::new, -// ActionDataConditional::new( -// DetectTarget::new(true), -// act_nest!( -// ActionChain::new, -// ExtractPosition::new(), -// MidPoint::new(), -// OffsetToPose::default(), -// ClampX::::new(0.3), -// FlipY::default(), -// Stability2Movement::new( -// context, -// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), -// ), -// ), -// act_nest!( -// ActionSequence::new, -// Terminal::new(), -// Stability2Movement::new( -// context, -// Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, None, DEPTH), -// ), -// ), -// ), -// OutputType::<()>::new(), -// ), -// ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), -// )), -// )), -// // ActionWhile::new(ActionChain::new( -// // VisionNormBottom::::new(context, PathCV::default()), -// // TupleSecond::new(ActionConcurrent::new( -// // act_nest!( -// // ActionChain::new, -// // act_nest!( -// // ActionChain::new, -// // ActionDataConditional::new( -// // DetectTarget::new(true), -// // act_nest!( -// // ActionChain::new, -// // ExtractPosition::new(), -// // MidPoint::new(), -// // OffsetToPose::default(), -// // LinearYawFromX::::default(), -// // Stability2Movement::new( -// // context, -// // Stability2Pos::new( -// // 0.0, -// // PATH_ALIGN_SPEED / 2.0, -// // 0.0, -// // 0.0, -// // None, -// // DEPTH -// // ), -// // ), -// // ), -// // act_nest!( -// // ActionSequence::new, -// // Terminal::new(), -// // Stability2Movement::new( -// // context, -// // Stability2Pos::new( -// // 0.0, -// // PATH_ALIGN_SPEED / 2.0, -// // 0.0, -// // 0.0, -// // None, -// // DEPTH -// // ), -// // ), -// // ), -// // OutputType::<()>::new(), -// // ), -// // act_nest!( -// // ActionSequence::new, -// // Terminal::new(), -// // Stability2Movement::new( -// // context, -// // Stability2Pos::new(0.0, PATH_ALIGN_SPEED / 1.5, 0.0, 0.0, None, DEPTH), -// // ), -// // ), -// // OutputType::<()>::new(), -// // ), -// // ActionChain::new(DetectTarget::new(true), CountFalse::new(10)), -// // )), -// // )), -// Terminal::new(), -// ) -// } - pub async fn path_align_procedural< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( From 34407be03b004f1d04b21d6f04c4c52cbc7de760 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Thu, 8 May 2025 05:20:13 -0400 Subject: [PATCH 15/21] feat (path align): use results from sassist commands --- src/missions/path_align.rs | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 03b2ba3762c..80ef7376677 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -33,7 +33,8 @@ pub async fn path_align_procedural< } }; - cb.stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, initial_yaw, DEPTH) + let _ = cb + .stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, initial_yaw, DEPTH) .await; let mut last_set_yaw = initial_yaw; @@ -66,19 +67,32 @@ pub async fn path_align_procedural< .into_iter() .filter_map(|d| d.class().then_some(d.position().clone())); + let x; + let y; + let yaw; + if let Some(position) = positions.next() { - let x = *position.x() as f32; - let y = (*position.y() as f32) * -1.0; - let angle = position.angle(); - last_set_yaw = current_yaw + *angle as f32; - cb.stability_2_speed_set(x, y, 0.0, 0.0, last_set_yaw, DEPTH) - .await; + x = *position.x() as f32; + y = (*position.y() as f32) * -1.0; + yaw = current_yaw + *position.angle() as f32; + + last_set_yaw = yaw; consec_detections += 1; } else { - cb.stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, last_set_yaw, DEPTH) - .await; + x = 0.0; + y = PATH_ALIGN_SPEED; + yaw = last_set_yaw; + consec_detections = 0; } + + if let Err(e) = cb + .stability_2_speed_set(x, y, 0.0, 0.0, last_set_yaw, DEPTH) + .await + { + #[cfg(feature = "logging")] + logln!("SASSIST2 command to cb resulted in error: `{e}`"); + } } else { #[cfg(feature = "logging")] logln!("Failed to get current angle"); From 5b1e48f857fb049aa90ad3a75ae8717982cd5619 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Sat, 17 May 2025 12:31:34 -0400 Subject: [PATCH 16/21] new path cv impl --- src/vision/path_cv.rs | 78 +++++++++++++++++++++++++++++++++---------- 1 file changed, 60 insertions(+), 18 deletions(-) diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index 6f144ddbc25..386083ed2a0 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -255,14 +255,30 @@ impl VisualDetector for PathCV { let mut angle = rect.angle as f64; let width = rect.size.width; let height = rect.size.height; - if width > height { - angle = rect.angle as f64; - } else { - angle = (rect.angle as f64) + 90.0; + + let mut boxRect = Mat::default(); + imgproc::box_points(rect, &mut boxRect)?; + + let boxVec: Vec> = boxRect.to_vec_2d()?; + + let zero = boxVec[0].clone(); + let one = boxVec[1].clone(); + let two = boxVec[2].clone(); + + let edge1 = (one[0] - zero[0], one[1] - zero[1]); + let edge2 = (two[0] - one[0], two[1] - one[1]); + + // let longest_edge = max_by_key(edge1, edge2, |e| (e.0.powf(2.0) + e.1.powf(2.0)).sqrt()); + let edge1mag = (edge1.0.powf(2.0) + edge1.1.powf(2.0)).sqrt(); + let edge2mag = (edge2.0.powf(2.0) + edge2.1.powf(2.0)).sqrt(); + let longest_edge = if edge2mag > edge1mag { edge2 } else { edge1 }; + + let mut angle = (longest_edge.0 / longest_edge.1).atan().to_degrees() * -1.0; + + angle = ((angle + 180.0) % 360.0) - 180.0; + if angle < -90.0 { + angle += 180.0; } - angle -= 90.0; - let mut box_rect = Mat::default(); - box_points(rect, &mut box_rect)?; println!("{:?}", angle); @@ -271,7 +287,12 @@ impl VisualDetector for PathCV { Ok(vec![VisualDetection { class: true, - position: PosVector::new(center_adjusted_x, center_adjusted_y, 0., angle), + position: PosVector::new( + center_adjusted_x, + center_adjusted_y, + 0., + angle as f64, + ), }]) } else { Ok(vec![VisualDetection { @@ -341,19 +362,35 @@ impl VisualDetector for PathCV { if let Some(contour) = max_contour { let area = contour_area_def(&contour)?; - if area > 500.0 { + if area > 5000.0 { let rect = min_area_rect(&contour)?; let mut angle = rect.angle as f64; let width = rect.size.width; let height = rect.size.height; - // if width > height { - // angle = rect.angle as f64; - // } else { - // angle = (rect.angle as f64) + 90.0; - // } - // angle -= 90.0; - let mut box_rect = Mat::default(); - box_points(rect, &mut box_rect)?; + + let mut boxRect = Mat::default(); + imgproc::box_points(rect, &mut boxRect)?; + + let boxVec: Vec> = boxRect.to_vec_2d()?; + + let zero = boxVec[0].clone(); + let one = boxVec[1].clone(); + let two = boxVec[2].clone(); + + let edge1 = (one[0] - zero[0], one[1] - zero[1]); + let edge2 = (two[0] - one[0], two[1] - one[1]); + + // let longest_edge = max_by_key(edge1, edge2, |e| (e.0.powf(2.0) + e.1.powf(2.0)).sqrt()); + let edge1mag = (edge1.0.powf(2.0) + edge1.1.powf(2.0)).sqrt(); + let edge2mag = (edge2.0.powf(2.0) + edge2.1.powf(2.0)).sqrt(); + let longest_edge = if edge2mag > edge1mag { edge2 } else { edge1 }; + + let mut angle = (longest_edge.0 / longest_edge.1).atan().to_degrees() * -1.0; + + angle = ((angle + 180.0) % 360.0) - 180.0; + if angle < -90.0 { + angle += 180.0; + } println!("{:?}", angle); @@ -362,7 +399,12 @@ impl VisualDetector for PathCV { Ok(vec![VisualDetection { class: true, - position: PosVector::new(center_adjusted_x, center_adjusted_y, 0., angle), + position: PosVector::new( + center_adjusted_x, + center_adjusted_y, + 0., + angle as f64, + ), }]) } else { Ok(vec![VisualDetection { From 45b0e1a49fc90deac160b1207e59fc16aa54d7a1 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sat, 17 May 2025 13:18:13 -0400 Subject: [PATCH 17/21] feat: working path align --- src/comms/control_board/mod.rs | 2 +- src/missions/path_align.rs | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs index 7f792e08e03..e7fa44517e1 100644 --- a/src/comms/control_board/mod.rs +++ b/src/comms/control_board/mod.rs @@ -157,7 +157,7 @@ impl ControlBoard { .await?; self.stability_assist_pid_tune('Y', 0.15, 0.0, 0.0, 0.1, false) .await?; - self.stability_assist_pid_tune('Z', 22.0, 1.0e-18, 250.0, 1.0, false) + self.stability_assist_pid_tune('Z', 20.0, 1.0e-18, 500.0, 1.0, false) .await?; self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false) .await diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 80ef7376677..90acbd6ef65 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -1,4 +1,5 @@ use tokio::io::WriteHalf; +use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; use crate::{act_nest, missions::vision::VisionNormBottomAngle, vision::path_cv::PathCV}; @@ -21,6 +22,7 @@ pub async fn path_align_procedural< logln!("Starting path align"); let cb = context.get_control_board(); + cb.bno055_periodic_read(true).await; let mut vision_norm_bottom = VisionNormBottomAngle::::new(context, PathCV::default()); @@ -74,7 +76,7 @@ pub async fn path_align_procedural< if let Some(position) = positions.next() { x = *position.x() as f32; y = (*position.y() as f32) * -1.0; - yaw = current_yaw + *position.angle() as f32; + yaw = current_yaw + (*position.angle() * -1.0) as f32; last_set_yaw = yaw; consec_detections += 1; @@ -101,4 +103,7 @@ pub async fn path_align_procedural< #[cfg(feature = "logging")] logln!("Positive detection count: {consec_detections}"); } + cb.stability_2_speed_set(0.0, 1.0, 0.0, 0.0, last_set_yaw, DEPTH) + .await; + sleep(Duration::from_secs(1)).await; } From a1dd897367f392076f54c5043a1751bf5cacca28 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sat, 17 May 2025 17:30:41 -0400 Subject: [PATCH 18/21] fix (path align): skip sending sassist when nothing is detected Sending a command to move to the current angle is both unnecessary and caused oscillations (the current angle would have changed since the time of access) --- src/missions/path_align.rs | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 90acbd6ef65..ec633539018 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -39,6 +39,8 @@ pub async fn path_align_procedural< .stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, initial_yaw, DEPTH) .await; + // sleep(Duration::from_secs(10)).await; + let mut last_set_yaw = initial_yaw; let mut consec_detections = 0; @@ -81,11 +83,8 @@ pub async fn path_align_procedural< last_set_yaw = yaw; consec_detections += 1; } else { - x = 0.0; - y = PATH_ALIGN_SPEED; - yaw = last_set_yaw; - consec_detections = 0; + continue; } if let Err(e) = cb From 7bab6f62b06b32e6de028e21c0192a1346b443f8 Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sat, 17 May 2025 17:35:00 -0400 Subject: [PATCH 19/21] fix (WIP): yaw & roll pid tune --- src/comms/control_board/mod.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs index e7fa44517e1..d3e3b5dd515 100644 --- a/src/comms/control_board/mod.rs +++ b/src/comms/control_board/mod.rs @@ -155,9 +155,9 @@ impl ControlBoard { async fn stab_tune(&self) -> Result<()> { self.stability_assist_pid_tune('X', 0.8, 0.0, 0.0, 0.6, false) .await?; - self.stability_assist_pid_tune('Y', 0.15, 0.0, 0.0, 0.1, false) + self.stability_assist_pid_tune('Y', 2.0, 0.0, 0.0, 0.1, false) .await?; - self.stability_assist_pid_tune('Z', 20.0, 1.0e-18, 500.0, 1.0, false) + self.stability_assist_pid_tune('Z', 10.0, 0.0, 0.0, 1.0, false) .await?; self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false) .await From 2d9c37a6316d9c4dd061b9833c35ffbf9b7f4f3d Mon Sep 17 00:00:00 2001 From: Cowboylaserkittenjetshark <82691052+Cowboylaserkittenjetshark@users.noreply.github.com> Date: Sat, 17 May 2025 17:36:48 -0400 Subject: [PATCH 20/21] fix: tune gate to not skip path (also changed depths) --- src/missions/coinflip.rs | 10 +++------- src/missions/gate.rs | 40 ++++++++++------------------------------ 2 files changed, 13 insertions(+), 37 deletions(-) diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index 66985338771..17731cb1e30 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -20,7 +20,7 @@ use super::{ wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionSequence, ActionWhile, FirstValid, }, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, + action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, basic::DelayAction, comms::StartBno055, extra::{CountTrue, OutputType}, @@ -29,18 +29,14 @@ use super::{ }; pub fn coinflip< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { const TRUE_COUNT: u32 = 4; const DELAY_TIME: f32 = 3.0; - const DEPTH: f32 = -1.75; + const DEPTH: f32 = -1.25; const ALIGN_X_SPEED: f32 = 0.0; const ALIGN_Y_SPEED: f32 = 0.0; const ALIGN_YAW_SPEED: f32 = -3.0; diff --git a/src/missions/gate.rs b/src/missions/gate.rs index ba5bf9b7bcf..ea6e5e376a0 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -24,7 +24,7 @@ use super::{ wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionMod, ActionSequence, ActionWhile, FirstValid, TupleSecond, }, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, + action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, basic::{descend_and_go_forward, DelayAction}, comms::StartBno055, extra::{CountFalse, CountTrue, OutputType}, @@ -36,11 +36,7 @@ use super::{ }; pub fn gate_run_naive< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { @@ -74,11 +70,7 @@ pub fn gate_run_naive< } pub fn gate_run_complex< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec> + '_ { @@ -111,17 +103,13 @@ pub fn gate_run_complex< } pub fn gate_run_coinflip< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec> + '_ { const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.6; + let depth: f32 = -1.25; act_nest!( ActionSequence::new, @@ -156,16 +144,16 @@ pub fn gate_run_coinflip< DetectTarget::, Offset2D>::new(Target::Red), DetectTarget::, Offset2D>::new(Target::Pole), ), - CountFalse::new(5), + CountFalse::new(1), )), ActionChain::new( Stability2Movement::new( context, - Stability2Pos::new(0.0, 1.0, 0.0, 0.0, None, depth), + Stability2Pos::new(0.0, 0.5, 0.0, 0.0, None, depth), ), OutputType::<()>::default() ), - DelayAction::new(3.0), + DelayAction::new(0.0), ZeroMovement::new(context, depth), ), ) @@ -173,11 +161,7 @@ pub fn gate_run_coinflip< pub fn adjust_logic< 'a, - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, X: 'a + ActionMod + ActionExec>, >( context: &'a Con, @@ -275,11 +259,7 @@ pub fn adjust_logic< } pub fn gate_run_testing< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { From a53b96d5719634a00df1fa2d6d5f3d0b892534b7 Mon Sep 17 00:00:00 2001 From: SarangaR <59266397+SarangaR@users.noreply.github.com> Date: Wed, 11 Jun 2025 04:56:57 -0400 Subject: [PATCH 21/21] 05-17-2025 pool test; gate -> path tuning --- playstreams.sh | 2 ++ src/missions/coinflip.rs | 2 +- src/missions/gate.rs | 8 ++++---- src/missions/path_align.rs | 2 +- src/missions/spin.rs | 2 +- 5 files changed, 9 insertions(+), 7 deletions(-) create mode 100755 playstreams.sh diff --git a/playstreams.sh b/playstreams.sh new file mode 100755 index 00000000000..a9dc9f5e169 --- /dev/null +++ b/playstreams.sh @@ -0,0 +1,2 @@ +mpv rtsp://192.168.2.5:8554/front.mp4 --title=Front --no-cache --untimed --no-demuxer-thread --vd-lavc-threads=1 --profile=low-latency --no-correct-pts --osc=no & +mpv rtsp://192.168.2.5:8554/bottom.mp4 --title=Front --no-cache --untimed --no-demuxer-thread --vd-lavc-threads=1 --profile=low-latency --no-correct-pts --osc=no & diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index 17731cb1e30..5fe485b4c8b 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -36,7 +36,7 @@ pub fn coinflip< const TRUE_COUNT: u32 = 4; const DELAY_TIME: f32 = 3.0; - const DEPTH: f32 = -1.25; + const DEPTH: f32 = -1.6; const ALIGN_X_SPEED: f32 = 0.0; const ALIGN_Y_SPEED: f32 = 0.0; const ALIGN_YAW_SPEED: f32 = -3.0; diff --git a/src/missions/gate.rs b/src/missions/gate.rs index ea6e5e376a0..bc57f699c7d 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -109,7 +109,7 @@ pub fn gate_run_coinflip< ) -> impl ActionExec> + '_ { const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.25; + let depth: f32 = -1.75; act_nest!( ActionSequence::new, @@ -117,7 +117,7 @@ pub fn gate_run_coinflip< ActionChain::new( Stability2Movement::new( context, - Stability2Pos::new(0.0, 1.0, 0.0, 0.0, None, depth), + Stability2Pos::new(0.0, 0.5, 0.0, 0.0, None, depth), ), OutputType::<()>::default() ), @@ -130,7 +130,7 @@ pub fn gate_run_coinflip< ActionChain::new( Stability2Movement::new( context, - Stability2Pos::new(0.0, 1.0, 0.0, 0.0, None, depth), + Stability2Pos::new(0.0, 0.5, 0.0, 0.0, None, depth), ), OutputType::<()>::default() ), @@ -149,7 +149,7 @@ pub fn gate_run_coinflip< ActionChain::new( Stability2Movement::new( context, - Stability2Pos::new(0.0, 0.5, 0.0, 0.0, None, depth), + Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, depth), ), OutputType::<()>::default() ), diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index ec633539018..a59b543809c 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -14,7 +14,7 @@ pub async fn path_align_procedural< >( context: &Con, ) { - const DEPTH: f32 = -1.25; + const DEPTH: f32 = -1.6; const PATH_ALIGN_SPEED: f32 = 0.3; const DETECTIONS: u8 = 10; diff --git a/src/missions/spin.rs b/src/missions/spin.rs index 2700cd051f0..7932b1ee71a 100644 --- a/src/missions/spin.rs +++ b/src/missions/spin.rs @@ -39,7 +39,7 @@ pub fn spin< ), OutputType::<()>::new(), ), - DelayAction::new(6.0), + DelayAction::new(5.0), ActionWhile::new(TupleSecond::new(ActionConcurrent::new( act_nest!( ActionSequence::new,