clean up path follower, edit pathfinder

Author: troylu8

godot/lunasim/path_manager.gd

@@ -28,7 +28,7 @@ func create_path(path:Array[Vector3]):
 func place_marker(pos:Vector3,num:String):
 	var marker = CSGCylinder3D.new()
 	marker.position=Vector3(pos.x,(pos.y/2)+0.05,pos.z)
-	marker.height=pos.y
+	marker.height=3
 	marker.radius=0.05
 	marker.name = str("marker",num)
 	marker.material=path_mat

lunabotics/common/src/lib.rs

@@ -4,6 +4,7 @@ use bitcode::{Decode, Encode};
 
 pub const AUDIO_FRAME_SIZE: u32 = 960;
 pub const AUDIO_SAMPLE_RATE: u32 = 48000;
+pub const THALASSIC_CELL_SIZE: f32 = 0.03125;
 pub const THALASSIC_WIDTH: u32 = 128;
 pub const THALASSIC_HEIGHT: u32 = 256;
 pub const THALASSIC_CELL_COUNT: u32 = THALASSIC_WIDTH * THALASSIC_HEIGHT;

lunabotics/lunabot-ai/src/autonomy/mod.rs

@@ -50,45 +50,33 @@ impl Autonomy {
 }
 
 pub fn autonomy() -> impl Behavior<LunabotBlackboard> {
-    ParallelAny::new((
-        Sequence::new((
+    WhileLoop::new(
+        |blackboard: &mut LunabotBlackboard| (*blackboard.get_autonomy() != Autonomy::None).into(),
+        ParallelAny::new((
             AssertCancelSafe(|blackboard: &mut LunabotBlackboard| {
-                blackboard.get_path_mut().clear();
-                let pos = blackboard.get_robot_isometry().translation;
-                let pos = Point3::new(pos.x, pos.y, pos.z);
-                blackboard.get_path_mut().extend_from_slice(&[
-                    pos,
-                    // Point3::new(-0.9144, 0.0, -1.905),
-                    Point3::new(1.905, 0.0, -1.5),
-                    Point3::new(1.27, 0.0, -0.5),
-                ]);
-                println!("set path", );
-                Status::Success
+                if *blackboard.lunabase_disconnected() {
+                    error!("Lunabase disconnected");
+                    return Status::Failure;
+                }
+                while let Some(msg) = blackboard.peek_from_lunabase() {
+                    match msg {
+                        FromLunabase::Steering(_) => {
+                            *blackboard.get_autonomy() = Autonomy::None;
+                            warn!("Received steering message while in autonomy mode");
+                            return Status::Success;
+                        }
+                        FromLunabase::SoftStop => {
+                            blackboard.pop_from_lunabase();
+                            return Status::Failure;
+                        }
+                        _ => blackboard.pop_from_lunabase(),
+                    };
+                }
+                Status::Running
             }),
-            ares_bt::converters::InfallibleShim(AssertCancelSafe(follow_path))
+            Sequence::new((dig(), dump(), traverse())),
         )),
-        AssertCancelSafe(|blackboard: &mut LunabotBlackboard| {
-            if *blackboard.lunabase_disconnected() {
-                error!("Lunabase disconnected");
-                return Status::Failure;
-            }
-            while let Some(msg) = blackboard.peek_from_lunabase() {
-                match msg {
-                    FromLunabase::Steering(_) => {
-                        *blackboard.get_autonomy() = Autonomy::None;
-                        warn!("Received steering message while in autonomy mode");
-                        return Status::Success;
-                    }
-                    FromLunabase::SoftStop => {
-                        blackboard.pop_from_lunabase();
-                        return Status::Failure;
-                    }
-                    _ => blackboard.pop_from_lunabase(),
-                };
-            }
-            Status::Running
-        }),
-    ))
+    )
 }
 
 fn follow_path(blackboard: &mut LunabotBlackboard) -> InfallibleStatus {
@@ -104,7 +92,6 @@ fn follow_path(blackboard: &mut LunabotBlackboard) -> InfallibleStatus {
 
     match find_target_point(pos, path) {
         Some(target) => {
-            println!("going to {} from {}", target, pos);
             let heading_angle = heading.angle(&Vector2::new(0.0, -1.0));
             let to_first_point = (target - pos).normalize();
 
@@ -117,34 +104,16 @@ fn follow_path(blackboard: &mut LunabotBlackboard) -> InfallibleStatus {
 
             *blackboard.get_poll_when() =
                 PollWhen::Instant(Instant::now() + Duration::from_millis(16));
-                // PollWhen::Instant(Instant::now());
-
-            // We reborrow path so that we can mutably access get_poll_when
-            // let path = blackboard.get_path_mut();
 
-            // when approaching an arc turn gradually
-            // if distance(&pos, &target) < ARC_THRESHOLD { // && within_arc(path, target) {
-                // let (l, r) = scaled_clamp(
-                //     -to_first_point.y + to_first_point.x,
-                //     -to_first_point.y - to_first_point.x,
-                //     1,
-                // );
-                // blackboard.enqueue_action(Action::SetSteering(Steering::new_left_right(l, r)));
-                // return InfallibleStatus::Running;
-            // }
-            println!("angle {} to next pt {}", to_first_point.angle(&Vector2::new(0.0, -1.0)), to_first_point);
             if to_first_point.angle(&Vector2::new(0.0, -1.0)).to_degrees() > 20.0 {
                 if to_first_point.x > 0.0 {
-                    println!("turning right", );
                     blackboard
                     .enqueue_action(Action::SetSteering(Steering::new_left_right(1.0, -1.0)))
                 } else {
-                    println!("turning left", );
                     blackboard
                         .enqueue_action(Action::SetSteering(Steering::new_left_right(-1.0, 1.0)))
                 }
             } else {
-                println!("straight ahead ", );
                 let (l, r) = scaled_clamp(
                     -to_first_point.y + to_first_point.x * 1.2,
                     -to_first_point.y - to_first_point.x * 1.2,
@@ -174,52 +143,15 @@ fn find_target_point(pos: Point2<f64>, path: &mut Vec<OPoint<f64, Const<3>>>) ->
         return None;
     }
 
-    // if the robot is near any point, delete all previous points
-    for (i, point) in path.iter().enumerate().rev() {
-        if distance(&point.xz(), &pos) < AT_POINT_THRESHOLD {
-            println!("path follower: made it to point {}", point);
-
-            // let res = Some(path[i+1].xz());
-            path.drain(0..=i);
+    let first_point = path[0].xz();
 
-            println!("drained path to {:?}", path);
-            return Some(path[0].xz());
-        }
+    // if the robot is near the first point, delete it
+    if distance(&first_point, &pos) < AT_POINT_THRESHOLD {
+        path.remove(0);
+        return Some(path[0].xz());
     }
-    //TODO: repeat for line segments
-
-    Some(path[0].xz())
-
-    // let mut min_dist = distance(&pos, &path[0].xz());
-    // let mut res = path[0].xz();
 
-    // for i in 1..path.len() {
-    //     let dist = dist_to_segment(pos, path[i - 1].xz(), path[i].xz());
-
-    //     if dist < min_dist {
-    //         min_dist = dist;
-    //         res = path[i].xz();
-    //     }
-    // }
-}
-
-fn dist_to_segment(point: Point2<f64>, a: Point2<f64>, b: Point2<f64>) -> f64 {
-    let mut line_from_origin = b - a; // move line segment to origin
-    let mut point = point - a; // move point the same amount
-
-    let angle = -line_from_origin.y.signum() * line_from_origin.angle(&Vector2::new(1.0, 0.0));
-
-    // rotate both until segment lines up with the x axis
-    line_from_origin = rotate_v2_ccw(line_from_origin, angle);
-    point = rotate_v2_ccw(point, angle);
-
-    return if point.x <= 0.0 {
-        point.magnitude()
-    } else if point.x >= line_from_origin.x {
-        (point - Vector2::new(line_from_origin.x, 0.0)).magnitude()
-    } else {
-        point.y.abs()
-    };
+    Some(first_point)
 }
 
 fn rotate_v2_ccw(vector2: Vector2<f64>, theta: f64) -> Vector2<f64> {
@@ -232,20 +164,6 @@ fn rotate_v2_ccw(vector2: Vector2<f64>, theta: f64) -> Vector2<f64> {
     return rot * vector2;
 }
 
-/// min distance for 2 path points to be considered part of an arc
-const ARC_THRESHOLD: f64 = 0.3;
-
-/// is this point considered part of an arc?
-fn within_arc(path: &[Point3<f64>], i: usize) -> bool {
-    if path.len() == 1 {
-        false
-    } else if i == path.len() - 1 {
-        distance(&path[i].xz(), &path[i - 1].xz()) < ARC_THRESHOLD
-    } else {
-        distance(&path[i].xz(), &path[i + 1].xz()) < ARC_THRESHOLD
-    }
-}
-
 /// clamps `a` and `b` so that `a.abs().max(b.abs) <= bound.abs()`,
 /// while maintaining the ratio between `a` and `b`
 fn scaled_clamp(a: f64, b: f64, bound: f64) -> (f64, f64) {

lunabotics/lunabot-ai/src/autonomy/traverse.rs

@@ -41,7 +41,7 @@ pub(super) fn traverse() -> impl Behavior<LunabotBlackboard> + CancelSafe {
                             AssertCancelSafe(|blackboard: &mut LunabotBlackboard| {
                                 blackboard.calculate_path(
                                     blackboard.get_robot_isometry().translation.vector.into(),
-                                    Point3::new(-3.0, 0.0, -6.0),
+                                    Point3::new(-2.0, 0.0, -5.0),
                                 );
                                 Status::Success
                             }),

lunabotics/lunabot/src/apps/sim.rs

@@ -8,8 +8,7 @@ use std::{
 };
 
 use common::{
-    lunasim::{FromLunasim, FromLunasimbot},
-    LunabotStage,
+    lunasim::{FromLunasim, FromLunasimbot}, LunabotStage, THALASSIC_HEIGHT, THALASSIC_WIDTH
 };
 use crossbeam::atomic::AtomicCell;
 use gputter::{
@@ -368,7 +367,7 @@ impl LunasimbotApp {
         let mut pathfinder = DefaultPathfinder {
             world_to_grid,
             grid_to_world,
-            grid: Grid::new(128, 256),
+            grid: Grid::new(THALASSIC_WIDTH as usize, THALASSIC_HEIGHT as usize),
         };
         pathfinder.grid.enable_diagonal_mode();
         pathfinder.grid.fill();