diff --git a/Cargo.lock b/Cargo.lock
index f21ef8f..16c83a0 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -1098,18 +1098,6 @@ version = "0.11.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0c87e182de0887fd5361989c677c4e8f5000cd9491d6d563161a8f3a5519fc7f"
 
-[[package]]
-name = "deprecate-until"
-version = "0.1.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7a3767f826efbbe5a5ae093920b58b43b01734202be697e1354914e862e8e704"
-dependencies = [
- "proc-macro2 1.0.81",
- "quote 1.0.36",
- "semver",
- "syn 2.0.60",
-]
-
 [[package]]
 name = "derive_builder"
 version = "0.7.2"
@@ -1391,12 +1379,6 @@ dependencies = [
  "windows-sys 0.52.0",
 ]
 
-[[package]]
-name = "fixedbitset"
-version = "0.4.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80"
-
 [[package]]
 name = "flate2"
 version = "1.0.28"
@@ -1956,15 +1938,6 @@ dependencies = [
  "hashbrown 0.14.3",
 ]
 
-[[package]]
-name = "integer-sqrt"
-version = "0.1.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "276ec31bcb4a9ee45f58bec6f9ec700ae4cf4f4f8f2fa7e06cb406bd5ffdd770"
-dependencies = [
- "num-traits",
-]
-
 [[package]]
 name = "interprocess"
 version = "2.0.0"
@@ -2525,12 +2498,11 @@ checksum = "e943b2c21337b7e3ec6678500687cdc741b7639ad457f234693352075c082204"
 name = "navigator"
 version = "0.1.0"
 dependencies = [
- "costmap",
  "fxhash",
  "indexmap 2.2.6",
  "nalgebra",
+ "obstacles",
  "ordered-float",
- "pathfinding",
  "rig",
  "simba",
  "spin_sleep",
@@ -3001,21 +2973,6 @@ version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8835116a5c179084a830efb3adc117ab007512b535bc1a21c991d3b32a6b44dd"
 
-[[package]]
-name = "pathfinding"
-version = "4.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f0a21c30f03223ae4a4c892f077b3189133689b8a659a84372f8422384ce94c9"
-dependencies = [
- "deprecate-until",
- "fixedbitset",
- "indexmap 2.2.6",
- "integer-sqrt",
- "num-traits",
- "rustc-hash",
- "thiserror",
-]
-
 [[package]]
 name = "peeking_take_while"
 version = "0.1.2"
@@ -4105,6 +4062,7 @@ dependencies = [
  "image",
  "log",
  "nalgebra",
+ "ordered-float",
  "rand",
  "rayon",
  "serde",
diff --git a/navigator/Cargo.toml b/navigator/Cargo.toml
index 691140e..b8a5e43 100644
--- a/navigator/Cargo.toml
+++ b/navigator/Cargo.toml
@@ -10,10 +10,10 @@ edition = "2021"
 unros = { path = "../unros" }
 rig = { path = "../rig" }
 nalgebra = { workspace = true }
+fxhash = { workspace = true }
 ordered-float = { workspace = true }
-costmap = { path = "../costmap" }
+obstacles = { path = "../obstacles" }
 spin_sleep = { workspace = true }
-pathfinding = "4"
-fxhash = { workspace = true }
+# pathfinding = "4"
 indexmap = "2.2.3"
 simba = { workspace = true }
\ No newline at end of file
diff --git a/navigator/src/drive.rs b/navigator/src/drive.rs
index 539f673..e20c401 100644
--- a/navigator/src/drive.rs
+++ b/navigator/src/drive.rs
@@ -1,14 +1,15 @@
 use core::{fmt::Debug, ops::Deref};
 
 use ordered_float::NotNan;
+use unros::float::Float;
 
 #[derive(Clone, Copy, PartialEq, Eq, Default)]
-pub struct Steering {
-    pub left: NotNan<f32>,
-    pub right: NotNan<f32>,
+pub struct Steering<N: Float=f32> {
+    pub left: NotNan<N>,
+    pub right: NotNan<N>,
 }
 
-impl Debug for Steering {
+impl<N: Float> Debug for Steering<N> {
     fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
         f.debug_struct("Steering")
             .field("left", self.left.deref())
@@ -17,27 +18,27 @@ impl Debug for Steering {
     }
 }
 
-impl Steering {
+impl<N: Float> Steering<N> {
     /// Shorthand to make this struct if you know the given values are not `NaN`.
     ///
     /// # Panics
     /// Panics if either left or right are `NaN`. To handle this possibility gracefully,
     /// you should just construct this struct normally as the fields are public.
-    pub fn new(left: f32, right: f32) -> Self {
+    pub fn new(left: N, right: N) -> Self {
         Self {
             left: NotNan::new(left).unwrap(),
             right: NotNan::new(right).unwrap(),
         }
     }
 
-    pub fn from_drive_and_steering(drive: NotNan<f32>, steering: NotNan<f32>) -> Self {
+    pub fn from_drive_and_steering(drive: NotNan<N>, steering: NotNan<N>) -> Self {
         let mut left = drive;
         let mut right = drive;
 
-        if steering.into_inner() > 0.0 {
-            right *= (0.5 - steering.into_inner()) * 2.0;
+        if steering.into_inner() > N::zero() {
+            right *= (nalgebra::convert::<_, N>(0.5) - steering.into_inner()) * nalgebra::convert(2.0);
         } else {
-            left *= (0.5 + steering.into_inner()) * 2.0;
+            left *= (nalgebra::convert::<_, N>(0.5) + steering.into_inner()) * nalgebra::convert(2.0);
         }
 
         Self { left, right }
diff --git a/navigator/src/lib.rs b/navigator/src/lib.rs
index b5a298a..fb93bde 100644
--- a/navigator/src/lib.rs
+++ b/navigator/src/lib.rs
@@ -3,16 +3,11 @@
 use std::{ops::Deref, sync::Arc, time::Duration};
 
 use drive::Steering;
-use nalgebra::{Point3, UnitVector2, Vector2};
+use nalgebra::{Point3, UnitVector2, Vector2, Vector3};
 use ordered_float::NotNan;
 use rig::{RigSpace, RobotBaseRef};
 use unros::{
-    node::AsyncNode,
-    pubsub::{subs::DirectSubscription, Publisher, PublisherRef, Subscriber, WatchSubscriber},
-    runtime::RuntimeContext,
-    setup_logging,
-    tokio::task::block_in_place,
-    DontDrop, ShouldNotDrop,
+    float::Float, node::AsyncNode, pubsub::{subs::DirectSubscription, Publisher, PublisherRef, Subscriber, WatchSubscriber}, runtime::RuntimeContext, setup_logging, tokio::task::block_in_place, DontDrop, ShouldNotDrop
 };
 
 pub mod drive;
@@ -20,9 +15,6 @@ pub mod pathfinding;
 // mod utils;
 // pub mod pathfinders;
 
-type Float = f32;
-
-const PI: Float = std::f64::consts::PI as Float;
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
 pub enum DriveMode {
@@ -32,19 +24,19 @@ pub enum DriveMode {
 }
 
 #[derive(ShouldNotDrop)]
-pub struct DifferentialDriver<F: FnMut(Float) -> Float + Send + 'static> {
-    path_sub: Subscriber<Arc<[Point3<Float>]>>,
-    steering_signal: Publisher<Steering>,
+pub struct DifferentialDriver<N: Float, F: FnMut(N) -> N + Send + 'static> {
+    path_sub: Subscriber<Arc<[Point3<N>]>>,
+    steering_signal: Publisher<Steering<N>>,
     robot_base: RobotBaseRef,
     pub refresh_rate: Duration,
     drive_mode: WatchSubscriber<DriveMode>,
-    pub full_turn_angle: Float,
-    pub completion_distance: Float,
+    pub full_turn_angle: N,
+    pub completion_distance: N,
     pub turn_fn: F,
     dont_drop: DontDrop<Self>,
 }
 
-impl DifferentialDriver<fn(Float) -> Float> {
+impl<N: Float> DifferentialDriver<N, fn(N) -> N> {
     pub fn new(robot_base: RobotBaseRef) -> Self {
         Self {
             path_sub: Subscriber::new(1),
@@ -52,21 +44,21 @@ impl DifferentialDriver<fn(Float) -> Float> {
             robot_base,
             refresh_rate: Duration::from_millis(20),
             drive_mode: WatchSubscriber::new(DriveMode::Both),
-            completion_distance: 0.15,
+            completion_distance: nalgebra::convert(0.15),
             // 30 degrees
-            full_turn_angle: std::f64::consts::FRAC_PI_6 as Float,
-            turn_fn: |frac| -2.0 * frac + 1.0,
+            full_turn_angle: N::frac_pi_6(),
+            turn_fn: |frac| nalgebra::convert::<_, N>(-2.0) * frac + N::one(),
             dont_drop: DontDrop::new("diff-driver"),
         }
     }
 }
 
-impl<F: FnMut(Float) -> Float + Send + 'static> DifferentialDriver<F> {
-    pub fn steering_pub(&self) -> PublisherRef<Steering> {
+impl<N: Float, F: FnMut(N) -> N + Send + 'static> DifferentialDriver<N, F> {
+    pub fn steering_pub(&self) -> PublisherRef<Steering<N>> {
         self.steering_signal.get_ref()
     }
 
-    pub fn create_path_sub(&self) -> DirectSubscription<Arc<[Point3<Float>]>> {
+    pub fn create_path_sub(&self) -> DirectSubscription<Arc<[Point3<N>]>> {
         self.path_sub.create_subscription()
     }
 
@@ -75,9 +67,9 @@ impl<F: FnMut(Float) -> Float + Send + 'static> DifferentialDriver<F> {
     }
 }
 
-impl<F> AsyncNode for DifferentialDriver<F>
+impl<N: Float, F> AsyncNode for DifferentialDriver<N, F>
 where
-    F: FnMut(Float) -> Float + Send + 'static,
+    F: FnMut(N) -> N + Send + 'static,
 {
     type Result = ();
 
@@ -104,7 +96,7 @@ where
                     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 position: Vector2<N> = nalgebra::convert(Vector2::new(isometry.translation.x, isometry.translation.z));
 
                     let (i, _) = path
                         .iter()
@@ -122,7 +114,7 @@ where
 
                     let next = Vector2::new(next.x, next.z);
 
-                    let forward = isometry.get_forward_vector();
+                    let forward: Vector3<N> = nalgebra::convert(isometry.get_forward_vector());
                     let forward = UnitVector2::new_normalize(Vector2::new(forward.x, forward.z));
 
                     let mut travel = next - position;
@@ -133,42 +125,42 @@ where
                     }
 
                     travel.unscale_mut(distance);
-                    let cross = (forward.x * travel.y - forward.y * travel.x).signum();
+                    let cross = (forward.x * travel.y - forward.y * travel.x).sign();
                     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 mut reversing = N::one();
 
                     if self.drive_mode.deref() == &DriveMode::ReverseOnly
-                        || (angle > PI / 2.0 && self.drive_mode.deref() != &DriveMode::ForwardOnly)
+                        || (angle > N::pi() / nalgebra::convert(2.0) && self.drive_mode.deref() != &DriveMode::ForwardOnly)
                     {
-                        angle = PI - angle;
-                        reversing = -1.0;
+                        angle = N::pi() - angle;
+                        reversing = -N::one();
                     }
 
                     if angle > self.full_turn_angle {
-                        if cross > 0.0 {
+                        if cross > N::zero() {
                             self.steering_signal
-                                .set(Steering::new(1.0 * reversing, -1.0 * reversing));
+                                .set(Steering::new(N::one() * reversing, -N::one() * reversing));
                         } else {
                             self.steering_signal
-                                .set(Steering::new(-1.0 * reversing, 1.0 * reversing));
+                                .set(Steering::new(-N::one() * reversing, N::one() * reversing));
                         }
                     } else {
                         let smaller_ratio = (self.turn_fn)(angle / self.full_turn_angle);
 
-                        if cross > 0.0 {
+                        if cross > N::zero() {
                             self.steering_signal
-                                .set(Steering::new(1.0 * reversing, smaller_ratio * reversing));
+                                .set(Steering::new(N::one() * reversing, smaller_ratio * reversing));
                         } else {
                             self.steering_signal
-                                .set(Steering::new(smaller_ratio * reversing, 1.0 * reversing));
+                                .set(Steering::new(smaller_ratio * reversing, N::one() * reversing));
                         }
                     }
 
                     sleeper.sleep(self.refresh_rate);
                 }
-                self.steering_signal.set(Steering::new(0.0, 0.0));
+                self.steering_signal.set(Steering::new(N::zero(), N::zero()));
             });
 
             // if let Some(goal_forward) = goal_forward {
diff --git a/navigator/src/pathfinding/alg.rs b/navigator/src/pathfinding/alg.rs
new file mode 100644
index 0000000..f7cefdc
--- /dev/null
+++ b/navigator/src/pathfinding/alg.rs
@@ -0,0 +1,123 @@
+use std::{cmp::Ordering, collections::BinaryHeap, future::Future, hash::{BuildHasherDefault, Hash}, ops::Add};
+
+use fxhash::FxHasher;
+use indexmap::{map::Entry, IndexMap};
+
+
+type FxIndexMap<K, V> = IndexMap<K, V, BuildHasherDefault<FxHasher>>;
+
+fn reverse_path<N, V, F>(parents: &FxIndexMap<N, V>, mut parent: F, start: usize) -> Vec<N>
+where
+    N: Eq + Hash + Clone,
+    F: FnMut(&V) -> usize,
+{
+    let mut i = start;
+    let path = std::iter::from_fn(|| {
+        parents.get_index(i).map(|(node, value)| {
+            i = parent(value);
+            node
+        })
+    })
+    .collect::<Vec<&N>>();
+    // Collecting the going through the vector is needed to revert the path because the
+    // unfold iterator is not double-ended due to its iterative nature.
+    path.into_iter().rev().cloned().collect()
+}
+
+struct SmallestCostHolder<K> {
+    estimated_cost: K,
+    cost: K,
+    index: usize,
+}
+
+impl<K: PartialEq> PartialEq for SmallestCostHolder<K> {
+    fn eq(&self, other: &Self) -> bool {
+        self.estimated_cost.eq(&other.estimated_cost) && self.cost.eq(&other.cost)
+    }
+}
+
+impl<K: PartialEq> Eq for SmallestCostHolder<K> {}
+
+impl<K: Ord> PartialOrd for SmallestCostHolder<K> {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl<K: Ord> Ord for SmallestCostHolder<K> {
+    fn cmp(&self, other: &Self) -> Ordering {
+        match other.estimated_cost.cmp(&self.estimated_cost) {
+            Ordering::Equal => self.cost.cmp(&other.cost),
+            s => s,
+        }
+    }
+}
+
+pub async fn astar<N, C, FN, IN, FH, FS, Fut>(
+    start: &N,
+    mut successors: FN,
+    mut heuristic: FH,
+    mut success: FS,
+) -> Option<(Vec<N>, C)>
+where
+    N: Eq + Hash + Clone,
+    C: Default + Ord + Add<Output = C> + Copy,
+    FN: FnMut(&N) -> Fut,
+    Fut: Future<Output = IN>,
+    IN: IntoIterator<Item = (N, C)>,
+    FH: FnMut(&N) -> C,
+    FS: FnMut(&N) -> bool,
+{
+    let mut to_see = BinaryHeap::new();
+    to_see.push(SmallestCostHolder {
+        estimated_cost: C::default(),
+        cost: C::default(),
+        index: 0,
+    });
+    let mut parents: FxIndexMap<N, (usize, C)> = FxIndexMap::default();
+    parents.insert(start.clone(), (usize::max_value(), C::default()));
+    while let Some(SmallestCostHolder { cost, index, .. }) = to_see.pop() {
+        let successors = {
+            let (node, &(_, c)) = parents.get_index(index).unwrap(); // Cannot fail
+            if success(node) {
+                let path = reverse_path(&parents, |&(p, _)| p, index);
+                return Some((path, cost));
+            }
+            // We may have inserted a node several time into the binary heap if we found
+            // a better way to access it. Ensure that we are currently dealing with the
+            // best path and discard the others.
+            if cost > c {
+                continue;
+            }
+            successors(node).await
+        };
+        for (successor, move_cost) in successors {
+            let new_cost = cost + move_cost;
+            let h; // heuristic(&successor)
+            let n; // index for successor
+            match parents.entry(successor) {
+                Entry::Vacant(e) => {
+                    h = heuristic(e.key());
+                    n = e.index();
+                    e.insert((index, new_cost));
+                }
+                Entry::Occupied(mut e) => {
+                    if e.get().1 > new_cost {
+                        h = heuristic(e.key());
+                        n = e.index();
+                        e.insert((index, new_cost));
+                    } else {
+                        continue;
+                    }
+                }
+            }
+
+            to_see.push(SmallestCostHolder {
+                estimated_cost: new_cost + h,
+                cost: new_cost,
+                index: n,
+            });
+        }
+    }
+    None
+}
\ No newline at end of file
diff --git a/navigator/src/pathfinding.rs b/navigator/src/pathfinding/mod.rs
similarity index 86%
rename from navigator/src/pathfinding.rs
rename to navigator/src/pathfinding/mod.rs
index b069026..e18c604 100644
--- a/navigator/src/pathfinding.rs
+++ b/navigator/src/pathfinding/mod.rs
@@ -1,29 +1,22 @@
 use std::{
-    marker::PhantomData,
-    sync::{
+    future::Future, marker::PhantomData, sync::{
         atomic::{AtomicU8, Ordering},
         Arc,
-    },
-    time::{Duration, Instant},
+    }, time::{Duration, Instant}
 };
 
-use costmap::Costmap;
 use nalgebra::{convert as nconvert, Point3, RealField, UnitQuaternion, Vector2, Vector3};
+use obstacles::ObstacleHub;
 use ordered_float::{FloatCore, NotNan};
-use pathfinding::directed::{astar::astar, bfs::bfs};
 use rig::RobotBaseRef;
 use simba::scalar::SupersetOf;
 use spin_sleep::SpinSleeper;
 use unros::{
-    node::SyncNode,
-    pubsub::{subs::DirectSubscription, Publisher, PublisherRef, Subscriber, WatchSubscriber},
-    runtime::RuntimeContext,
-    service::{new_service, Service, ServiceHandle},
-    setup_logging,
-    tokio::{self, sync::oneshot},
-    DontDrop, ShouldNotDrop,
+    float::Float, node::AsyncNode, pubsub::{subs::DirectSubscription, Publisher, PublisherRef, Subscriber, WatchSubscriber}, runtime::RuntimeContext, service::{new_service, Service, ServiceHandle}, setup_logging, tokio::{self, sync::oneshot}, DontDrop, ShouldNotDrop
 };
 
+mod alg;
+
 #[derive(Debug)]
 pub enum NavigationError {
     NoPath,
@@ -68,26 +61,26 @@ pub type NavigationServiceHandle<N> =
 //     }
 // }
 
-pub trait PathfindingEngine<N: RealField>: Send + 'static {
+pub trait PathfindingEngine<N: Float>: Send + 'static {
     fn pathfind(
         &mut self,
         start: Point3<N>,
         end: Point3<N>,
-        costmap: &Costmap<N>,
+        obstacle_hub: &ObstacleHub<N>,
         resolution: N,
         agent_radius: N,
         max_height_diff: N,
         context: &RuntimeContext,
-    ) -> Option<Vec<Point3<N>>>;
+    ) -> impl Future<Output=Option<Vec<Point3<N>>>>;
 }
 
 #[derive(ShouldNotDrop)]
 pub struct Pathfinder<
-    N: RealField + SupersetOf<f32> + Copy = f32,
+    N: Float = f32,
     E: PathfindingEngine<N> = DirectPathfinder<N>,
 > {
     engine: E,
-    costmap_sub: Subscriber<Costmap<N>>,
+    obstacle_hub: ObstacleHub<N>,
     dont_drop: DontDrop<Self>,
     service: Service<Point3<N>, NavigationError, NavigationProgress, Result<(), NavigationError>>,
     service_handle: NavigationServiceHandle<N>,
@@ -100,12 +93,12 @@ pub struct Pathfinder<
     pub refresh_rate: Duration,
 }
 
-impl<N: RealField + SupersetOf<f32> + Copy, E: PathfindingEngine<N>> Pathfinder<N, E> {
-    pub fn new_with_engine(agent_radius: N, engine: E, robot_base: RobotBaseRef) -> Self {
+impl<N: Float, E: PathfindingEngine<N>> Pathfinder<N, E> {
+    pub fn new_with_engine(agent_radius: N, engine: E, obstacle_hub: ObstacleHub<N>, robot_base: RobotBaseRef) -> Self {
         let (service, service_handle) = new_service();
         Self {
             engine,
-            costmap_sub: Subscriber::new(1),
+            obstacle_hub,
             dont_drop: DontDrop::new("pathfinder"),
             service,
             service_handle,
@@ -119,10 +112,6 @@ impl<N: RealField + SupersetOf<f32> + Copy, E: PathfindingEngine<N>> Pathfinder<
         }
     }
 
-    pub fn create_costmap_sub(&self) -> DirectSubscription<Costmap<N>> {
-        self.costmap_sub.create_subscription()
-    }
-
     pub fn get_path_pub(&self) -> PublisherRef<Arc<[Point3<N>]>> {
         self.path_pub.get_ref()
     }
@@ -133,34 +122,16 @@ impl<N: RealField + SupersetOf<f32> + Copy, E: PathfindingEngine<N>> Pathfinder<
 }
 
 impl<
-        N: RealField
-            + SupersetOf<f32>
-            + SupersetOf<usize>
-            + SupersetOf<i64>
-            + SupersetOf<isize>
-            + SupersetOf<u8>
-            + FloatCore
-            + Copy,
+        N: Float,
         E: PathfindingEngine<N>,
-    > SyncNode for Pathfinder<N, E>
+    > AsyncNode for Pathfinder<N, E>
 {
     type Result = ();
 
-    fn run(mut self, context: RuntimeContext) -> Self::Result {
+    async fn run(mut self, context: RuntimeContext) -> Self::Result {
         setup_logging!(context);
         drop(self.service_handle);
         self.dont_drop.ignore_drop = true;
-        let (costmap_sender, costmap_receiver) = oneshot::channel();
-        tokio::spawn(async move {
-            let Ok(costmap) = self.costmap_sub.into_watch_or_closed().await else {
-                return;
-            };
-            let _ = costmap_sender.send(costmap);
-        });
-        let Ok(mut costmap) = costmap_receiver.blocking_recv() else {
-            warn!("Costmap dropped before we could start");
-            return;
-        };
         let mut start_time = Instant::now();
         let sleeper = SpinSleeper::default();
 
@@ -184,16 +155,15 @@ impl<
                 let mut start: Vector3<N> =
                     nalgebra::convert(self.robot_base.get_isometry().translation.vector);
 
-                WatchSubscriber::try_update(&mut costmap);
                 if let Some(path) = self.engine.pathfind(
                     start.into(),
                     end,
-                    &costmap,
+                    &self.obstacle_hub,
                     self.resolution,
                     self.agent_radius,
                     self.max_height_diff,
                     &context,
-                ) {
+                ).await {
                     let path: Arc<[Point3<N>]> = Arc::from(path.into_boxed_slice());
                     self.path_pub.set(path.clone());
 
@@ -229,7 +199,7 @@ impl<
                                 to.coords,
                                 self.agent_radius,
                                 self.max_height_diff,
-                                &costmap,
+                                &self.obstacle_hub,
                                 self.resolution,
                             ) {
                                 break 'repathfind;
@@ -249,22 +219,13 @@ pub struct DirectPathfinder<N> {
     phantom: PhantomData<N>,
 }
 
-impl<N> PathfindingEngine<N> for DirectPathfinder<N>
-where
-    N: RealField
-        + Copy
-        + SupersetOf<usize>
-        + SupersetOf<isize>
-        + SupersetOf<i64>
-        + SupersetOf<f32>
-        + SupersetOf<u8>
-        + SupersetOf<u32>,
+impl<N: Float> PathfindingEngine<N> for DirectPathfinder<N>
 {
     fn pathfind(
         &mut self,
         start: Point3<N>,
         end: Point3<N>,
-        costmap: &Costmap<N>,
+        obstacle_hub: &ObstacleHub<N>,
         resolution: N,
         agent_radius: N,
         max_height_diff: N,
@@ -275,7 +236,7 @@ where
         let mut pre_path = vec![];
 
         if !costmap.is_global_point_safe(start.into(), agent_radius, max_height_diff) {
-            if let Some(mut path) = bfs(
+            if let Some(mut path) = astar(
                 &CVec3 {
                     inner: start,
                     resolution,
@@ -451,7 +412,7 @@ fn traverse_to<N>(
     to: Vector3<N>,
     agent_radius: N,
     max_height_diff: N,
-    costmap: &Costmap<N>,
+    obstacle_hub: &ObstacleHub<N>,
     resolution: N,
 ) -> bool
 where
diff --git a/unros-core/Cargo.toml b/unros-core/Cargo.toml
index 175d294..a04e090 100644
--- a/unros-core/Cargo.toml
+++ b/unros-core/Cargo.toml
@@ -28,4 +28,5 @@ color-eyre = { version = "0.6", default-features = false }
 ctrlc = "3.4.2"
 rayon = { workspace = true }
 nalgebra = { workspace = true }
-simba = { workspace = true }
\ No newline at end of file
+simba = { workspace = true }
+ordered-float = { workspace = true }
\ No newline at end of file
diff --git a/unros-core/src/float.rs b/unros-core/src/float.rs
index e0efbc8..bf74470 100644
--- a/unros-core/src/float.rs
+++ b/unros-core/src/float.rs
@@ -1,6 +1,7 @@
 use std::iter::Sum;
 
 use nalgebra::RealField;
+use ordered_float::FloatCore;
 use simba::scalar::{SubsetOf, SupersetOf};
 
 pub trait Float:
@@ -21,6 +22,7 @@ pub trait Float:
     + SupersetOf<i32>
     + SupersetOf<i16>
     + SupersetOf<i8>
+    + FloatCore
     + Sum
 {
     const MAX: Self;
@@ -28,7 +30,7 @@ pub trait Float:
 
     fn to_f32(self) -> f32;
     fn to_f64(self) -> f64;
-    fn is_nan(self) -> bool;
+    fn sign(self) -> Self;
 
     fn is_f32() -> bool;
     fn is_f64() -> bool;
@@ -46,8 +48,8 @@ impl Float for f32 {
         self as f64
     }
 
-    fn is_nan(self) -> bool {
-        self.is_nan()
+    fn sign(self) -> Self {
+        self.signum()
     }
 
     fn is_f32() -> bool {
@@ -70,8 +72,8 @@ impl Float for f64 {
         self
     }
 
-    fn is_nan(self) -> bool {
-        self.is_nan()
+    fn sign(self) -> Self {
+        self.signum()
     }
 
     fn is_f32() -> bool {