From aa7170199e1e27745d5a92692b572e06399d52b5 Mon Sep 17 00:00:00 2001 From: Manglemix Date: Sun, 14 Apr 2024 20:33:49 -0600 Subject: [PATCH] finalizing window localization --- localization/src/engines/window.rs | 39 +++++--- lunabase/lunasim.tscn | 18 ++-- lunasimbot/src/main.rs | 14 ++- navigator/src/lib.rs | 154 ++++++++++++++--------------- navigator/src/pathfinding.rs | 2 +- unros-core/src/pubsub/mod.rs | 12 +++ unros-core/src/pubsub/subs.rs | 18 +++- 7 files changed, 148 insertions(+), 109 deletions(-) diff --git a/localization/src/engines/window.rs b/localization/src/engines/window.rs index c76e14c4..2e69e069 100644 --- a/localization/src/engines/window.rs +++ b/localization/src/engines/window.rs @@ -40,6 +40,7 @@ pub struct WindowLocalizer { orientation_queue: VecDeque, N>>, last_orientation: Instant, + additional_time_factor: N, max_no_observation_duration: Duration, isometry_func: FI, @@ -66,8 +67,9 @@ where .sum() } - fn iter_with_factor( - queue: &VecDeque>, + fn iter_with_factor<'a, T>( + &'a self, + queue: &'a VecDeque>, ) -> impl Iterator, N)> { let total_time = Self::total_time(queue); let two = N::one() + N::one(); @@ -75,13 +77,18 @@ where let mut elapsed_time = N::zero(); let total_variance = Self::total_variance(queue); + let additional_height = self.additional_time_factor / total_time; queue.iter().map(move |obs| { let delta: N = nconvert(obs.delta.as_secs_f32()); - let start_time = -time_height / total_time * elapsed_time + time_height; - let end_time = -time_height / total_time * (elapsed_time + delta) + time_height; + let start_time = + -time_height / total_time * elapsed_time + time_height + additional_height; + let end_time = -time_height / total_time * (elapsed_time + delta) + + time_height + + additional_height; elapsed_time += delta; - let time_factor = (end_time + start_time) / two * delta; + let time_factor = + (end_time + start_time) / two * delta / (N::one() + self.additional_time_factor); let variance_factor = obs.variance / total_variance; @@ -90,26 +97,29 @@ where } fn process(&mut self) { - if let Some(result) = Self::iter_with_factor(&self.linear_acceleration_queue) + if let Some(result) = self + .iter_with_factor(&self.linear_acceleration_queue) .map(|(obs, factor)| obs.observation * factor) .reduce(|a, b| a + b) { self.linear_acceleration = result; } - if let Some(result) = Self::iter_with_factor(&self.linear_velocity_queue) + if let Some(result) = self + .iter_with_factor(&self.linear_velocity_queue) .map(|(obs, factor)| obs.observation * factor) .reduce(|a, b| a + b) { self.linear_velocity = result; } - if let Some(result) = Self::iter_with_factor(&self.position_queue) + if let Some(result) = self + .iter_with_factor(&self.position_queue) .map(|(obs, factor)| obs.observation * factor) .reduce(|a, b| a + b) { self.isometry.translation = result.into(); } if let Some(result) = quat_mean( - Self::iter_with_factor(&self.angular_velocity_queue) + self.iter_with_factor(&self.angular_velocity_queue) .map(|(obs, factor)| (obs.observation, factor)), ) { match result { @@ -120,7 +130,7 @@ where } } if let Some(result) = quat_mean( - Self::iter_with_factor(&self.orientation_queue) + self.iter_with_factor(&self.orientation_queue) .map(|(obs, factor)| (obs.observation, factor)), ) { match result { @@ -210,19 +220,21 @@ where } pub type DefaultWindowConfig = WindowConfig< + N, fn(&mut Isometry3), fn(&mut Vector3), fn(&mut Vector3), fn(&mut UnitQuaternion), >; -pub struct WindowConfig { +pub struct WindowConfig { pub bucket_duration: Duration, pub isometry_func: FI, pub linear_velocity_func: FV, pub linear_acceleration_func: FA, pub angular_velocity_func: FAV, pub max_no_observation_duration: Duration, + pub additional_time_factor: N, } impl Default for DefaultWindowConfig { @@ -234,6 +246,7 @@ impl Default for DefaultWindowConfig { linear_velocity_func: |_| {}, linear_acceleration_func: |_| {}, angular_velocity_func: |_| {}, + additional_time_factor: N::zero(), } } } @@ -245,7 +258,7 @@ where FA: FnMut(&mut Vector3) + Send + 'static + Clone, FAV: FnMut(&mut UnitQuaternion) + Send + 'static + Clone, { - type Config = WindowConfig; + type Config = WindowConfig; fn from_config(config: &Self::Config, robot_base: RobotBaseRef) -> Self { let now = Instant::now(); @@ -272,6 +285,8 @@ where orientation_queue: VecDeque::new(), last_orientation: now, + additional_time_factor: config.additional_time_factor, + max_no_observation_duration: config.max_no_observation_duration, isometry_func: config.isometry_func.clone(), diff --git a/lunabase/lunasim.tscn b/lunabase/lunasim.tscn index 5f8b687c..cc8acfee 100644 --- a/lunabase/lunasim.tscn +++ b/lunabase/lunasim.tscn @@ -93,10 +93,14 @@ var right_steering := 0.0 func _physics_process(delta: float) -> void: - if is_zero_approx(user_accel.slide(Vector3.DOWN).length()): + var acceleration = user_accel - global_transform.basis.z.slide(Vector3.UP).normalized() * ACCEL * (left_steering + right_steering) / 2 + var angle := (left_steering - right_steering) * SPEED / WHEEL_SEPARATION + global_rotate(Vector3.UP, - angle * delta) + + if is_zero_approx(acceleration.slide(Vector3.DOWN).length()): velocity = velocity.move_toward(Vector3.ZERO, ACCEL * delta) else: - velocity += (user_accel - velocity.normalized() * velocity.length_squared() * DRAG_FACTOR) * delta + velocity += (acceleration - velocity.normalized() * velocity.length_squared() * DRAG_FACTOR) * delta ang_vel = last_quat.inverse() * quaternion if ang_vel.get_axis().is_normalized(): ang_vel = Quaternion(ang_vel.get_axis(), ang_vel.get_angle() / delta) @@ -117,9 +121,9 @@ func _physics_process(delta: float) -> void: conn.put_float(velocity.x) conn.put_float(0.0) conn.put_float(velocity.z) - conn.put_float(user_accel.x) - conn.put_float(user_accel.y) - conn.put_float(user_accel.z) + conn.put_float(acceleration.x) + conn.put_float(acceleration.y) + conn.put_float(acceleration.z) conn.put_float(quaternion.w) conn.put_float(quaternion.x) conn.put_float(quaternion.y) @@ -169,11 +173,7 @@ func _physics_process(delta: float) -> void: var distance := child.global_position.distance_to(last_child.global_position) last_child.global_position -= last_child.global_transform.basis.z * distance / 2 last_child.scale.z = distance * 40 - - velocity += - global_transform.basis.z.slide(Vector3.UP).normalized() * SPEED * (left_steering + right_steering) / 2 - var angle := (left_steering - right_steering) * SPEED / WHEEL_SEPARATION - global_rotate(Vector3.UP, - angle * delta) #prints(left_steering, right_steering) move_and_slide() diff --git a/lunasimbot/src/main.rs b/lunasimbot/src/main.rs index 07341dd4..a56b8b2a 100644 --- a/lunasimbot/src/main.rs +++ b/lunasimbot/src/main.rs @@ -8,7 +8,7 @@ use localization::{ Localizer, }; use nalgebra::{Point3, Quaternion, UnitQuaternion, Vector3}; -use navigator::{pathfinding::Pathfinder, DifferentialDriver}; +use navigator::{pathfinding::Pathfinder, DifferentialDriver, DriveMode}; // use navigator::{pathfinders::DirectPathfinder, DifferentialDriver}; use rand_distr::{Distribution, Normal}; use rig::Robot; @@ -64,8 +64,8 @@ async fn main(context: MainRuntimeContext) -> anyhow::Result<()> { } }; let position = debug_element_ref.get_global_isometry().translation.vector; - let img = costmap.get_obstacle_map(position.into(), 0.015, 400, 400, 0.3, 0.15); - // let img = costmap.get_cost_map(position.into(), 0.015, 400, 400); + // let img = costmap.get_obstacle_map(position.into(), 0.015, 400, 400, 0.5, 0.05); + let img = costmap.get_cost_map(position.into(), 0.015, 400, 400); costmap_display.write_frame(Arc::new(img.into())).unwrap(); } }); @@ -81,7 +81,8 @@ async fn main(context: MainRuntimeContext) -> anyhow::Result<()> { .accept_subscription(path_sub.create_subscription()); let driver = DifferentialDriver::new(robot_base.get_ref()); - // driver.can_reverse = true; + let mut drive_mode_pub = driver.create_drive_mode_sub().into_mono_pub(); + drive_mode_pub.set(DriveMode::ForwardOnly); pathfinder .get_path_pub() .accept_subscription(driver.create_path_sub()); @@ -96,10 +97,7 @@ async fn main(context: MainRuntimeContext) -> anyhow::Result<()> { scaled_axis.z = 0.0; isometry.rotation = UnitQuaternion::new(scaled_axis); }; - // localizer.likelihood_table.position = - // Box::new(|pos| if pos.y.abs() >= 0.2 { 0.0 } else { 1.0 }); - // localizer.likelihood_table.linear_velocity = - // Box::new(|vel| if vel.magnitude() > 0.75 { 0.0 } else { 1.0 }); + localizer.engine_config.additional_time_factor = 2.0; let position_pub = Publisher::default(); position_pub.accept_subscription(localizer.create_position_sub().set_name("position")); diff --git a/navigator/src/lib.rs b/navigator/src/lib.rs index 7b8fab47..05cbdb01 100644 --- a/navigator/src/lib.rs +++ b/navigator/src/lib.rs @@ -1,4 +1,4 @@ -use std::{sync::Arc, time::Duration}; +use std::{ops::Deref, sync::Arc, time::Duration}; use drive::Steering; use nalgebra::{Point3, UnitVector2, Vector2}; @@ -6,7 +6,7 @@ use ordered_float::NotNan; use rig::{RigSpace, RobotBaseRef}; use unros::{ node::AsyncNode, - pubsub::{subs::DirectSubscription, Publisher, PublisherRef, Subscriber}, + pubsub::{subs::DirectSubscription, Publisher, PublisherRef, Subscriber, WatchSubscriber}, runtime::RuntimeContext, setup_logging, tokio::task::block_in_place, @@ -22,13 +22,20 @@ type Float = f32; const PI: Float = std::f64::consts::PI as Float; +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum DriveMode { + ForwardOnly, + Both, + ReverseOnly, +} + #[derive(ShouldNotDrop)] pub struct DifferentialDriver Float + Send + 'static> { path_sub: Subscriber]>>, steering_signal: Publisher, robot_base: RobotBaseRef, pub refresh_rate: Duration, - pub can_reverse: bool, + drive_mode: WatchSubscriber, pub full_turn_angle: Float, pub turn_fn: F, dont_drop: DontDrop, @@ -41,7 +48,7 @@ impl DifferentialDriver Float> { steering_signal: Default::default(), robot_base, refresh_rate: Duration::from_millis(20), - can_reverse: false, + drive_mode: WatchSubscriber::new(DriveMode::Both), // 30 degrees full_turn_angle: std::f64::consts::FRAC_PI_6 as Float, turn_fn: |frac| -2.0 * frac + 1.0, @@ -58,6 +65,10 @@ impl Float + Send + 'static> DifferentialDriver { pub fn create_path_sub(&self) -> DirectSubscription]>> { self.path_sub.create_subscription() } + + pub fn create_drive_mode_sub(&self) -> DirectSubscription { + self.drive_mode.create_subscription() + } } impl AsyncNode for DifferentialDriver @@ -66,7 +77,7 @@ where { type Result = (); - async fn run(mut self, mut context: RuntimeContext) -> Self::Result { + async fn run(mut self, context: RuntimeContext) -> Self::Result { setup_logging!(context); self.dont_drop.ignore_drop = true; let sleeper = spin_sleep::SpinSleeper::default(); @@ -74,92 +85,79 @@ where loop { let mut path = self.path_sub.recv().await; - let (path_sub, steering_signal, robot_base, turn_fn, tmp_context) = - block_in_place(move || { - loop { - if context.is_runtime_exiting() { - break; - } - if let Some(new_path) = self.path_sub.try_recv() { - path = new_path; - } + block_in_place(|| { + loop { + if context.is_runtime_exiting() { + break; + } + if let Some(new_path) = self.path_sub.try_recv() { + path = new_path; + } - if path.is_empty() { - break; - } + if path.is_empty() { + break; + } + WatchSubscriber::try_update(&mut self.drive_mode); - let isometry = self.robot_base.get_isometry(); - let position = Vector2::new(isometry.translation.x, isometry.translation.z); + let isometry = self.robot_base.get_isometry(); + let position = Vector2::new(isometry.translation.x, isometry.translation.z); - let (i, _) = path - .iter() - .map(|v| Vector2::new(v.x, v.z)) - .enumerate() - .map(|(i, v)| (i, (v - position).magnitude_squared())) - .min_by_key(|(_, distance)| NotNan::new(*distance).unwrap()) - .unwrap(); + let (i, _) = path + .iter() + .map(|v| Vector2::new(v.x, v.z)) + .enumerate() + .map(|(i, v)| (i, (v - position).magnitude_squared())) + .min_by_key(|(_, distance)| NotNan::new(*distance).unwrap()) + .unwrap(); - if i == path.len() - 1 { - break; - } + if i == path.len() - 1 { + break; + } - let next = path[i + 1]; - let next = Vector2::new(next.x, next.z); + let next = path[i + 1]; + let next = Vector2::new(next.x, next.z); - let forward = isometry.get_forward_vector(); - let forward = - UnitVector2::new_normalize(Vector2::new(forward.x, forward.z)); + let forward = isometry.get_forward_vector(); + let forward = UnitVector2::new_normalize(Vector2::new(forward.x, forward.z)); - let travel = (next - position).normalize(); - let cross = (forward.x * travel.y - forward.y * travel.x).signum(); - assert!(!travel.x.is_nan(), "{position:?} {next:?} {path:?}"); - assert!(!travel.y.is_nan(), "{position:?} {next:?} {path:?}"); - let mut angle = forward.angle(&travel); - let mut reversing = 1.0; + let travel = (next - position).normalize(); + let cross = (forward.x * travel.y - forward.y * travel.x).signum(); + assert!(!travel.x.is_nan(), "{position:?} {next:?} {path:?}"); + assert!(!travel.y.is_nan(), "{position:?} {next:?} {path:?}"); + let mut angle = forward.angle(&travel); + let mut reversing = 1.0; - if angle > PI / 2.0 && self.can_reverse { - angle = PI - angle; - reversing = -1.0; - } + if self.drive_mode.deref() == &DriveMode::ReverseOnly + || (angle > PI / 2.0 && self.drive_mode.deref() != &DriveMode::ForwardOnly) + { + angle = PI - angle; + reversing = -1.0; + } - if angle > self.full_turn_angle { - if cross > 0.0 { - self.steering_signal - .set(Steering::new(1.0 * reversing, -1.0 * reversing)); - } else { - self.steering_signal - .set(Steering::new(-1.0 * reversing, 1.0 * reversing)); - } + if angle > self.full_turn_angle { + if cross > 0.0 { + self.steering_signal + .set(Steering::new(1.0 * reversing, -1.0 * reversing)); } else { - let smaller_ratio = (self.turn_fn)(angle / self.full_turn_angle); - - if cross > 0.0 { - self.steering_signal - .set(Steering::new(1.0 * reversing, smaller_ratio * reversing)); - } else { - self.steering_signal - .set(Steering::new(smaller_ratio * reversing, 1.0 * reversing)); - } + self.steering_signal + .set(Steering::new(-1.0 * reversing, 1.0 * reversing)); } + } else { + let smaller_ratio = (self.turn_fn)(angle / self.full_turn_angle); - sleeper.sleep(self.refresh_rate); + if cross > 0.0 { + self.steering_signal + .set(Steering::new(1.0 * reversing, smaller_ratio * reversing)); + } else { + self.steering_signal + .set(Steering::new(smaller_ratio * reversing, 1.0 * reversing)); + } } - self.steering_signal.set(Steering::new(0.0, 0.0)); - - ( - self.path_sub, - self.steering_signal, - self.robot_base, - self.turn_fn, - context, - ) - }); - - self.path_sub = path_sub; - self.steering_signal = steering_signal; - self.robot_base = robot_base; - self.turn_fn = turn_fn; - context = tmp_context; + + sleeper.sleep(self.refresh_rate); + } + self.steering_signal.set(Steering::new(0.0, 0.0)); + }); // if let Some(goal_forward) = goal_forward { // // Turn to goal orientation diff --git a/navigator/src/pathfinding.rs b/navigator/src/pathfinding.rs index 3e54bfa9..b336321b 100644 --- a/navigator/src/pathfinding.rs +++ b/navigator/src/pathfinding.rs @@ -113,7 +113,7 @@ impl + Copy, E: PathfindingEngine> Pathfinder< refresh_rate: Duration::from_millis(50), resolution: agent_radius, agent_radius, - max_height_diff: nalgebra::convert(0.15), + max_height_diff: nalgebra::convert(0.05), } } diff --git a/unros-core/src/pubsub/mod.rs b/unros-core/src/pubsub/mod.rs index e13032a5..91420543 100644 --- a/unros-core/src/pubsub/mod.rs +++ b/unros-core/src/pubsub/mod.rs @@ -162,6 +162,18 @@ impl> MonoPublisher { pub fn new() -> Self { Self { sub: None } } + + pub fn into_boxed(mut self) -> MonoPublisher + where + S: Send + 'static, + { + MonoPublisher { + sub: self.sub.take().map(|x| { + let dyn_box: Box + Send> = Box::new(x); + dyn_box + }), + } + } } impl> Drop for MonoPublisher { diff --git a/unros-core/src/pubsub/subs.rs b/unros-core/src/pubsub/subs.rs index fbacf586..04c42e82 100644 --- a/unros-core/src/pubsub/subs.rs +++ b/unros-core/src/pubsub/subs.rs @@ -11,7 +11,7 @@ use std::{ use log::warn; -use super::SubscriberInner; +use super::{MonoPublisher, Publisher, SubscriberInner}; /// A token produced only by Publishers, ensuring that only Unros /// can call specific methods. @@ -114,6 +114,22 @@ pub trait Subscription { /// Decrements the publisher count of the `Subscriber`, which is important for it to know /// when no more publishers are connected to it. fn decrement_publishers(&self, token: PublisherToken); + + fn into_mono_pub(self) -> MonoPublisher + where + Self: Sized, + { + MonoPublisher::from(self) + } + + fn into_pub(self) -> Publisher + where + Self: Sized + Send + 'static, + { + let publisher = Publisher::default(); + publisher.accept_subscription(self); + publisher + } } /// An object that must be passed to a `Publisher`, enabling the `Subscriber`