autowebtweaks of wall thresholder

also implemented wall masking

simcontroller/Cargo.toml

@@ -4,6 +4,7 @@ version = "0.1.0"
 edition = "2021"
 
 [dependencies]
+const-tweaker = "0.3.1"
 opencv = {version = "0.92.3", features = ["clang-runtime"]}
 thiserror = "1.0.63"
 webots = { git = "https://github.com/katharostech/webots-rust" }

simcontroller/Justfile

@@ -0,0 +1,4 @@
+debug:
+    cargo build
+    cp target/debug/simcontroller ../simulation/controllers/cruise/cruise
+

simcontroller/src/main.rs

@@ -1,5 +1,5 @@
 use camera::Camera;
-use opencv::{core::{KeyPoint, KeyPointTraitConst, Mat, Ptr, VecN, Vector, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, ORB}, highgui, imgcodecs::ImreadModes, imgproc, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}};
+use opencv::{core::{bitwise_and, bitwise_and_def, bitwise_or_def, in_range, KeyPoint, KeyPointTraitConst, Mat, MatTraitConst, MatTraitConstManual, Ptr, Size, VecN, Vector, CV_64F, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, ORB}, gapi::bgr2_luv, highgui::{self, MouseCallback}, imgcodecs::ImreadModes, imgproc::{self, cvt_color_def, gaussian_blur, gaussian_blur_def, sobel_def, COLOR_BGR2GRAY, COLOR_BGR2HLS}, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}};
 use webots::prelude::*;
 use std::{f64::consts::TAU, time::Duration};
 mod camera;
@@ -7,6 +7,20 @@ mod camera;
 const TIME_STEP: Duration = Duration::from_millis(20);
 const MAX_SPEED: f64 = TAU;
 
+// const tweaker only supports floating point, these are mapped
+#[const_tweaker::tweak]
+const LOWERB_H: f32 = 18. / 180.;
+#[const_tweaker::tweak]
+const LOWERB_L: f32 = 23. / 255.;
+#[const_tweaker::tweak]
+const LOWERB_S: f32 = 36. / 255.;
+#[const_tweaker::tweak]
+const UPPERB_H: f32 = 92. / 180.;
+#[const_tweaker::tweak]
+const UPPERB_L: f32 = 151. / 255.;
+#[const_tweaker::tweak]
+const UPPERB_S: f32 = 169. / 255.;
+
 struct MyRobot {
     camera: Camera,
     left_motor: Motor,
@@ -40,6 +54,8 @@ impl Robot for MyRobot {
         let window = "processed";
         //highgui::named_window(window, 1).unwrap();
 
+        //highgui::set_mouse_callback(window, |x,y,_,_| {dbg!(x+y);}).unwrap();
+
         let orb_detector = ORB::create_def().unwrap();
         let matcher = opencv::features2d::BFMatcher::new_def().unwrap();
         //let last_frame_descriptors = opencv::core::Mat::default();
@@ -69,13 +85,41 @@ impl Robot for MyRobot {
             //opencv::imgcodecs::imwrite_def("cam.png", &img).unwrap();
             //highgui::imshow(self.window, &img).unwrap();
 
+
+            let mut blurred = opencv::core::Mat::default();
+            let mut edge = opencv::core::Mat::default();
+            gaussian_blur_def(&img, &mut blurred, Size::new(3, 3), 0.).unwrap();
+            //sobel_def(&blurred, &mut edge, CV_64F, 0, 2).unwrap();
+            cvt_color_def(&blurred, &mut edge, COLOR_BGR2HLS).unwrap();
+            highgui::imshow(self.window, &blurred).unwrap();
+            let mut mask_a = opencv::core::Mat::default();
+            let mut mask_b = opencv::core::Mat::default();
+            let mut mask = opencv::core::Mat::default();
+            opencv::imgcodecs::imwrite_def("blurred.png", &blurred).unwrap();
+            println!("Lowrerh {}", LOWERB_H);
+            let lowerb = dbg!([(*LOWERB_H*255.) as u8,(*LOWERB_L*255.) as u8,(*LOWERB_S*255.) as u8,255]);
+            let upperb = [(*UPPERB_H*255.) as u8,(*UPPERB_L*255.) as u8,(*UPPERB_S*255.) as u8,255];
+            in_range(&blurred, &lowerb, &upperb, &mut mask_a).unwrap(); // h low
+                                                                                         // 103/180
+            in_range(&blurred, &[0,30,4,255], &[52,209,63,255], &mut mask_b).unwrap(); // hh28
+            highgui::imshow("a", &mask_a).unwrap();
+            //highgui::imshow("b", &mask_b).unwrap();
+            dbg!(blurred.row(240/2).unwrap().col(320/2).unwrap().data_bytes().unwrap());
+            let mut range = opencv::core::Mat::default();
+            bitwise_or_def(&mask_a, &mask_a, &mut mask).unwrap();
+            bitwise_and(&img, &img, &mut range, &mask).unwrap();
+
+            highgui::imshow("edge", &range).unwrap();
+
             let mask = opencv::core::Mat::default();
-            let mut debugout = opencv::core::Mat::default();
             let mut keypoints = opencv::core::Vector::default();
             let mut descriptors = opencv::core::Mat::default();
-            self.orb_detector.detect_and_compute_def(&img, &mask, &mut keypoints, &mut descriptors).unwrap();
+            self.orb_detector.detect_and_compute_def(&blurred, &mask, &mut keypoints, &mut descriptors).unwrap();
+            let mut debugout = opencv::core::Mat::default();
             opencv::features2d::draw_keypoints(&img, &keypoints, &mut debugout, opencv::core::VecN([0.,255.,255.,255.]), opencv::features2d::DrawMatchesFlags::DEFAULT).unwrap();
-            highgui::imshow("keypoints", &debugout).unwrap();
+            //highgui::imshow("keypoints", &debugout).unwrap();
+
+
 
 
             let mut matches: Vector<opencv::core::DMatch> = Vector::new();
@@ -121,7 +165,7 @@ impl Robot for MyRobot {
         let goal = if _time.elapsed.as_secs() % 8 < 4 {
             90. * DEG_TO_PX
         } else {
-            90. * DEG_TO_PX 
+            0. * DEG_TO_PX 
         };
         let error = goal - self.direction as f64;
         let mut error = error * 0.02;
@@ -133,8 +177,10 @@ impl Robot for MyRobot {
         let mut left_speed = error * MAX_SPEED;
         let mut right_speed = -error * MAX_SPEED;
 
-        self.left_motor.set_velocity(left_speed.clamp(-10., 10.));
+        //self.left_motor.set_velocity(left_speed.clamp(-10., 10.));
-        self.right_motor.set_velocity(right_speed.clamp(-10., 10.));
+        //self.right_motor.set_velocity(right_speed.clamp(-10., 10.));
+        self.left_motor.set_velocity(0.);
+        self.right_motor.set_velocity(0.);
     }
 }