diff --git a/Dockerfile b/Dockerfile index b9627b75055..a375a3065dc 100644 --- a/Dockerfile +++ b/Dockerfile @@ -29,7 +29,7 @@ RUN apt-get install nano # User setup USER aquapack WORKDIR /home/aquapack -RUN \ +# RUN \ # Install Rust via rustup (apt's Rust is too old) curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y \ && . "$HOME/.cargo/env" && echo '. "$HOME/.cargo/env"' >> ~/.bashrc \ diff --git a/build.rs b/build.rs index 3fe230eb977..967b2d3c253 100644 --- a/build.rs +++ b/build.rs @@ -1,180 +1,26 @@ -#[cfg(feature = "graphing")] -mod graphing { - use proc_macro2::Span; - use quote::quote; - use std::env::var_os; - use std::ffi::OsStr; - use std::fs::{create_dir_all, read_dir, read_to_string, write}; - use std::path::{Path, PathBuf}; - use syn::fold::{fold_item_fn, fold_use_path, Fold}; - - use syn::{ - parse_file, Ident, ItemFn, ReturnType, Token, Type, TypeParamBound, UsePath, UseTree, - }; - - struct ReplaceActionExec<'a> { - actions: &'a mut Vec, - } - - impl<'a> ReplaceActionExec<'a> { - fn new(actions: &'a mut Vec) -> Self { - Self { actions } - } - } - - impl Fold for ReplaceActionExec<'_> { - // Replace every impl ActionExec with impl Action - // Uses GraphAction to always be sure Action is imported - // Also strips generics - fn fold_item_fn(&mut self, i: ItemFn) -> ItemFn { - let mut j = i.clone(); - if let ReturnType::Type(_, ref mut ret_type) = j.sig.output { - if let Type::ImplTrait(ref mut impl_trait) = **ret_type { - impl_trait.bounds.iter_mut().for_each(|bound| { - if let TypeParamBound::Trait(ref mut t) = bound { - t.path.segments.iter_mut().for_each(|seg| { - if seg.ident == "ActionExec" { - seg.ident = Ident::new("GraphActionExec", seg.ident.span()); - - if j.sig.inputs.len() == 1 { - self.actions.push(j.sig.ident.to_string()); - } - - /* - j.sig.generics.type_params_mut().for_each(|param| { - if param.ident == "Con" { - param.bounds = Punctuated::new() - } - }); - */ - } - }); - } - }); - } - }; - // Call on the regular recursion - fold_item_fn(self, j) - } - - // Replace local paths with global paths - fn fold_use_path(&mut self, i: syn::UsePath) -> syn::UsePath { - println!("Folding path segment: {:?}", i.ident); - let mut j = i.clone(); - match j.ident.to_string().as_str() { - "crate" => j.ident = Ident::new("sw8s_rust_lib", j.ident.span()), - "super" => { - j.ident = Ident::new("sw8s_rust_lib", j.ident.span()); - j.tree = Box::new(UseTree::Path(UsePath { - ident: Ident::new("missions", Span::call_site()), - colon2_token: Token![::](Span::call_site()), - tree: j.tree, - })); - } - _ => (), - } - // Call on the regular recursion - fold_use_path(self, j) - } - } - - fn get_files(file: PathBuf) -> Vec { - if file.is_file() { - vec![file] - } else if file.is_dir() { - read_dir(file) - .unwrap() - .flat_map(|inner| get_files(inner.unwrap().path())) - .collect() - } else { - vec![] - } - } - - pub fn graphing_variants() { - // Command to cargo - println!("cargo:rerun-if-changed=src/missions"); - - // Calculating output dir - let out_dir = var_os("OUT_DIR").unwrap().to_str().unwrap().to_string(); - println!("out_dir: {out_dir}"); - let out_path_raw = out_dir + "/graph_missions"; - let out_path = Path::new(&out_path_raw); - - // Get parseable file clones - let mission_files = get_files("src/missions".into()) - .into_iter() - .filter(|path| path.extension().unwrap_or(OsStr::new("")) == "rs") - .map(|path| { - println!("Path: {:?}", path); - ( - path.clone(), - parse_file(&read_to_string(path).unwrap()).unwrap().clone(), - ) - }); - - // Modify clones and write to output dir - mission_files - .map(|(path, file)| { - let mut actions = vec![]; - ( - path, - ReplaceActionExec::new(&mut actions).fold_file(file), - actions, - ) - }) - .for_each(|(path, file, actions)| { - let actions_str = - "pub fn graph_actions> + GraphActionContext::GetFrontCamMat + GraphActionContext::GetBottomCamMat + Send + Sync + std::marker::Unpin>(context: &'static T) -> Vec<(String, Box)> { vec![" - .to_string() - + &actions - .into_iter() - .fold("".to_string(), |acc, x| acc + &format!("(\"{x}\".to_string(), Box::new({x}(context))),")) - + "]}"; - let file_contents = - quote! { use sw8s_rust_lib::missions::action::Action as GraphAction; use sw8s_rust_lib::missions::action::ActionExec as GraphActionExec; use sw8s_rust_lib::missions::action_context as GraphActionContext; #file }; - let output_loc = out_path.join(path.strip_prefix::("src/missions".into()).unwrap()); - create_dir_all(output_loc.parent().unwrap()).unwrap(); - write( - out_path.join(path.strip_prefix::("src/missions".into()).unwrap()), - format!("{} {}", file_contents, actions_str), - ) - .unwrap() - }); - } -} - -fn main() { - #[cfg(feature = "graphing")] - graphing::graphing_variants(); - - #[cfg(feature = "cuda")] - { - // https://arnon.dk/matching-sm-architectures-arch-and-gencode-for-various-nvidia-cards/ - const COMPUTE_CODES: &[&str] = &[ - "52", "53", "60", "61", "62", "70", "72", "75", "80", "86", "87", "89", "90", "90a", - ]; - - // Rebuild on any kernel change - println!("cargo:rerun-if-changed=src/cuda_kernels"); - - // Rebuild for specific files that use kernels changing - println!("cargo:rerun-if-changed=src/vision/nn_cv2.rs"); - - let mut build = cc::Build::new(); - build.cuda(true).flag("-cudart=shared"); - - for code in COMPUTE_CODES { - build - .flag("-gencode") - .flag(&format!("arch=compute_{},code=sm_{}", code, code)); - } - - // Specify all cuda kernels that need to be built - build - .file("src/cuda_kernels/process_net.cu") - .compile("libsw8s_cuda.a"); - - println!("cargo:rustc-link-lib=cudart"); - } -} +// fn main() { +// // #[cfg(feature = "cuda")] +// // { +// // // https://arnon.dk/matching-sm-architectures-arch-and-gencode-for-various-nvidia-cards/ +// // const COMPUTE_CODES: &[&str] = &[ +// // "52", "53", "60", "61", "62", "70", "72", "75", "80", "86", "87", "89", "90", "90a", +// // ]; + +// // // Rebuild on any kernel change +// // println!("cargo:rerun-if-changed=src/cuda_kernels"); + +// // // Rebuild for specific files that use kernels changing +// // println!("cargo:rerun-if-changed=src/vision/nn_cv2.rs"); + +// // let mut build = cc::Build::new(); +// // build.cuda(true).flag("-cudart=shared"); + +// // for code in COMPUTE_CODES { +// // build +// // .flag("-gencode") +// // .flag(&format!("arch=compute_{},code=sm_{}", code, code)); +// // } + +// // println!("cargo:rustc-link-lib=cudart"); +// // } +// } diff --git a/front b/front deleted file mode 100644 index e6b61711b8e..00000000000 Binary files a/front and /dev/null differ diff --git a/model_calib_data/gate_poles.png b/model_calib_data/gate_poles.png deleted file mode 100644 index 4d5a2667fce..00000000000 Binary files a/model_calib_data/gate_poles.png and /dev/null differ diff --git a/scripts/seawolf b/scripts/seawolf deleted file mode 100755 index 396a8dd41a6..00000000000 --- a/scripts/seawolf +++ /dev/null @@ -1,52 +0,0 @@ -#!/bin/bash - -build() { - cargo build -} - -build-jetson() { - cd /workspaces/SW8S-Rust/jetson/ && cargo run --all-features -} - -run() { - cargo run -} - -help() { - echo "Usage:" - echo " build:" - echo " \"\" -- Builds locally using cargo" - echo " jetson -- Cross compiles for the Jetson (arm64)" - echo " local -- Compiles for the local machine (x86_64)" - echo " run -- Runs the project using cargo " - echo " " - echo "" - echo "Usage: seawolf (build | run)" -} - -case $1 in - "run") - run - ;; - "build") - case $2 in - "jetson") - build-jetson - ;; - "local") - build - ;; - "") - build - ;; - *) - echo "Unknown option: \"$2\"" - help - ;; - esac - ;; - *) - echo "Unknown option: \"$1\"" - help - ;; -esac \ No newline at end of file diff --git a/src/comms/auv_control_board/response.rs b/src/comms/auv_control_board/response.rs index b6ca0bc722a..5d89d7efa4f 100644 --- a/src/comms/auv_control_board/response.rs +++ b/src/comms/auv_control_board/response.rs @@ -105,7 +105,7 @@ where } */ - #[cfg(all(feature = "logging", not(feature = "unblocked_logging")))] + #[cfg(all(feature = "logging"))] { write_log(&[buffer.clone()], dump_file).await; } diff --git a/src/config/mod.rs b/src/config/mod.rs index 624217c6f4d..c5d521cc03e 100644 --- a/src/config/mod.rs +++ b/src/config/mod.rs @@ -5,6 +5,7 @@ pub mod octagon; pub mod path_align; pub mod slalom; pub mod sonar; +pub mod spin; use std::fs::read_to_string; @@ -78,6 +79,7 @@ pub struct Missions { pub bin: bin::Config, pub octagon: octagon::Config, pub coinflip: coinflip::Config, + pub spin: spin::Config, } #[derive(Debug, Serialize, Deserialize)] diff --git a/src/config/spin.rs b/src/config/spin.rs new file mode 100644 index 00000000000..e79adf9417e --- /dev/null +++ b/src/config/spin.rs @@ -0,0 +1,20 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Config { + pub depth: f32, + pub spin_speed: f32, + pub num_spins: i32, + pub hysteresis: f32, +} + +impl Default for Config { + fn default() -> Self { + Self { + depth: -1.25, + spin_speed: 1.0, + num_spins: 2, + hysteresis: 10.0, + } + } +} diff --git a/src/cuda_kernels/process_net.cu b/src/cuda_kernels/process_net.cu deleted file mode 100644 index 8dac2f872c6..00000000000 --- a/src/cuda_kernels/process_net.cu +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include - -#define MAX_THREADS (128) -#define WARP_SIZE (32) - -struct CudaFormatMat { - int32_t rows; - int32_t cols; - float *bytes; -}; -struct YoloDetectionCuda { - double confidence; - double x; - double y; - double width; - double height; - int32_t class_id; -}; - -__forceinline__ __device__ float adjust_base(uintptr_t idx, float const factor, - float const *row_bytes) { - return row_bytes[idx] * factor; -} - -__forceinline__ __device__ float x_adjust(uintptr_t idx, float const factor, - float const *row_bytes) { - return (adjust_base(idx, factor, row_bytes) / 640.0) * 800.0; -} - -__forceinline__ __device__ float y_adjust(uintptr_t idx, float const factor, - float const *row_bytes) { - return (adjust_base(idx, factor, row_bytes) / 640.0) * 600.0; -} - -__global__ void process_net(const uintptr_t num_rows, const uintptr_t num_cols, - const float threshold, const float factor, - const float *__restrict__ mat_bytes, - YoloDetectionCuda *__restrict__ processed_detects, - bool *__restrict__ processed_valid) { - const auto id = blockIdx.x * blockDim.x + threadIdx.x; - - // Get rid of leftover threads - if (id >= num_rows) - return; - - float const *__restrict__ row = mat_bytes + (id * num_cols); - - const float confidence = row[4]; - const bool valid = confidence > threshold; - processed_valid[id] = valid; - - // Skip remaining processing for invalid - if (!valid) - return; - - // Start at offset in data, then shift to starting at 0. - uintptr_t class_id = 5; - float class_value = row[class_id]; - for (uintptr_t i = 6; i < num_cols; ++i) { - if (class_value < row[i]) { - class_id = i; - class_value = row[i]; - } - } - class_id -= 5; - - const float center_x = x_adjust(0, factor, row); - const float center_y = y_adjust(1, factor, row); - const float width = x_adjust(2, factor, row); - const float height = y_adjust(3, factor, row); - - const float left = center_x - (width / 2.0); - const float top = center_y - (height / 2.0); - - processed_detects[id] = YoloDetectionCuda{ - confidence, left, top, width, height, static_cast(class_id)}; -} - -extern "C" { -int process_net_kernel(CudaFormatMat *const result, uintptr_t const num_levels, - float const threshold, float const factor, - uintptr_t const total_rows, - YoloDetectionCuda *processed_detects, - bool *processed_valid) { - - /* -cudaStream_t kernel_stream; -cudaStreamCreate(&kernel_stream); -*/ - - YoloDetectionCuda *processed_detects_cuda; - bool *processed_valid_cuda; - cudaMalloc(&processed_detects_cuda, sizeof(YoloDetectionCuda) * total_rows); - cudaMalloc(&processed_valid_cuda, sizeof(bool) * total_rows); - /* - - uintptr_t row_offset = 0; - for (uintptr_t i = 0; i < num_levels; ++i) { - CudaFormatMat *mat = result + i; - auto num_rows = mat->rows; - uintptr_t num_cols = static_cast(mat->cols); - auto mat_size = num_rows * num_cols * sizeof(float); - float *mat_bytes; - - cudaMalloc(&mat_bytes, mat_size); - cudaMemcpy(mat_bytes, mat->bytes, mat_size, cudaMemcpyHostToDevice); - - int32_t blocksize = MAX_THREADS; - int32_t block_count; - if (num_rows < blocksize) { - blocksize = num_rows; - block_count = 1; - } else { - // Ceiling divide, from https://stackoverflow.com/a/14878734 - block_count = num_rows / MAX_THREADS + (num_rows % MAX_THREADS != 0); - } - - process_net<<>>( - num_rows, num_cols, threshold, factor, mat_bytes, - processed_detects_cuda + row_offset, processed_valid_cuda + row_offset); - - cudaStreamSynchronize(kernel_stream); - cudaFree(mat_bytes); - - row_offset += num_rows; - } - - cudaMemcpy(processed_detects, processed_detects_cuda, - sizeof(YoloDetectionCuda) * total_rows, cudaMemcpyDeviceToHost); - cudaMemcpy(processed_valid, processed_valid_cuda, sizeof(bool) * total_rows, - cudaMemcpyDeviceToHost); - */ - cudaFree(processed_detects_cuda); - cudaFree(processed_valid_cuda); - /* - cudaStreamDestroy(kernel_stream); - */ - - return 0; -} -} diff --git a/src/main.rs b/src/main.rs index 40d3bbfea3e..a575a7986cf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -14,19 +14,12 @@ use sw8s_rust_lib::{ missions::{ action::ActionExec, action_context::FullActionContext, - align_buoy::{buoy_align, buoy_align_shot}, basic::descend_and_go_forward, bin::bin, - circle_buoy::{ - buoy_circle_sequence, buoy_circle_sequence_blind, buoy_circle_sequence_model, - }, - coinflip::{coinflip, coinflip_procedural}, + coinflip::coinflip_procedural, example::{initial_descent, pid_test}, fire_torpedo::{FireLeftTorpedo, FireRightTorpedo}, - gate::{ - gate_run_complex, gate_run_cv_procedural, gate_run_dead_reckon, gate_run_naive, - gate_run_procedural, gate_run_testing, - }, + gate::{gate_run_cv_procedural, gate_run_dead_reckon, gate_run_procedural}, meb::WaitArm, octagon::octagon, path_align::{path_align_procedural, static_align_procedural}, @@ -37,7 +30,6 @@ use sw8s_rust_lib::{ vision::PIPELINE_KILL, }, video_source::appsink::Camera, - vision::buoy::Target, TIMESTAMP, }; use tokio::{ @@ -45,7 +37,7 @@ use tokio::{ signal, sync::{ mpsc::{self, UnboundedSender}, - OnceCell, RwLock, Semaphore, + OnceCell, Semaphore, }, time::{sleep, timeout}, }; @@ -129,13 +121,6 @@ async fn bottom_cam() -> &'static Camera { .await } -static GATE_TARGET: OnceCell> = OnceCell::const_new(); -async fn gate_target() -> &'static RwLock { - GATE_TARGET - .get_or_init(|| async { RwLock::new(Target::Earth1) }) - .await -} - static STATIC_CONTEXT: OnceCell>> = OnceCell::const_new(); async fn static_context() -> &'static FullActionContext<'static, WriteHalf> { STATIC_CONTEXT @@ -145,7 +130,6 @@ async fn static_context() -> &'static FullActionContext<'static, WriteHalf Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, - )) - .execute()), - "gate_run_naive" => ctwrap!(gate_run_naive(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - )) - .execute()), - "gate_run_complex" => ctwrap!(gate_run_complex(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, )) .execute()), "gate_run_coinflip" => ctwrap!(gate_run_cv_procedural( @@ -394,7 +361,6 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, ), &config.missions.gate, &config.get_color_profile().unwrap(), @@ -405,7 +371,6 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, ), &config.missions.gate )), @@ -415,19 +380,9 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, ), &config.missions.gate, - &config.get_color_profile().unwrap(), )), - "gate_run_testing" => ctwrap!(gate_run_testing(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - )) - .execute()), "start_cam" => { // This has not been tested logln!("Opening camera"); @@ -442,7 +397,6 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, ), &config.missions.path_align, &config.get_color_profile().unwrap(), @@ -453,7 +407,6 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, ), &config.missions.path_align, )), @@ -462,7 +415,6 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, )) .execute()), "pid_test" => ctwrap!(pid_test(&FullActionContext::new( @@ -470,7 +422,6 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { meb().await, front_cam().await, bottom_cam().await, - gate_target().await, )) .execute()), "octagon" => ctwrap!(octagon( @@ -479,22 +430,7 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { &config.get_color_profile().unwrap() ) .execute()), - "buoy_circle" => ctwrap!(buoy_circle_sequence(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - )) - .execute()), - "buoy_model" => ctwrap!(buoy_circle_sequence_model(static_context().await).execute()), - "buoy_blind" => ctwrap!(buoy_circle_sequence_blind(static_context().await).execute()), - "buoy_align" => ctwrap!(buoy_align(static_context().await).execute()), - "spin" => ctwrap!(spin(static_context().await).execute()), - "torpedo" | "fire_torpedo" => { - let _ = buoy_align_shot(static_context().await).execute().await; - Ok(()) - } + "spin" => ctwrap!(spin(static_context().await, &config.missions.spin)), "torpedo_only" => { FireRightTorpedo::new(static_context().await) .execute() @@ -538,7 +474,7 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { let _ = sonar(static_context().await, &config.sonar, cancel).await; Ok(()) } - "bin" => ctwrap!(bin(static_context().await, &config.missions.bin)), + "bin" => ctwrap!(bin(static_context().await)), x => bail!("Invalid argument: [{x}]"), }; diff --git a/src/missions/action_context.rs b/src/missions/action_context.rs index 2af13b5fb4c..1de8e3f1b9a 100644 --- a/src/missions/action_context.rs +++ b/src/missions/action_context.rs @@ -3,15 +3,11 @@ use opencv::core::Mat; #[cfg(feature = "annotated_streams")] use opencv::mod_prelude::ToInputArray; use tokio::io::{AsyncWriteExt, WriteHalf}; -use tokio::sync::RwLock; use tokio_serial::SerialStream; +use crate::comms::{control_board::ControlBoard, meb::MainElectronicsBoard}; use crate::video_source::appsink::Camera; use crate::video_source::MatSource; -use crate::{ - comms::{control_board::ControlBoard, meb::MainElectronicsBoard}, - vision::buoy::Target, -}; /** * Inherit this trait if you have a control board */ @@ -34,8 +30,6 @@ 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; } /** @@ -60,7 +54,6 @@ pub struct FullActionContext<'a, T: AsyncWriteExt + Unpin + Send> { main_electronics_board: &'a MainElectronicsBoard>, front_cam: &'a Camera, bottom_cam: &'a Camera, - desired_buoy_target: &'a RwLock, } impl<'a, T: AsyncWriteExt + Unpin + Send> FullActionContext<'a, T> { @@ -69,14 +62,12 @@ impl<'a, T: AsyncWriteExt + Unpin + Send> FullActionContext<'a, T> { main_electronics_board: &'a MainElectronicsBoard>, front_cam: &'a Camera, bottom_cam: &'a Camera, - desired_buoy_target: &'a RwLock, ) -> Self { Self { control_board, main_electronics_board, front_cam, bottom_cam, - desired_buoy_target, } } } @@ -101,14 +92,6 @@ impl FrontCamIO for FullActionContext<'_, T> { 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() - } - async fn set_desired_buoy_gate(&mut self, value: Target) -> &Self { - *self.desired_buoy_target.write().await = value; - self - } } impl BottomCamIO for FullActionContext<'_, T> { @@ -141,12 +124,6 @@ impl FrontCamIO for EmptyActionContext { async fn annotate_front_camera(&self, _image: &impl ToInputArray) { todo!(); } - async fn get_desired_buoy_gate(&self) -> Target { - todo!() - } - async fn set_desired_buoy_gate(&mut self, _value: Target) -> &Self { - todo!() - } } impl BottomCamIO for EmptyActionContext { diff --git a/src/missions/align_buoy.rs b/src/missions/align_buoy.rs deleted file mode 100644 index 8a5277b0820..00000000000 --- a/src/missions/align_buoy.rs +++ /dev/null @@ -1,287 +0,0 @@ -use tokio::io::WriteHalf; -use tokio_serial::SerialStream; - -use crate::{ - act_nest, - missions::{ - action::{ - ActionChain, ActionConcurrent, ActionDataConditional, ActionSequence, ActionWhile, - TupleSecond, - }, - basic::DelayAction, - comms::StartBno055, - extra::{AlwaysTrue, CountFalse, CountTrue, IsSome, OutputType, Terminal}, - fire_torpedo::{FireLeftTorpedo, FireRightTorpedo}, - movement::{ - AdjustType, ClampX, ConstYaw, LinearYawFromX, MultiplyX, OffsetToPose, ReplaceX, SetX, - SetY, Stability2Adjust, Stability2Movement, Stability2Pos, ZeroMovement, - }, - vision::{ - DetectTarget, ExtractPosition, MidPoint, Norm, SizeUnder, Vision, VisionSizeLock, - }, - }, - vision::{ - buoy_model::{BuoyModel, Target}, - nn_cv2::OnnxModel, - Offset2D, - }, - POOL_YAW_SIGN, -}; - -use super::{ - action::ActionExec, - action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, -}; - -pub fn buoy_align< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, ->( - context: &'static Con, -) -> impl ActionExec<()> + 'static { - const Y_SPEED: f32 = 0.2; - const Y_SPEED_FAST: f32 = 0.5; - const DEPTH: f32 = -1.5; - const FALSE_COUNT: u32 = 5; - - const ALIGN_X_SPEED: f32 = 0.0; - const ALIGN_Y_SPEED: f32 = 0.0; - const ALIGN_YAW_SPEED: f32 = -4.0 * POOL_YAW_SIGN; - - const FAST_DISTANCE: f64 = 3_000.0; - const CORRECT_YAW_SPEED: f32 = 3.0; - const CORRECT_X_MULTIPLY: f32 = 0.5; - const CORRECT_X_CLAMP: f32 = 0.15; - - act_nest!( - ActionSequence::new, - StartBno055::new(context), - act_nest!( - ActionChain::new, - Stability2Movement::new(context, Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH)), - OutputType::<()>::new(), - ), - DelayAction::new(2.0), - ActionWhile::new(ActionSequence::new( - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(ALIGN_X_SPEED, ALIGN_Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - act_nest!( - ActionChain::new, - Vision::, f64>::new(context, BuoyModel::default()), - IsSome::default(), - CountTrue::new(2) - ) - )), - ActionWhile::new(act_nest!( - ActionChain::new, - Vision::, f64>::new(context, BuoyModel::default()), - TupleSecond::<_, bool>::new(ActionConcurrent::new( - ActionSequence::new( - act_nest!( - ActionChain::new, - ActionDataConditional::new( - DetectTarget::new(Target::Buoy), - ActionDataConditional::new( - SizeUnder::new(FAST_DISTANCE), - act_nest!( - ActionChain::new, - Norm::new(BuoyModel::default()), - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::>::default(), - ReplaceX::new(), - LinearYawFromX::::new(CORRECT_YAW_SPEED), - MultiplyX::new(CORRECT_X_MULTIPLY), - ClampX::::new(CORRECT_X_CLAMP), - SetY::::new(AdjustType::Replace( - Y_SPEED_FAST - )), - ), - act_nest!( - ActionChain::new, - Norm::new(BuoyModel::default()), - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::>::default(), - ReplaceX::new(), - LinearYawFromX::::new(CORRECT_YAW_SPEED), - MultiplyX::new(CORRECT_X_MULTIPLY), - ClampX::::new(CORRECT_X_CLAMP), - SetY::::new(AdjustType::Replace(Y_SPEED)), - ) - ), - act_nest!( - ActionSequence::new, - Terminal::new(), - SetY::::new(AdjustType::Replace(0.0)), - SetX::::new(AdjustType::Replace(0.1)), - ) - ), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - AlwaysTrue::new() - ), - ActionChain::new(IsSome::default(), CountFalse::new(FALSE_COUNT)) - )), - ),), - ZeroMovement::new(context, DEPTH), - OutputType::<()>::new() - ) -} - -pub fn buoy_align_shot< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, ->( - context: &'static Con, -) -> impl ActionExec<()> + 'static { - const Y_SPEED: f32 = 0.2; - const DEPTH: f32 = -0.9; - const TRUE_COUNT: u32 = 2; - const FALSE_COUNT: u32 = 5; - - const BACKUP_Y_SPEED: f32 = -0.5; - const BACKUP_TIME: f32 = 6.0; - - const ALIGN_X_SPEED: f32 = 0.0; - const ALIGN_Y_SPEED: f32 = 0.0; - const ALIGN_YAW_SPEED: f32 = -3.0 * POOL_YAW_SIGN; - - const SHOT_DEPTH: f32 = -0.6; - //const SHOT_ANGLE: f32 = 22.5; - const SHOT_ANGLE: f32 = 45.0; - - act_nest!( - ActionSequence::new, - act_nest!( - ActionChain::new, - Stability2Movement::new(context, Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH)), - OutputType::<()>::new(), - ), - DelayAction::new(4.0), - act_nest!( - ActionChain::new, - Stability2Movement::new( - context, - Stability2Pos::new(0.0, BACKUP_Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(BACKUP_TIME), - act_nest!( - ActionChain::new, - Stability2Movement::new(context, Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH)), - OutputType::<()>::new(), - ), - DelayAction::new(4.0), - ActionWhile::new(ActionSequence::new( - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(-ALIGN_X_SPEED, ALIGN_Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - act_nest!( - ActionChain::new, - Vision::, f64>::new(context, BuoyModel::default()), - IsSome::default(), - CountTrue::new(TRUE_COUNT) - ) - )), - ActionWhile::new(act_nest!( - ActionChain::new, - VisionSizeLock::, f64>::new(context, BuoyModel::default()), - TupleSecond::<_, bool>::new(ActionConcurrent::new( - ActionSequence::new( - act_nest!( - ActionChain::new, - ActionDataConditional::new( - DetectTarget::new(Target::Buoy), - act_nest!( - ActionChain::new, - Norm::new(BuoyModel::default()), - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::>::default(), - ReplaceX::new(), - LinearYawFromX::::new(3.0), - MultiplyX::new(0.5), - ClampX::::new(0.15), - SetY::::new(AdjustType::Replace(Y_SPEED)), - ), - act_nest!( - ActionSequence::new, - Terminal::new(), - SetY::::new(AdjustType::Replace(0.0)), - SetX::::new(AdjustType::Replace(0.0)), - ) - ), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - AlwaysTrue::new() - ), - ActionChain::new(IsSome::default(), CountFalse::new(FALSE_COUNT)) - )), - ),), - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(-0.2, 0.0, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(0.5), - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.0, SHOT_ANGLE, 0.0, None, SHOT_DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(1.0), - FireRightTorpedo::new(context), - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(0.2, 0.0, SHOT_ANGLE, 0.0, None, SHOT_DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(1.0), - FireLeftTorpedo::new(context), - DelayAction::new(3.0), - OutputType::<()>::new() - ) -} diff --git a/src/missions/bin.rs b/src/missions/bin.rs index da99b82f3d2..3fe94dc70bc 100644 --- a/src/missions/bin.rs +++ b/src/missions/bin.rs @@ -2,37 +2,17 @@ use tokio::io::WriteHalf; use tokio_serial::SerialStream; use super::action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}; -use crate::{ - config::bin::Config, - missions::{action::ActionExec, vision::VisionNormBottom}, - vision::{ - bin::Bin, - nn_cv2::OnnxModel, - }, -}; pub async fn bin< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, - config: &Config, ) { #[cfg(feature = "logging")] logln!("Starting bin"); let cb = context.get_control_board(); let _ = cb.bno055_periodic_read(true).await; - - let mut vision = VisionNormBottom::, f64>::new(context, Bin::default()); - - loop { - #[cfg(feature = "logging")] - logln!("DOING BIN DETECTION"); - if let Ok(detections) = vision.execute().await { - unimplemented!(); - } - } - #[cfg(feature = "logging")] logln!("Finished bin"); } diff --git a/src/missions/buoy_hit.rs b/src/missions/buoy_hit.rs deleted file mode 100644 index 32d521f90d4..00000000000 --- a/src/missions/buoy_hit.rs +++ /dev/null @@ -1,157 +0,0 @@ -use super::{ - action::{Action, ActionExec, ActionSequence, ActionWhile}, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, - basic::DelayAction, - movement::{StraightMovement, ZeroMovement}, -}; -use crate::vision::{buoy::Buoy, nn_cv2::OnnxModel, VisualDetector}; - -use anyhow::Result; -use core::fmt::Debug; -use tokio::io::WriteHalf; -use tokio_serial::SerialStream; - -/// Action to drive to a Buoy using vision -/// will not set the power to zero on its own. -#[derive(Debug)] -pub struct DriveToBuoyVision<'a, T> { - context: &'a T, - target_depth: f32, - forward_power: f32, - k_p: f32, - buoy_model: Buoy, -} - -pub struct FindBuoy<'a, T> { - context: &'a T, - buoy_model: Buoy, -} - -impl<'a, T> FindBuoy<'a, T> { - pub fn new(context: &'a T, buoy_model: Buoy) -> Self { - FindBuoy { - context, - buoy_model, - } - } -} - -impl<'a, T> DriveToBuoyVision<'a, T> { - pub fn new(context: &'a T, target_depth: f32, forward_power: f32) -> Self { - DriveToBuoyVision { - context, - target_depth, - forward_power, - k_p: 0.3, - buoy_model: Buoy::default(), - } - } -} - -impl Action for DriveToBuoyVision<'_, T> {} - -impl Action for FindBuoy<'_, T> {} - -impl ActionExec> for FindBuoy<'_, T> -where - T: GetControlBoard> + FrontCamIO + Sync + Unpin, -{ - async fn execute(&mut self) -> Result<()> { - let camera_aquisition = self.context.get_front_camera_mat(); - let class_of_interest = self.context.get_desired_buoy_gate().await; - - let model_acquisition = self.buoy_model.detect(&camera_aquisition.await); - match model_acquisition { - Ok(acquisition_vec) if !acquisition_vec.is_empty() => { - acquisition_vec - .iter() - .find(|&result| *result.class() == class_of_interest); - } - Ok(_) => todo!(), - Err(_) => todo!(), - }; - Ok(()) - } -} -impl ActionExec> for DriveToBuoyVision<'_, T> -where - T: GetControlBoard> + FrontCamIO + Sync + Unpin, -{ - async fn execute(&mut self) -> Result<()> { - let camera_aquisition = self.context.get_front_camera_mat(); - let class_of_interest = self.context.get_desired_buoy_gate().await; - - let model_acquisition = self.buoy_model.detect(&camera_aquisition.await); - match model_acquisition { - Ok(acquisition_vec) if !acquisition_vec.is_empty() => { - let detected_item = acquisition_vec - .iter() - .find(|&result| *result.class() == class_of_interest); - - if let Some(scan) = detected_item { - let position = self.buoy_model.normalize(scan.position()); - self.context - .get_control_board() - .stability_2_speed_set_initial_yaw( - self.forward_power, - self.k_p * position.x as f32, - 0.0, - 0.0, - self.target_depth, - ) - .await?; - Ok(()) // Repeat the action - } else { - Err(anyhow::format_err!("no longer detected")) // Stop the action - } - } - _ => { - Err(anyhow::format_err!( - "No buoy detected or error in detection" - )) // Stop the action - } - } - } -} - -// Create and return the outer ActionSequence -pub fn buoy_collision_sequence< - 'a, - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, - T: Send + Sync, ->( - context: &'a Con, -) -> impl ActionExec + 'a -where - ZeroMovement<'a, Con>: ActionExec, -{ - const DEPTH: f32 = 1.0; - - let forward_power = 0.3; - let delay_s = 6.0; - - // Instantiate DriveToBuoyVision with provided values - - let drive_to_buoy_vision = DriveToBuoyVision::new(context, DEPTH, forward_power); - let drive_while_buoy_visible = ActionWhile::new(drive_to_buoy_vision); - - let forward_action = StraightMovement::new(context, DEPTH, true); - // Create a DelayAction with hardcoded delay - let delay_action = DelayAction::new(delay_s); - - // Instantiate ZeroMovement with provided values - let zero_movement = ZeroMovement::new(context, DEPTH); - - // Create the inner ActionSequence - let inner_sequence = ActionSequence::new( - forward_action, - ActionSequence::new(delay_action, zero_movement), - ); - // Create and return the outer ActionSequence - ActionSequence::new(drive_while_buoy_visible, inner_sequence) -} diff --git a/src/missions/circle_buoy.rs b/src/missions/circle_buoy.rs deleted file mode 100644 index 75549a9b860..00000000000 --- a/src/missions/circle_buoy.rs +++ /dev/null @@ -1,185 +0,0 @@ -use crate::{ - act_nest, - missions::{ - action::{ActionChain, ActionConcurrent, ActionWhile, TupleSecond}, - basic::descend_and_go_forward, - extra::{AlwaysTrue, CountTrue, OutputType, ToVec, Transform}, - movement::{ - aggressive_yaw_from_x, AdjustType, CautiousConstantX, ConstYaw, FlatX, LinearYawFromX, - MinYaw, OffsetToPose, SetX, SideMult, Stability1Adjust, Stability1Movement, - Stability1Pos, Stability2Adjust, Stability2Movement, Stability2Pos, StripY, - }, - vision::{Average, DetectTarget, ExtractPosition, VisionNorm}, - }, - vision::{ - buoy_model::{BuoyModel, Target}, - nn_cv2::{OnnxModel, YoloClass}, - path::{Path, Yuv}, - Offset2D, - }, -}; - -use super::{ - action::{ActionExec, ActionSequence}, - action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, - basic::DelayAction, - movement::ZeroMovement, -}; - -use opencv::core::Size; -use tokio::io::WriteHalf; -use tokio_serial::SerialStream; - -pub fn buoy_circle_sequence< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, ->( - context: &Con, -) -> impl ActionExec<()> + '_ { - const DEPTH: f32 = -1.5; - - let delay_s = 1.0; - // Create a DelayAction with hardcoded delay - let delay_action = DelayAction::new(delay_s); - - // Create the inner ActionSequence - ActionSequence::new( - ZeroMovement::new(context, DEPTH), - ActionSequence::new( - delay_action.clone(), - ActionWhile::new(ActionSequence::new( - act_nest!( - ActionChain::new, - VisionNorm::::new( - context, - Path::new( - (Yuv { y: 0, u: 0, v: 128 })..=(Yuv { - y: 255, - u: 127, - v: 255, - }), - 20.0..=800.0, - 10, - Size::from((400, 300)), - 3, - ) - ), - DetectTarget::>::new(true), - ToVec::new(), - ExtractPosition::new(), - Average::new(), - OffsetToPose::default(), - Transform::new(Stability2Adjust::default(), |input| aggressive_yaw_from_x( - input, 40.0 - )), - StripY::default(), - FlatX::default(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new() - ), - AlwaysTrue::new(), - )), - ), - ) -} - -pub fn buoy_circle_sequence_model< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, ->( - context: &'static Con, -) -> impl ActionExec<()> + 'static { - const BUOY_X_SPEED: f32 = -0.0; - const BUOY_Y_SPEED: f32 = 0.0; - const DEPTH: f32 = -1.0; - //const NUM_MODEL_THREADS: NonZeroUsize = nonzero!(4_usize); - - act_nest!( - ActionSequence::new, - descend_and_go_forward(context), - ActionWhile::new(act_nest!( - ActionChain::new, - VisionNorm::, f64>::new(context, BuoyModel::default()), - DetectTarget::, Offset2D>::new(Target::Buoy), - TupleSecond::new(ActionConcurrent::new( - act_nest!( - ActionChain::new, - ToVec::new(), - ExtractPosition::new(), - Average::new(), - //BoxToPose::default(), - OffsetToPose::default(), - LinearYawFromX::::new(4.0), - CautiousConstantX::::new(-0.3), - StripY::::new(), - //FlipYaw::::new(), - //MinYaw::::new(-3.0), - MinYaw::::new(12.0), - Stability1Movement::new( - context, - Stability1Pos::new(BUOY_X_SPEED, BUOY_Y_SPEED, 0.0, 0.0, 0.0, DEPTH) - ), - OutputType::<()>::new() - ), - AlwaysTrue::default(), - )), - )), - OutputType::<()>::new() - ) -} - -pub fn buoy_circle_sequence_blind< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, ->( - context: &'static Con, -) -> impl ActionExec<()> + 'static { - const BUOY_X_SPEED: f32 = 0.4; - const BUOY_Y_SPEED: f32 = 0.15; - const BUOY_YAW_SPEED: f32 = 12.0; - const DEPTH: f32 = -1.5; - const DESCEND_WAIT_DURATION: f32 = 5.0; - const CIRCLE_COUNT: u32 = 34; - - act_nest!( - ActionSequence::new, - ActionChain::new( - Stability2Movement::new(context, Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH)), - OutputType::<()>::new() - ), - DelayAction::new(DESCEND_WAIT_DURATION), - ActionWhile::new(act_nest!( - ActionSequence::new, - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(BUOY_YAW_SPEED)), - SetX::::new(AdjustType::Replace(BUOY_X_SPEED)), - SideMult::new(), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, BUOY_Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new() - ), - DelayAction::new(1.0), - ActionChain::::new(AlwaysTrue::default(), CountTrue::new(CIRCLE_COUNT)), - )), - ZeroMovement::new(context, DEPTH), - OutputType::<()>::new() - ) -} diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index 3a24c25a121..06de717eea4 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -1,34 +1,19 @@ use itertools::Itertools; use tokio::io::WriteHalf; -use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; use crate::{ - act_nest, config::coinflip::Config, - missions::{ - extra::AlwaysTrue, - meb::WaitArm, - movement::{AdjustType, ConstYaw}, - }, vision::{ gate_poles::{GatePoles, Target}, - nn_cv2::{OnnxModel, YoloClass}, - Offset2D, + nn_cv2::OnnxModel, }, }; use super::{ - action::{ - wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionSequence, ActionWhile, - FirstValid, - }, + action::ActionExec, action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, - basic::DelayAction, - comms::StartBno055, - extra::{CountTrue, OutputType}, - movement::{Stability2Adjust, Stability2Movement, Stability2Pos}, - vision::{DetectTarget, VisionNorm}, + vision::VisionNorm, }; pub async fn coinflip_procedural< @@ -45,25 +30,23 @@ pub async fn coinflip_procedural< let mut vision = VisionNorm::, f64>::new(context, GatePoles::default()); - let initial_yaw = loop { - if let Some(initial_angle) = cb.responses().get_angles().await { - break *initial_angle.yaw(); - } else { - #[cfg(feature = "logging")] - logln!("Failed to get initial angle"); - } - }; + // let initial_yaw = loop { + // if let Some(initial_angle) = cb.responses().get_angles().await { + // break *initial_angle.yaw(); + // } else { + // #[cfg(feature = "logging")] + // logln!("Failed to get initial angle"); + // } + // }; - let DEPTH = config.depth; + let depth = config.depth; let _ = cb - .stability_1_speed_set(0.0, 0.0, config.angle_correction, 0.0, 0.0, DEPTH) + .stability_1_speed_set(0.0, 0.0, config.angle_correction, 0.0, 0.0, depth) .await; let mut true_count = 0; let max_true_count = config.true_count; - let mut target_yaw = initial_yaw; - let angle_correction = config.angle_correction; loop { #[allow(unused_variables)] @@ -86,20 +69,20 @@ pub async fn coinflip_procedural< .filter(|d| matches!(d.class().identifier, Target::Sawfish)) .collect_vec(); - let leftPole = detections + let left_pole = detections .iter() .filter(|d| matches!(d.class().identifier, Target::LeftPole)) .collect_vec(); - let rightPole = detections - .iter() - .filter(|d| matches!(d.class().identifier, Target::RightPole)) - .collect_vec(); + // let right_pole = detections + // .iter() + // .filter(|d| matches!(d.class().identifier, Target::RightPole)) + // .collect_vec(); - if (shark.len() > 0 || sawfish.len() > 0 || !leftPole.is_empty()) { + if shark.len() > 0 || sawfish.len() > 0 || !left_pole.is_empty() { if true_count > max_true_count { let _ = cb - .stability_1_speed_set(0.0, 0.0, 0.0, 0.0, 0.0, DEPTH) + .stability_1_speed_set(0.0, 0.0, 0.0, 0.0, 0.0, depth) .await; break; } else { @@ -110,72 +93,3 @@ pub async fn coinflip_procedural< } } } - -pub fn coinflip< - 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.6; - const ALIGN_X_SPEED: f32 = 0.0; - const ALIGN_Y_SPEED: f32 = 0.0; - const ALIGN_YAW_SPEED: f32 = -3.0; - const ALIGN_YAW_CORRECTION_SPEED: f32 = 0.0; - - act_nest!( - ActionSequence::new, - ActionConcurrent::new(WaitArm::new(context), StartBno055::new(context)), - ActionChain::new( - Stability2Movement::new(context, Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH)), - OutputType::<()>::new() - ), - DelayAction::new(DELAY_TIME), - ActionWhile::new(ActionSequence::new( - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(ALIGN_X_SPEED, ALIGN_Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - act_nest!( - ActionChain::new, - VisionNorm::, f64>::new( - context, - GatePoles::load_640(0.8), - ), - act_nest!( - wrap_action(ActionConcurrent::new, FirstValid::new), - DetectTarget::, Offset2D>::new(Target::Gate), - DetectTarget::, Offset2D>::new(Target::Middle), - DetectTarget::, Offset2D>::new(Target::LeftPole), - DetectTarget::, Offset2D>::new( - Target::RightPole - ), - DetectTarget::, Offset2D>::new(Target::Shark), - DetectTarget::, Offset2D>::new(Target::Sawfish), - ), - CountTrue::new(TRUE_COUNT), - ), - )), - ActionWhile::new(act_nest!( - ActionSequence::new, - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_CORRECTION_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(ALIGN_X_SPEED, ALIGN_Y_SPEED, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(2.0), - ActionChain::::new(AlwaysTrue::new(), CountTrue::new(2),), - )) - ) -} diff --git a/src/missions/gate.rs b/src/missions/gate.rs index 403b3488852..40c6afdf7ce 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -4,39 +4,19 @@ use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; use crate::{ - act_nest, config::{gate::Config, ColorProfile, Side}, - missions::{ - action::{ActionConcurrentSplit, ActionDataConditional}, - basic::descend_depth_and_go_forward, - extra::{AlwaysFalse, AlwaysTrue, Terminal}, - movement::{ - AdjustType, ClampX, FlipX, InvertX, ReplaceX, SetSideBlue, SetSideRed, SetX, SetY, - }, - vision::{MidPoint, OffsetClass}, - }, vision::{ gate_cv::GateCV, gate_poles::{GatePoles, Target}, - nn_cv2::{OnnxModel, YoloClass}, - Offset2D, + nn_cv2::OnnxModel, }, }; use super::{ - action::{ - wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionMod, ActionSequence, - ActionWhile, FirstValid, TupleSecond, - }, + action::ActionExec, action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, - basic::{descend_and_go_forward, DelayAction}, - comms::StartBno055, - extra::{CountFalse, CountTrue, OutputType}, - movement::{ - AdjustMovementAngle, LinearYawFromX, OffsetToPose, Stability2Adjust, Stability2Movement, - Stability2Pos, ZeroMovement, - }, - vision::{DetectTarget, ExtractPosition, VisionNorm, VisionNormOffset}, + basic::DelayAction, + vision::VisionNorm, }; pub async fn gate_run_dead_reckon< @@ -44,7 +24,6 @@ pub async fn gate_run_dead_reckon< >( context: &Con, config: &Config, - color_profile: &ColorProfile, ) { #[cfg(feature = "logging")] logln!("Starting Procedural Gate"); @@ -126,7 +105,7 @@ pub async fn gate_run_cv_procedural< .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) .await; - const TOLERANCE: f32 = 0.3; + // const TOLERANCE: f32 = 0.3; let mut gate_state = GateState::Align; let mut yaw_target = 0.0; @@ -141,14 +120,14 @@ pub async fn gate_run_cv_procedural< vec![] }); - let leftPole = detections.iter().filter(|d| *d.class()).collect_vec(); - let leftPole_avg_x = leftPole + let left_pole = detections.iter().filter(|d| *d.class()).collect_vec(); + let left_pole_avg_x = left_pole .iter() .map(|d| *d.position().x() as f32) .sum::(); - let rightPole = detections.iter().filter(|d| !*d.class()).collect_vec(); - let rightPole_avg_x = rightPole + let right_pole = detections.iter().filter(|d| !*d.class()).collect_vec(); + let right_pole_avg_x = right_pole .iter() .map(|d| *d.position().x() as f32) .sum::(); @@ -156,10 +135,10 @@ pub async fn gate_run_cv_procedural< match gate_state { GateState::Align => match config.side { Side::Left => { - if leftPole.len() > 0 { + if left_pole.len() > 0 { false_count = 0; - let mut correction; - if leftPole_avg_x < 0.2 { + let correction; + if left_pole_avg_x < 0.2 { true_count += 1; if true_count >= config.true_count { #[cfg(feature = "logging")] @@ -174,7 +153,7 @@ pub async fn gate_run_cv_procedural< logln!("true_count: {true_count}/4"); } } else { - correction = dbg!(config.correction_factor * leftPole_avg_x); + correction = dbg!(config.correction_factor * left_pole_avg_x); let _ = cb .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth) .await; @@ -206,10 +185,10 @@ pub async fn gate_run_cv_procedural< } Side::Right => { - if rightPole.len() > 0 { + if right_pole.len() > 0 { false_count = 0; - let mut correction; - if rightPole_avg_x < 0.2 { + let correction; + if right_pole_avg_x < 0.2 { true_count += 1; if true_count >= config.true_count { #[cfg(feature = "logging")] @@ -224,7 +203,7 @@ pub async fn gate_run_cv_procedural< logln!("true_count: {true_count}/4"); } } else { - correction = dbg!(config.correction_factor * rightPole_avg_x); + correction = dbg!(config.correction_factor * right_pole_avg_x); let _ = cb .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth) .await; @@ -278,12 +257,12 @@ pub async fn gate_run_cv_procedural< sleep(Duration::from_secs(config.strafe_duration as u64)).await; - yaw_target = (yaw_target + yaw_target = yaw_target + (if let Side::Left = config.side { config.yaw_adjustment } else { -config.yaw_adjustment - })); + }); let _ = cb .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, yaw_target, config.depth) @@ -343,10 +322,10 @@ pub async fn gate_run_procedural< vec![] }); - let rightPole = detections - .iter() - .filter(|d| matches!(d.class().identifier, Target::RightPole)) - .collect_vec(); + // let right_pole = detections + // .iter() + // .filter(|d| matches!(d.class().identifier, Target::RightPole)) + // .collect_vec(); /* let middle = detections .iter() @@ -435,11 +414,11 @@ pub async fn gate_run_procedural< Side::Right => { if !sawfish.is_empty() { // Center on average x of blue - let avg_x = (sawfish + let avg_x = sawfish .iter() .map(|d| *d.position().x() as f32) .sum::() - / sawfish.len() as f32); + / sawfish.len() as f32; #[cfg(feature = "logging")] logln!("SAWFISH AVG X: {}", avg_x); @@ -505,236 +484,6 @@ pub async fn gate_run_procedural< } } -pub fn gate_run_naive< - Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, ->( - context: &Con, -) -> impl ActionExec<()> + '_ { - let depth: f32 = -1.5; - - ActionSequence::new( - ActionConcurrent::new(descend_and_go_forward(context), StartBno055::new(context)), - ActionSequence::new( - ActionWhile::new(ActionChain::new( - VisionNormOffset::, f64>::new( - context, - GatePoles::default(), - ), - TupleSecond::new(ActionConcurrent::new( - AdjustMovementAngle::new(context, depth), - CountTrue::new(3), - )), - )), - ActionWhile::new(ActionChain::new( - VisionNormOffset::, f64>::new( - context, - GatePoles::default(), - ), - TupleSecond::new(ActionConcurrent::new( - AdjustMovementAngle::new(context, depth), - CountFalse::new(10), - )), - )), - ), - ) -} - -pub fn gate_run_complex< - Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, ->( - context: &Con, -) -> impl ActionExec> + '_ { - let depth: f32 = -1.40; - - act_nest!( - ActionSequence::new, - DelayAction::new(3.0), - ActionConcurrent::new( - descend_depth_and_go_forward(context, depth), - StartBno055::new(context), - ), - act_nest!( - ActionSequence::new, - adjust_logic(context, depth, CountTrue::new(4)), - adjust_logic(context, depth, CountFalse::new(4)), - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 1.0, 0.0, 0.0, None, depth), - ), - OutputType::<()>::default() - ), - DelayAction::new(3.0), - ZeroMovement::new(context, depth), - ), - ) -} - -pub fn gate_run_coinflip< - 'a, - Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, ->( - context: &'a Con, - config: &Config, -) -> impl ActionExec> + 'a { - let depth = config.depth; - - act_nest!( - ActionSequence::new, - ActionConcurrent::new( - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.5, 0.0, 0.0, None, depth), - ), - OutputType::<()>::default() - ), - StartBno055::new(context), - ), - act_nest!( - ActionSequence::new, - adjust_logic(context, depth, CountTrue::new(config.true_count)), - // adjust_logic(context, depth, CountFalse::new(10)), - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.5, 0.0, 0.0, None, depth), - ), - OutputType::<()>::default() - ), - ActionWhile::new(act_nest!( - ActionChain::new, - VisionNorm::, f64>::new(context, GatePoles::default()), - act_nest!( - wrap_action(ActionConcurrent::new, FirstValid::new), - DetectTarget::, Offset2D>::new(Target::Blue), - DetectTarget::, Offset2D>::new(Target::Middle), - DetectTarget::, Offset2D>::new(Target::Red), - DetectTarget::, Offset2D>::new(Target::Pole), - ), - CountFalse::new(config.false_count), - )), - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, depth), - ), - OutputType::<()>::default() - ), - DelayAction::new(0.0), - ZeroMovement::new(context, depth), - ), - ) -} - -pub fn adjust_logic< - 'a, - Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, - X: 'a + ActionMod + ActionExec>, ->( - context: &'a Con, - depth: f32, - end_condition: X, -) -> impl ActionExec<()> + 'a { - const GATE_TRAVERSAL_SPEED: f32 = 0.2; - - ActionWhile::new(ActionChain::new( - VisionNorm::, f64>::new(context, GatePoles::default()), - ActionChain::new( - TupleSecond::new(ActionConcurrent::new( - ActionDataConditional::new( - //act_nest!( - //wrap_action(ActionConcurrent::new, FirstValid::new), - DetectTarget::, Offset2D>::new(Target::Blue), - //DetectTarget::, Offset2D>::new( - //Target::Middle - //), - //), - ActionSequence::new(SetSideBlue::new(), Terminal::new()), - ActionDataConditional::new( - DetectTarget::, Offset2D>::new(Target::Red), - ActionSequence::new(SetSideRed::new(), Terminal::new()), - Terminal::new(), - ), - ), - ActionDataConditional::new( - act_nest!( - wrap_action(ActionConcurrent::new, FirstValid::new), - DetectTarget::, Offset2D>::new(Target::Blue), - DetectTarget::, Offset2D>::new( - Target::Middle - ), - DetectTarget::, Offset2D>::new(Target::Red), - ), - act_nest!( - ActionConcurrent::new, - act_nest!( - ActionChain::new, - OffsetClass::new(Target::Middle, Offset2D::::new(-0.05, 0.0)), - //OffsetClass::new(Target::Blue, Offset2D::::new(-0.1, 0.0)), - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::default(), - LinearYawFromX::::new(5.0), - ClampX::new(0.2), - SetY::::new(AdjustType::Adjust(0.02)), - FlipX::default(), - ), - AlwaysTrue::new(), - ), - ActionDataConditional::new( - DetectTarget::, Offset2D>::new(Target::Pole), - act_nest!( - ActionConcurrent::new, - act_nest!( - ActionChain::new, - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::default(), - InvertX::new(), - LinearYawFromX::::new(-7.0), - //ClampX::new(0.8), - SetY::::new(AdjustType::Replace(0.2)), - ReplaceX::new(), - ), - AlwaysTrue::new(), - ), - ActionConcurrent::new( - act_nest!( - ActionSequence::new, - Terminal::new(), - SetY::::new(AdjustType::Replace(0.4)), - SetX::::new(AdjustType::Replace(0.0)), - ), - AlwaysFalse::new(), - ), - ), - ), - )), - TupleSecond::new(ActionConcurrentSplit::new( - act_nest!( - ActionChain::new, - Stability2Movement::new( - context, - Stability2Pos::new(0.0, GATE_TRAVERSAL_SPEED, 0.0, 0.0, None, depth), - ), - OutputType::<()>::new(), - ), - end_condition, - )), - ), - )) -} - -pub fn gate_run_testing< - Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, ->( - context: &Con, -) -> impl ActionExec<()> + '_ { - let depth: f32 = -1.0; - adjust_logic(context, depth, CountTrue::new(3)) -} - enum GateState { Align, Approach, diff --git a/src/missions/mod.rs b/src/missions/mod.rs index e8b1096e425..ba1bc36ca74 100644 --- a/src/missions/mod.rs +++ b/src/missions/mod.rs @@ -1,10 +1,7 @@ pub mod action; pub mod action_context; -pub mod align_buoy; pub mod basic; pub mod bin; -pub mod buoy_hit; -pub mod circle_buoy; pub mod coinflip; pub mod comms; pub mod example; diff --git a/src/missions/octagon.rs b/src/missions/octagon.rs index 047eb5a3000..6354d8a5615 100644 --- a/src/missions/octagon.rs +++ b/src/missions/octagon.rs @@ -45,6 +45,8 @@ pub fn octagon< config: &Config, color_profile: &ColorProfile, ) -> impl ActionExec<()> + 'static { + let _ = config; + let _ = color_profile; const FULL_SPEED_Y: f32 = 0.7; const FULL_SPEED_X: f32 = 0.0; const FULL_SPEED_PITCH: f32 = -45.0 / 4.0; diff --git a/src/missions/slalom.rs b/src/missions/slalom.rs index 220732691e9..227992ba31b 100644 --- a/src/missions/slalom.rs +++ b/src/missions/slalom.rs @@ -1,28 +1,13 @@ -use hdbscan::{Center, Hdbscan}; -use itertools::Itertools; -use std::f64::consts::PI; - use tokio::{ io::WriteHalf, - select, time::{sleep, Duration}, }; -use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; -use tokio_util::sync::CancellationToken; - -use bluerobotics_ping::{ - device::{Ping360, PingDevice}, - ping360::AutoDeviceDataStruct, -}; +use tokio_serial::SerialStream; use super::action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}; use crate::{ - config::{slalom::Config, sonar::Config as SonarConfig, ColorProfile, Side::*}, - missions::{ - action::ActionExec, - basic::DelayAction, - vision::{VisionNorm, VisionNormAngle}, - }, + config::{slalom::Config, ColorProfile, Side::*}, + missions::{action::ActionExec, vision::VisionNormAngle}, }; // TODO: Consider filtering detections by angle (poles will always be upright) @@ -64,9 +49,6 @@ pub async fn slalom< let mut yaw_target = 0.0; let mut true_count = 0; let mut false_count = 0; - let mut init_timer = DelayAction::new(config.init_duration); - let mut traversal_timer = DelayAction::new(config.traversal_duration); // forward duration in second - let mut strafe_timer = DelayAction::new(config.strafe_duration); enum SlalomState { Align, @@ -108,10 +90,12 @@ pub async fn slalom< let x = *position.x() as f32; dbg!(&x); let mut correction = 0.0; + let _ = correction; if x.abs() < 0.2 { true_count += 1; if true_count >= 4 { correction = 0.0; + let _ = correction; #[cfg(feature = "logging")] logln!("ALIGNED"); slalom_state = SlalomState::Approach; @@ -178,12 +162,12 @@ pub async fn slalom< sleep(Duration::from_secs(config.strafe_duration as u64)).await; - yaw_target = (yaw_target + yaw_target = yaw_target + (if let Left = config.side { config.yaw_adjustment } else { -config.yaw_adjustment - })); + }); let _ = cb .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, yaw_target, config.depth) diff --git a/src/missions/spin.rs b/src/missions/spin.rs index cd69c477005..23d4b73e2e7 100644 --- a/src/missions/spin.rs +++ b/src/missions/spin.rs @@ -1,109 +1,83 @@ -use std::marker::PhantomData; - -use anyhow::bail; -use tokio::io::{AsyncWriteExt, WriteHalf}; +use super::action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}; +use crate::config::spin::Config; +use tokio::io::WriteHalf; +use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; -use crate::{ - act_nest, logln, - missions::{ - action::{ActionChain, ActionConcurrent, ActionSequence, ActionWhile, TupleSecond}, - basic::DelayAction, - extra::{AlwaysFalse, OutputType}, - movement::{GlobalMovement, GlobalPos, Stability2Movement, Stability2Pos, ZeroMovement}, - }, -}; - -use super::{ - action::{Action, ActionExec}, - action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}, -}; - -pub fn spin< +pub async fn spin< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, -) -> impl ActionExec<()> + '_ { - const GATE_DEPTH: f32 = -1.75; - const DEPTH: f32 = -1.15; - const Z_TARGET: f32 = 0.0; - const FORWARD_SPEED: f32 = 0.0; - const SPIN_SPEED: f32 = 1.0; + config: &Config, +) { + #[cfg(feature = "logging")] + logln!("Starting spin"); - act_nest!( - ActionSequence::new, - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new(0.0, FORWARD_SPEED, 0.0, 0.0, None, GATE_DEPTH), - ), - OutputType::<()>::new(), - ), - DelayAction::new(2.0), - ActionWhile::new(TupleSecond::new(ActionConcurrent::new( - act_nest!( - ActionSequence::new, - ActionChain::new( - GlobalMovement::new( - context, - GlobalPos::new(0.0, 0.0, Z_TARGET, 0.0, SPIN_SPEED, 0.0), - ), - OutputType::<()>::new(), - ), - ActionChain::new(AlwaysFalse::new(), OutputType::>::new(),), - ), - SpinCounter::new(4, context) - ))), - ZeroMovement::new(context, DEPTH), - DelayAction::new(3.0), - OutputType::<()>::new(), - ) -} + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; -struct SpinCounter<'a, T, U> { - target: usize, - half_loops: usize, - control_board: &'a T, - _phantom: PhantomData, -} + let initial_roll = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.roll(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; -impl<'a, T, U> SpinCounter<'a, T, U> { - pub fn new(target: usize, control_board: &'a T) -> Self { - Self { - target, - half_loops: 0, - control_board, - _phantom: PhantomData, + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); } - } -} + }; + + let mut spin_count = 0; + let mut in_spin = false; -impl Action for SpinCounter<'_, T, U> {} + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; + sleep(Duration::from_secs(1)).await; + let _ = cb + .global_speed_set(0.0, 0.0, 0.0, 0.0, config.spin_speed, 0.0) + .await; -impl + Send + Sync, U: AsyncWriteExt + Unpin + Send + Sync> - ActionExec> for SpinCounter<'_, T, U> -{ - async fn execute(&mut self) -> anyhow::Result<()> { - let cntrl_board = self.control_board.get_control_board(); - if let Some(angles) = cntrl_board.responses().get_angles().await { - let roll = *angles.roll(); - if self.half_loops % 2 == 0 { - if roll < -20.0 && roll > -150.0 { - logln!("Roll at 0 trigger: {}", roll); - self.half_loops += 1; - logln!("Loop count: {}", self.half_loops); - } - } else if roll < 160.0 && roll > 0.0 { - logln!("Roll at 1 trigger: {}", roll); - self.half_loops += 1; - logln!("Loop count: {}", self.half_loops); + loop { + let curr_roll = loop { + if let Some(angle) = cb.responses().get_angles().await { + break *angle.roll(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get angle"); } + }; + + let mut diff: f32 = curr_roll - initial_roll; + if diff > 180.0 { + diff -= 360.0; + } else if diff < -180.0 { + diff += 360.0; } - if self.half_loops < self.target { - Ok(()) - } else { - bail!("") + if !in_spin && diff.abs() > config.hysteresis { + in_spin = true; + } + + if in_spin && diff.abs() < config.hysteresis { + spin_count += 1; + #[cfg(feature = "logging")] + logln!("Completed spin {}", spin_count); + in_spin = false; + } + + if spin_count >= config.num_spins { + break; } } + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; } diff --git a/src/video_source/appsink.rs b/src/video_source/appsink.rs index 85b28d9bddb..a261ed42ec6 100644 --- a/src/video_source/appsink.rs +++ b/src/video_source/appsink.rs @@ -7,10 +7,7 @@ use std::{fs::create_dir_all, path::Path, sync::Arc, thread::spawn}; use tokio::sync::Mutex; #[cfg(feature = "logging")] -use { - crate::logln, - opencv::videoio::{VideoCaptureTraitConst, CAP_GSTREAMER}, -}; +use crate::logln; #[cfg(feature = "annotated_streams")] use { opencv::{ diff --git a/src/vision/buoy.rs b/src/vision/buoy.rs deleted file mode 100644 index e146eb306cd..00000000000 --- a/src/vision/buoy.rs +++ /dev/null @@ -1,156 +0,0 @@ -use anyhow::Result; -use opencv::{core::Size, prelude::Mat}; - -use crate::load_onnx; - -use super::{ - nn_cv2::{OnnxModel, VisionModel, YoloDetection}, - yolo_model::YoloProcessor, -}; - -use core::hash::Hash; -use std::{error::Error, fmt::Display}; - -#[derive(Debug, PartialEq, Eq, Hash, Clone)] -pub enum Target { - Earth1, - Earth2, - Abydos1, - Abydos2, -} - -#[derive(Debug)] -pub struct TargetError { - x: i32, -} - -impl Display for TargetError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{} is outside known classIDs [0, 3]", self.x) - } -} - -impl Error for TargetError {} - -impl TryFrom for Target { - type Error = TargetError; - fn try_from(value: i32) -> std::result::Result { - match value { - 0 => Ok(Self::Earth1), - 1 => Ok(Self::Earth2), - 2 => Ok(Self::Abydos1), - 3 => Ok(Self::Abydos2), - x => Err(TargetError { x }), - } - } -} - -impl Target { - pub fn to_integer_id(&self) -> i32 { - match self { - Target::Earth1 => 0, - Target::Earth2 => 1, - Target::Abydos1 => 2, - Target::Abydos2 => 3, - } - } -} - -impl Display for Target { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{self:?}") - } -} - -#[derive(Debug)] -pub struct Buoy { - model: T, - threshold: f64, -} - -impl Buoy { - pub fn new(model_name: &str, model_size: i32, threshold: f64) -> Result { - Ok(Self { - model: OnnxModel::from_file(model_name, model_size, 4)?, - threshold, - }) - } - - pub fn load_320(threshold: f64) -> Self { - Self { - model: load_onnx!("models/buoy_320.onnx", 320, 4), - threshold, - } - } - - pub fn load_640(threshold: f64) -> Self { - Self { - model: load_onnx!("models/buoy_640.onnx", 640, 4), - threshold, - } - } -} - -impl Default for Buoy { - fn default() -> Self { - Self::load_320(0.7) - } -} - -impl YoloProcessor for Buoy { - type Target = Target; - - fn detect_yolo_v5(&mut self, image: &Mat) -> Vec { - self.model.detect_yolo_v5(image, self.threshold) - } - - fn model_size(&self) -> Size { - self.model.size() - } -} - -#[cfg(test)] -mod tests { - use itertools::Itertools; - use opencv::{ - core::Vector, - imgcodecs::{imread, imwrite, IMREAD_COLOR}, - }; - - use crate::{ - logln, - vision::{Draw, VisualDetector}, - }; - use assert_approx_eq::assert_approx_eq; - - use super::*; - - #[test] - fn detect_single() { - let mut image = imread("tests/vision/resources/buoy_images/1.jpeg", IMREAD_COLOR).unwrap(); - let detections = Buoy::default().detect(&image).unwrap(); - let detect_unique: Vec<_> = detections.iter().unique().collect(); - - detect_unique - .iter() - .for_each(|result| result.draw(&mut image).unwrap()); - - logln!("Detections: {:#?}", detect_unique); - imwrite( - "tests/vision/output/buoy_images/1.jpeg", - &image, - &Vector::default(), - ) - .unwrap(); - - let abydos_1_pos = detect_unique - .iter() - .find(|&result| *result.class() == Target::Abydos1) - .unwrap() - .position(); - assert_approx_eq!(abydos_1_pos.x, 134.9113845825195, 1.0); - assert_approx_eq!(abydos_1_pos.y, 163.99715423583984, 1.0); - assert_approx_eq!(abydos_1_pos.width, 149.86732482910156, 1.0); - assert_approx_eq!(abydos_1_pos.height, 141.14679336547852, 1.0); - } -} diff --git a/src/vision/buoy_model.rs b/src/vision/buoy_model.rs deleted file mode 100644 index f994e006911..00000000000 --- a/src/vision/buoy_model.rs +++ /dev/null @@ -1,164 +0,0 @@ -use anyhow::Result; -use derive_getters::Getters; -use opencv::{core::Rect2d, core::Size, prelude::Mat}; - -use crate::{load_onnx, logln}; - -use super::{ - nn_cv2::{OnnxModel, VisionModel, YoloClass, YoloDetection}, - yolo_model::YoloProcessor, -}; - -use core::hash::Hash; -use std::{cmp::Ordering, error::Error, fmt::Display}; - -#[derive(Debug, PartialEq, Eq, Hash, Clone)] -pub enum Target { - Buoy, -} - -impl From> for Target { - fn from(value: YoloClass) -> Self { - value.identifier - } -} - -#[derive(Debug)] -pub struct TargetError { - x: i32, -} - -impl Display for TargetError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{} is outside known classIDs [0, 0]", self.x) - } -} - -impl Error for TargetError {} - -impl TryFrom for Target { - type Error = TargetError; - fn try_from(value: i32) -> std::result::Result { - match value { - 0 => Ok(Self::Buoy), - x => Err(TargetError { x }), - } - } -} - -impl Display for Target { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{self:?}") - } -} - -#[derive(Debug, Clone, Getters)] -pub struct BuoyModel { - model: T, - threshold: f64, -} - -impl BuoyModel { - pub fn new(model_name: &str, model_size: i32, threshold: f64) -> Result { - Ok(Self { - model: OnnxModel::from_file(model_name, model_size, 1)?, - threshold, - }) - } - - pub fn load_640(threshold: f64) -> Self { - Self { - model: load_onnx!("models/buoy_single_class_640.onnx", 640, 1), - threshold, - } - } -} - -impl Default for BuoyModel { - fn default() -> Self { - Self::load_640(0.9) - } -} - -impl YoloProcessor for BuoyModel { - type Target = Target; - - fn detect_yolo_v5(&mut self, image: &Mat) -> Vec { - let bounding_box_size = |b: &Rect2d| -> f64 { b.width * b.height }; - - let mut detection = self.model.detect_yolo_v5(image, self.threshold); - detection.sort_unstable_by(|lhs, rhs| { - std::cmp::PartialOrd::partial_cmp( - &bounding_box_size(lhs.bounding_box()), - &bounding_box_size(rhs.bounding_box()), - ) - .unwrap_or(Ordering::Equal) - }); - logln!("Detection MIN buoy: {:#?}", detection.first()); - logln!("Detection MAX buoy: {:#?}", detection.last()); - detection.into_iter().rev().take(1).collect() - } - - fn model_size(&self) -> Size { - self.model.size() - } -} - -/* -impl BuoyModel { - /// Convert into [`ModelPipelined`]. - /// - /// See [`ModelPipelined::new`] for arguments. - pub async fn into_pipelined( - self, - model_threads: NonZeroUsize, - post_processing_threads: NonZeroUsize, - ) -> ModelPipelined { - ModelPipelined::new( - self.model, - model_threads, - post_processing_threads, - self.threshold, - ) - .await - } -} -*/ - -impl VisionModel for BuoyModel { - type ModelOutput = ::ModelOutput; - type PostProcessArgs = ::PostProcessArgs; - - fn detect_yolo_v5(&mut self, image: &Mat, threshold: f64) -> Vec { - let bounding_box_size = |b: &Rect2d| -> f64 { b.width * b.height }; - logln!("CALLED BUOY DETECT"); - - let mut detection = self.model.detect_yolo_v5(image, threshold); - detection.sort_unstable_by(|lhs, rhs| { - std::cmp::PartialOrd::partial_cmp( - &bounding_box_size(lhs.bounding_box()), - &bounding_box_size(rhs.bounding_box()), - ) - .unwrap_or(Ordering::Equal) - }); - logln!("Detection MIN buoy: {:#?}", detection.first()); - logln!("Detection MAX buoy: {:#?}", detection.last()); - detection.into_iter().rev().take(1).collect() - } - fn forward(&mut self, image: &Mat) -> Self::ModelOutput { - self.model.forward(image) - } - fn post_process_args(&self) -> Self::PostProcessArgs { - self.model.post_process_args() - } - fn post_process( - args: Self::PostProcessArgs, - output: Self::ModelOutput, - threshold: f64, - ) -> Vec { - OnnxModel::post_process(args, output, threshold) - } - fn size(&self) -> Size { - self.model.size() - } -} diff --git a/src/vision/gate_cv.rs b/src/vision/gate_cv.rs index 31ed93df615..fa0c77e713e 100644 --- a/src/vision/gate_cv.rs +++ b/src/vision/gate_cv.rs @@ -1,13 +1,13 @@ -use crate::config::{ColorProfile, Side}; +use crate::config::ColorProfile; use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv}; use opencv::{ core::{in_range, Point, Scalar, Size, Vector}, imgproc::{ - box_points, contour_area_def, cvt_color_def, find_contours_def, min_area_rect, - CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, RETR_EXTERNAL, + contour_area_def, cvt_color_def, find_contours_def, min_area_rect, CHAIN_APPROX_SIMPLE, + COLOR_BGR2YUV, RETR_EXTERNAL, }, - prelude::{Mat, MatTraitConst, MatTraitConstManual}, + prelude::{Mat, MatTraitConst}, }; use std::ops::RangeInclusive; diff --git a/src/vision/gate_poles.rs b/src/vision/gate_poles.rs index 1b9774dd8b1..de8df0a092d 100644 --- a/src/vision/gate_poles.rs +++ b/src/vision/gate_poles.rs @@ -1,17 +1,10 @@ use anyhow::Result; use derive_getters::Getters; -use itertools::MergeJoinBy; -use opencv::core::{multiply, multiply_def, MatTraitManual, BORDER_CONSTANT, CV_8U}; -use opencv::imgproc::{ - dilate_def, get_structuring_element, morphology_default_border_value, MORPH_RECT, -}; +use opencv::core::{multiply, BORDER_CONSTANT, CV_8U}; +use opencv::imgproc::{dilate, morphology_default_border_value}; use opencv::{ - core::{in_range, merge, split, Point, Scalar, Size, Vector}, - imgproc::{ - self, contour_area_def, cvt_color_def, dilate, find_contours_def, min_area_rect, - CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, LINE_8, RETR_EXTERNAL, - }, - prelude::{Mat, MatTraitConst, MatTraitConstManual}, + core::{merge, split, Point, Size, Vector}, + prelude::{Mat, MatTraitConst}, }; use crate::load_onnx; diff --git a/src/vision/mod.rs b/src/vision/mod.rs index 616c7dabee7..a9e8c16aa89 100644 --- a/src/vision/mod.rs +++ b/src/vision/mod.rs @@ -1,4 +1,4 @@ -use anyhow::{anyhow, Result}; +use anyhow::Result; use derive_getters::Getters; use itertools::Itertools; use num_traits::{zero, FromPrimitive, Num}; @@ -16,17 +16,13 @@ use std::{ }; pub mod bin; -pub mod buoy; -pub mod buoy_model; pub mod gate; pub mod gate_cv; pub mod gate_poles; pub mod image_prep; pub mod nn_cv2; pub mod octagon; -pub mod path; pub mod path_cv; -pub mod pca; pub mod slalom; pub mod slalom_yolo; pub mod yolo_model; @@ -303,7 +299,7 @@ impl Draw for DrawRect2d { // 0, // )?; - let center = Point::new((self.x as i32), (self.y as i32)); + let center = Point::new(self.x as i32, self.y as i32); imgproc::circle_def(canvas, center, 5, Scalar::from((0.0, 0.0, 255.0)))?; Ok(()) @@ -482,3 +478,24 @@ impl Mul<&Mat> for PosVector { } } } + +impl Draw for VisualDetection> { + fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> { + let color = if self.class { + 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, + )?; + Ok(()) + } +} diff --git a/src/vision/nn_cv2.rs b/src/vision/nn_cv2.rs index 317eb878bd6..d203a8e7c7c 100644 --- a/src/vision/nn_cv2.rs +++ b/src/vision/nn_cv2.rs @@ -12,9 +12,6 @@ use std::{ sync::Mutex, }; -#[cfg(feature = "cuda_min_max_loc")] -use opencv::cudaarithm::min_max_loc as cuda_min_max_loc; - #[derive(Debug, Clone, Getters, PartialEq)] pub struct YoloDetection { class_id: i32, diff --git a/src/vision/path.rs b/src/vision/path.rs deleted file mode 100644 index 6990324d120..00000000000 --- a/src/vision/path.rs +++ /dev/null @@ -1,399 +0,0 @@ -use super::{ - image_prep::{kmeans, resize}, - pca::PosVector, - MatWrapper, VisualDetection, VisualDetector, -}; -use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; -use itertools::Itertools; -use opencv::{ - core::{in_range, AlgorithmHint, Size, VecN}, - imgproc::{cvt_color, COLOR_RGB2YUV, COLOR_YUV2RGB}, - prelude::{Mat, MatTraitConst, MatTraitConstManual}, -}; -use std::ops::RangeInclusive; - -#[cfg(feature = "logging")] -use { - opencv::{core::Vector, imgcodecs::imwrite}, - std::fs::create_dir_all, - uuid::Uuid, -}; - -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 Path { - color_bounds: RangeInclusive, - width_bounds: RangeInclusive, - num_regions: i32, - size: Size, - attempts: i32, - image: MatWrapper, -} - -impl Path { - pub fn image(&self) -> Mat { - (*self.image).clone() - } -} - -impl Path { - 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 Path { - fn default() -> Self { - Path::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 Path { - 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, - AlgorithmHint::ALGO_HINT_DEFAULT, - ) - .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, - AlgorithmHint::ALGO_HINT_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); - - 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 Path { - 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, - AlgorithmHint::ALGO_HINT_DEFAULT, - ) - .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, - AlgorithmHint::ALGO_HINT_DEFAULT, - ) - .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 = Path::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(); - } -} diff --git a/src/vision/pca.rs b/src/vision/pca.rs deleted file mode 100644 index 5b9a2e3f502..00000000000 --- a/src/vision/pca.rs +++ /dev/null @@ -1,121 +0,0 @@ -use std::{fmt::Debug, ops::Mul}; - -use derive_getters::Getters; -use opencv::{ - core::{MatTraitConst, Point, Scalar}, - imgproc::{self, LINE_8}, - prelude::Mat, -}; - -use crate::logln; - -use super::{Angle2D, Draw, Offset2D, RelPosAngle, VisualDetection}; - -#[derive(Debug, Clone, Getters)] -pub struct PosVector { - x: f64, - y: f64, - angle: f64, - width: f64, - length: f64, - length_2: f64, -} - -impl PosVector { - pub fn new(x: f64, y: f64, angle: f64, width: f64, length: f64, length_2: f64) -> Self { - Self { - x, - y, - angle, - width, - length, - length_2, - } - } -} - -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), - width: self.width * (size.width as f64), - length: self.length * (size.width as f64), - length_2: self.length_2 * (size.width as f64), - 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(()) - } -} - -impl Draw for VisualDetection> { - fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> { - let color = if self.class { - 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, - )?; - Ok(()) - } -} diff --git a/src/vision/slalom.rs b/src/vision/slalom.rs index e6f0c5b8557..2eb23759932 100644 --- a/src/vision/slalom.rs +++ b/src/vision/slalom.rs @@ -73,8 +73,8 @@ impl VisualDetector for Slalom { input_image: &Mat, ) -> anyhow::Result>> { let areas = self.area_bounds.clone(); - let MIN_AREA = areas.start(); - let MAX_AREA = areas.end(); + let min_area = areas.start(); + let max_area = areas.end(); self.image = resize(input_image, &self.size)?.into(); let mut yuv_image = Mat::default(); @@ -114,7 +114,7 @@ impl VisualDetector for Slalom { #[cfg(feature = "logging")] logln!("AREA: {area}"); - if area > *MIN_AREA && area < *MAX_AREA { + if area > *min_area && area < *max_area { let rect = min_area_rect(&contour)?; let mut box_rect = Mat::default();