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/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/comms/control_board/mod.rs b/src/comms/control_board/mod.rs index d6359177c56..d3e3b5dd515 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 } @@ -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', 1.6, 1e-6, 0.0, 0.8, 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 @@ -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/action_context.rs b/src/missions/action_context.rs index ebe1b78c654..51f4ad73ec5 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; @@ -28,8 +29,10 @@ 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); async fn get_desired_buoy_gate(&self) -> Target; async fn set_desired_buoy_gate(&mut self, value: Target) -> &Self; } @@ -38,8 +41,10 @@ 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); } /* @@ -93,10 +98,14 @@ 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 } + #[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() @@ -107,10 +116,14 @@ 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 } + #[cfg(feature = "annotated_streams")] + async fn annotate_bottom_camera(&self, image: &impl ToInputArray) { + self.bottom_cam.push_annotated_frame(image); + } } impl GetControlBoard> for EmptyActionContext { @@ -125,10 +138,14 @@ impl GetMainElectronicsBoard for EmptyActionContext { } } -impl GetFrontCamMat for EmptyActionContext { +impl FrontCamIO 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!() } @@ -137,8 +154,12 @@ impl GetFrontCamMat for EmptyActionContext { } } -impl GetBottomCamMat for EmptyActionContext { +impl BottomCamIO 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/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 276d96eab46..5fe485b4c8b 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::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, basic::DelayAction, comms::StartBno055, extra::{CountTrue, OutputType}, @@ -29,21 +29,17 @@ use super::{ }; pub fn coinflip< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + GetFrontCamMat, + 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.15; + const DEPTH: f32 = -1.6; 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/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/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 93c46ba5bb7..bc57f699c7d 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::{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 - + GetFrontCamMat, + 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 - + GetFrontCamMat, + 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 - + GetFrontCamMat, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec> + '_ { const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.0; + let depth: f32 = -1.75; act_nest!( ActionSequence::new, @@ -129,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() ), @@ -142,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() ), @@ -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.0, 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 - + GetFrontCamMat, + 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 - + GetFrontCamMat, + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( context: &Con, ) -> impl ActionExec<()> + '_ { diff --git a/src/missions/movement.rs b/src/missions/movement.rs index a5148b868d0..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; @@ -1076,6 +1077,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, @@ -1302,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/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 77ea658181f..a59b543809c 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -1,77 +1,108 @@ use tokio::io::WriteHalf; +use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; -use crate::{ - act_nest, - missions::{ - action::{ - ActionChain, ActionConcurrent, ActionDataConditional, ActionSequence, ActionWhile, - TupleSecond, - }, - basic::DelayAction, - extra::{CountTrue, OutputType, Terminal, ToVec}, - movement::{ - LinearYawFromX, OffsetToPose, Stability2Adjust, Stability2Movement, Stability2Pos, - ZeroMovement, - }, - vision::{DetectTarget, ExtractPosition, MidPoint, VisionNormBottom}, - }, - vision::path::Path, -}; +use crate::{act_nest, missions::vision::VisionNormBottomAngle, vision::path_cv::PathCV}; use super::{ action::ActionExec, - action_context::{GetBottomCamMat, GetControlBoard, GetMainElectronicsBoard}, + action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}, }; -pub fn path_align< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + GetBottomCamMat, +pub async fn path_align_procedural< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, -) -> impl ActionExec<()> + '_ { - const DEPTH: f32 = -1.25; +) { + const DEPTH: f32 = -1.6; const PATH_ALIGN_SPEED: f32 = 0.3; + const DETECTIONS: u8 = 10; + + #[cfg(feature = "logging")] + 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()); + + 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"); + } + }; + + let _ = cb + .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; + + #[cfg(feature = "logging")] + logln!("Starting path detection"); + + loop { + 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())); + + let x; + let y; + let yaw; + + if let Some(position) = positions.next() { + x = *position.x() as f32; + y = (*position.y() as f32) * -1.0; + yaw = current_yaw + (*position.angle() * -1.0) as f32; + + last_set_yaw = yaw; + consec_detections += 1; + } else { + consec_detections = 0; + continue; + } + + 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"); + } - act_nest!( - ActionSequence::new, - ZeroMovement::new(context, DEPTH), - DelayAction::new(2.0), - ActionWhile::new(ActionChain::new( - VisionNormBottom::::new(context, Path::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::::default(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 30.0, 0.0, None, DEPTH), - ), - ), - act_nest!( - ActionSequence::new, - Terminal::new(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, PATH_ALIGN_SPEED, 30.0, 0.0, None, DEPTH), - ), - ), - ), - OutputType::<()>::new(), - ), - ActionChain::new(DetectTarget::new(true), CountTrue::new(5)), - )), - )), - Terminal::new(), - ) + #[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; } diff --git a/src/missions/spin.rs b/src/missions/spin.rs index 0b1449c3fa8..7932b1ee71a 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<()> + '_ { @@ -43,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, diff --git a/src/missions/vision.rs b/src/missions/vision.rs index 7ef8f32dc02..eb146ed3fd8 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -5,18 +5,20 @@ 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; -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}; 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 +51,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> @@ -86,6 +88,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_front_camera(&mat).await; } let positions: Vec<_> = detections @@ -128,7 +132,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> @@ -165,6 +169,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_bottom_camera(&mat).await; } let positions: Vec<_> = detections @@ -208,7 +214,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>>>> @@ -246,6 +252,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_front_camera(&mat).await; } Ok(detections @@ -284,7 +292,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>>>> @@ -322,6 +330,8 @@ where &Vector::default(), ) .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_bottom_camera(&mat).await; } Ok(detections @@ -336,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. @@ -406,7 +494,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> @@ -449,7 +537,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>>> @@ -764,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/video_source/appsink.rs b/src/video_source/appsink.rs index fec4e1f3b81..50005f3936a 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,51 @@ impl Camera { + camera_name + ".mp4\" "; + #[cfg(feature = "annotated_streams")] + 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(); + #[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 +110,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/mod.rs b/src/vision/mod.rs index dc14d541cfc..c74b4de5ef5 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; @@ -109,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 new file mode 100644 index 00000000000..386083ed2a0 --- /dev/null +++ b/src/vision/path_cv.rs @@ -0,0 +1,468 @@ +use std::{ + fs::create_dir_all, + ops::{Mul, RangeInclusive}, +}; + +use derive_getters::Getters; +use itertools::Itertools; +use opencv::{ + core::{in_range, Point, Scalar, Size, VecN, Vector}, + imgcodecs::imwrite, + 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}, + 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)) + }; + + 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), + 10, + color, + 2, + LINE_8, + 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), + // 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, + 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: 175 })..=(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_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 > 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; + + 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); + + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new( + center_adjusted_x, + center_adjusted_y, + 0., + angle as f64, + ), + }]) + } 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 { + 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, + 0., + *pos.angle(), + ) + } +} + +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_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 > 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; + + 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); + + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new( + center_adjusted_x, + center_adjusted_y, + 0., + angle as f64, + ), + }]) + } 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 { + 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, + 0., + *pos.angle(), + ) + } +} + +#[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(); + } +}