ank/cruisecontrol
1
Fork 0
Code Issues Activity

autowebtweaks of wall thresholder

Browse source 
also implemented wall masking
This commit is contained in:
Andy Killorin 2024-09-10 21:04:53 -04:00
parent c6341820ca
commit 21272ee47b
Signed by: ank
GPG key ID: B6241CA3B552BCA4
4 changed files with 1650 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1598
simcontroller/Cargo.lock generated
View file

File diff suppressed because it is too large Load diff

1
simcontroller/Cargo.toml
View file

@ -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" }

4
simcontroller/Justfile Normal file
View file

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

60
simcontroller/src/main.rs
View file

@ -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.right_motor.set_velocity(right_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.left_motor.set_velocity(0.);
self.right_motor.set_velocity(0.);
}
}