Warn on behavior bug (#1408)

HULKs · Jul 21, 2024 · 1975da5 · 1975da5
1 parent 89ed237
commit 1975da5
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/crates/control/src/path_planner.rs b/crates/control/src/path_planner.rs
@@ -1,6 +1,7 @@
 use color_eyre::{eyre::eyre, Result};
 use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
 use linear_algebra::{distance, point, vector, Isometry2, Orientation2, Point2};
+use log::warn;
 use ordered_float::NotNan;
 use smallvec::SmallVec;
 
@@ -281,6 +282,8 @@ impl PathPlanner {
             if let Some(to_destination_normalized) = to_destination.try_normalize(f32::EPSILON) {
                 let safety_radius = circle.radius * 1.1;
                 destination += to_destination_normalized * (safety_radius - to_destination.norm());
+            } else {
+                warn!("Behavior bug: Path planning destination was an obstacle")
             }
         }