Skip to content
This repository has been archived by the owner on Dec 4, 2024. It is now read-only.

Commit

Permalink
finalizing window localization
Browse files Browse the repository at this point in the history
  • Loading branch information
@manglemix
manglemix committed Apr 15, 2024
1 parent ecf8e75 commit aa71701
Show file tree
Hide file tree
Showing 7 changed files with 148 additions and 109 deletions.
39 changes: 27 additions & 12 deletions localization/src/engines/window.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ pub struct WindowLocalizer<N: Float, FI, FV, FA, FAV> {
orientation_queue: VecDeque<Observation<UnitQuaternion<N>, N>>,
last_orientation: Instant,

additional_time_factor: N,
max_no_observation_duration: Duration,

isometry_func: FI,
Expand All @@ -66,22 +67,28 @@ where
.sum()
}

fn iter_with_factor<T>(
queue: &VecDeque<Observation<T, N>>,
fn iter_with_factor<'a, T>(
&'a self,
queue: &'a VecDeque<Observation<T, N>>,
) -> impl Iterator<Item = (&Observation<T, N>, N)> {
let total_time = Self::total_time(queue);
let two = N::one() + N::one();
let time_height = two / total_time;
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;

Expand All @@ -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 {
Expand All @@ -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 {
Expand Down Expand Up @@ -210,19 +220,21 @@ where
}

pub type DefaultWindowConfig<N> = WindowConfig<
N,
fn(&mut Isometry3<N>),
fn(&mut Vector3<N>),
fn(&mut Vector3<N>),
fn(&mut UnitQuaternion<N>),
>;

pub struct WindowConfig<FI, FV, FA, FAV> {
pub struct WindowConfig<N: Float, FI, FV, FA, FAV> {
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<N: Float> Default for DefaultWindowConfig<N> {
Expand All @@ -234,6 +246,7 @@ impl<N: Float> Default for DefaultWindowConfig<N> {
linear_velocity_func: |_| {},
linear_acceleration_func: |_| {},
angular_velocity_func: |_| {},
additional_time_factor: N::zero(),
}
}
}
Expand All @@ -245,7 +258,7 @@ where
FA: FnMut(&mut Vector3<N>) + Send + 'static + Clone,
FAV: FnMut(&mut UnitQuaternion<N>) + Send + 'static + Clone,
{
type Config = WindowConfig<FI, FV, FA, FAV>;
type Config = WindowConfig<N, FI, FV, FA, FAV>;

fn from_config(config: &Self::Config, robot_base: RobotBaseRef) -> Self {
let now = Instant::now();
Expand All @@ -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(),
Expand Down
18 changes: 9 additions & 9 deletions lunabase/lunasim.tscn
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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()
Expand Down
14 changes: 6 additions & 8 deletions lunasimbot/src/main.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
}
});
Expand All @@ -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());
Expand All @@ -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"));
Expand Down
Loading

0 comments on commit aa71701

Please sign in to comment.