From 554ad6473c7fd396160d72181e8533172d075461 Mon Sep 17 00:00:00 2001 From: troy Date: Thu, 30 Jan 2025 14:14:28 -0700 Subject: [PATCH] clean up path follower, edit pathfinder --- godot/lunasim/path_manager.gd | 2 +- lunabotics/common/src/lib.rs | 1 + lunabotics/lunabot-ai/src/autonomy/mod.rs | 142 ++++-------------- .../lunabot-ai/src/autonomy/traverse.rs | 2 +- lunabotics/lunabot/src/apps/sim.rs | 5 +- lunabotics/lunabot/src/pathfinding.rs | 117 +++++++-------- lunabotics/lunabot/src/pipelines/thalassic.rs | 66 +++++--- 7 files changed, 135 insertions(+), 200 deletions(-) diff --git a/godot/lunasim/path_manager.gd b/godot/lunasim/path_manager.gd index 8cb4f9b6..18358d60 100644 --- a/godot/lunasim/path_manager.gd +++ b/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 diff --git a/lunabotics/common/src/lib.rs b/lunabotics/common/src/lib.rs index 125eb5f2..47fafaa3 100644 --- a/lunabotics/common/src/lib.rs +++ b/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; diff --git a/lunabotics/lunabot-ai/src/autonomy/mod.rs b/lunabotics/lunabot-ai/src/autonomy/mod.rs index b6685121..0c232019 100644 --- a/lunabotics/lunabot-ai/src/autonomy/mod.rs +++ b/lunabotics/lunabot-ai/src/autonomy/mod.rs @@ -50,45 +50,33 @@ impl Autonomy { } pub fn autonomy() -> impl Behavior { - 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, path: &mut Vec>>) -> 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, a: Point2, b: Point2) -> 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, theta: f64) -> Vector2 { @@ -232,20 +164,6 @@ fn rotate_v2_ccw(vector2: Vector2, theta: f64) -> Vector2 { 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], 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) { diff --git a/lunabotics/lunabot-ai/src/autonomy/traverse.rs b/lunabotics/lunabot-ai/src/autonomy/traverse.rs index 1bc6caea..6182f74f 100644 --- a/lunabotics/lunabot-ai/src/autonomy/traverse.rs +++ b/lunabotics/lunabot-ai/src/autonomy/traverse.rs @@ -41,7 +41,7 @@ pub(super) fn traverse() -> impl Behavior + 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 }), diff --git a/lunabotics/lunabot/src/apps/sim.rs b/lunabotics/lunabot/src/apps/sim.rs index 86c42de3..775161e7 100644 --- a/lunabotics/lunabot/src/apps/sim.rs +++ b/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(); diff --git a/lunabotics/lunabot/src/pathfinding.rs b/lunabotics/lunabot/src/pathfinding.rs index 7012364b..4a190817 100644 --- a/lunabotics/lunabot/src/pathfinding.rs +++ b/lunabotics/lunabot/src/pathfinding.rs @@ -1,14 +1,16 @@ +use common::THALASSIC_CELL_SIZE; use nalgebra::{Point3, Transform3}; use pathfinding::{grid::Grid, prelude::astar}; use tasker::shared::SharedDataReceiver; use tracing::{error, warn}; use crate::utils::distance_between_tuples; -use crate::pipelines::thalassic::{set_observe_depth, ThalassicData}; +use crate::pipelines::thalassic::{set_observe_depth, ThalassicData, CellState}; const REACH: usize = 10; pub struct DefaultPathfinder { + pub world_to_grid: Transform3, pub grid_to_world: Transform3, pub grid: Grid, @@ -27,16 +29,31 @@ impl DefaultPathfinder { shared_thalassic_data.try_get(); set_observe_depth(true); let mut map_data = shared_thalassic_data.get(); + + let RADIUS = 0.5; //TODO: original value was 0.5 + loop { - if map_data.current_robot_radius == 0.5 { + if map_data.current_robot_radius == RADIUS { break; } - map_data.set_robot_radius(0.5); + map_data.set_robot_radius(RADIUS); drop(map_data); map_data = shared_thalassic_data.get(); } set_observe_depth(false); + into.clear(); + + let mut append_path = |path: Vec<(usize, usize)>| { + into.extend(path.into_iter().map(|(x, y)| { + let mut p = Point3::new(x as f64, 0.0, y as f64); + p = self.grid_to_world * p; + p.y = map_data.get_height((x, y)) as f64; + p + })); + }; + + // allows checking if position is known inside `move || {}` closures without moving `map_data` let is_known = |pos: (usize, usize)| { map_data.is_known(pos) @@ -49,25 +66,24 @@ impl DefaultPathfinder { let (x, y) = potential_neighbor; - // neighbors are: within reach AND known AND unoccupied - x.abs_diff($p.0) <= REACH && y.abs_diff($p.1) <= REACH && - map_data.is_known(potential_neighbor) && - !map_data.is_occupied(potential_neighbor) + // neighbors are: within reach AND not red + x.abs_diff($p.0) <= REACH && y.abs_diff($p.1) <= REACH && + map_data.get_cell_state(potential_neighbor) != CellState::RED }) .into_iter() .map(move |neighbor| { // unknown cells have 2x the cost let unknown_multiplier = match is_known(neighbor) { - true => 1, - false => 2, + true => 1.0, + false => 2.0, }; ( neighbor, // the cost of moving from a to b is the distance between a to b - (distance_between_tuples($p, neighbor) * 10000.0) as usize * unknown_multiplier + (distance_between_tuples($p, neighbor) * unknown_multiplier * 10000.0) as usize ) }) }; @@ -75,18 +91,22 @@ impl DefaultPathfinder { let from_grid = self.world_to_grid * from; let to_grid = self.world_to_grid * to; - let mut start = (from_grid.x as usize, from_grid.z as usize); + + let robot_cell_pos = (from_grid.x as usize, from_grid.z as usize); + + let mut start = robot_cell_pos; let end = (to_grid.x as usize, to_grid.z as usize); + let heuristic = move |p: &(usize, usize)| { (distance_between_tuples(*p, end) * 10000.0) as usize }; - into.clear(); + // if in red, prepend a path to safety { - if !map_data.is_known(start) || map_data.is_occupied(start) { - warn!("Current cell is occupied, finding closest safe cell"); + if map_data.get_cell_state(start) == CellState::RED { + println!("Current cell is occupied, finding closest safe cell"); if let Some((path, _)) = astar( &start, |&p| { @@ -105,17 +125,10 @@ impl DefaultPathfinder { }) }, |_| 0, - |&pos| { - map_data.get_height(pos) != 0.0 && !map_data.is_occupied(pos) - }, + |&pos| map_data.get_cell_state(pos) == CellState::GREEN, ) { start = *path.last().unwrap(); - into.extend(path.into_iter().map(|(x, y)| { - let mut p = Point3::new(x as f64, 0.0, y as f64); - p = self.grid_to_world * p; - p.y = map_data.get_height((x, y)) as f64; - p - })); + append_path(path); } else { error!("Failed to find path to safety"); return; @@ -123,49 +136,27 @@ impl DefaultPathfinder { } } - let mut closest = start; - let mut closest_distance = self.grid.distance(closest, end); - let mut using_closest = false; - - let mut path = astar( - &start, - |&p| { - let d = self.grid.distance(p, end); - if d < closest_distance { - closest = p; - closest_distance = d; - } - neighbours!(p) - }, - heuristic, - |p| p == &end, - ) - .unwrap_or_else(|| { - warn!("Failed to find path, using closest..."); - using_closest = true; - astar(&start, |&p| neighbours!(p), heuristic, |p| p == &closest).unwrap() - }) - .0; + let mut path = astar(&start, |&p| neighbours!(p), heuristic, |p| p == &end) + .expect("there should always be a possible path to the goal") + .0; + + println!("path colors ===", ); + for pt in &path { + println!("color at {:?}: {:?}", pt, map_data.get_cell_state(*pt)); + } // truncate path so that it ends before entering explored region - for (index, pt) in path.iter().enumerate() { + // for (index, pt) in path.iter().enumerate() { - if !map_data.is_safe_for_robot(*pt) { - warn!("truncated path"); - path.truncate(index-1); - break; - } - } + // if let Err(unknown_cell) = map_data.is_safe_for_robot(robot_cell_pos, *pt) { + // println!("truncate: keeping only up to before {}, unknown cell: {:?}", index, unknown_cell); + // path.truncate(index); + // break; + // } + // } // add final path to `into` - into.extend(path.into_iter().map(|(x, y)| { - let mut p = Point3::new(x as f64, 0.0, y as f64); - p = self.grid_to_world * p; - p.y = map_data.get_height((x, y)) as f64; - p - })); - if using_closest { - into.push(to); - } + append_path(path); + } -} +} \ No newline at end of file diff --git a/lunabotics/lunabot/src/pipelines/thalassic.rs b/lunabotics/lunabot/src/pipelines/thalassic.rs index 480a9cb3..3e5d06a9 100644 --- a/lunabotics/lunabot/src/pipelines/thalassic.rs +++ b/lunabotics/lunabot/src/pipelines/thalassic.rs @@ -7,7 +7,7 @@ use std::{ }; use arc_swap::ArcSwapOption; -use common::{THALASSIC_CELL_COUNT, THALASSIC_WIDTH, THALASSIC_HEIGHT}; +use common::{THALASSIC_CELL_COUNT, THALASSIC_CELL_SIZE, THALASSIC_HEIGHT, THALASSIC_WIDTH}; use crossbeam::{ atomic::AtomicCell, sync::{Parker, Unparker}, @@ -43,6 +43,9 @@ pub struct ThalassicData { new_robot_radius: AtomicCell>, } +#[derive(PartialEq, Debug)] +pub enum CellState { RED, GREEN, UNKNOWN } + impl Default for ThalassicData { fn default() -> Self { Self { @@ -77,43 +80,65 @@ impl ThalassicData { } - /// whether this position is safe for the robot to be - /// - this position must be green - /// - every single cell within `robot radius` of this position must be known - pub fn is_safe_for_robot(&self, pos: (usize, usize)) -> bool { - - if self.is_occupied(pos) { - return false; + /// whether a target position is safe for the robot to be + /// - the target position must be green + /// - every single cell within `robot radius` of the target must be known + /// + /// if unsafe, returns `Err(one of the unknown cells that makes the target unsafe)` + pub fn is_safe_for_robot(&self, robot_cell_pos: (usize, usize), target_cell_pos: (usize, usize)) -> Result<(), (usize, usize)> { + + if robot_cell_pos == target_cell_pos { + return Ok(()); + } + + let robot_cell_radius = (self.current_robot_radius / THALASSIC_CELL_SIZE).ceil(); + + // if the target cell is near the robot, its okay if its not green + if + self.get_cell_state(target_cell_pos) != CellState::GREEN && + distance_between_tuples(target_cell_pos, robot_cell_pos) > robot_cell_radius + { + return Err(target_cell_pos); } - let radius = self.current_robot_radius.ceil() as i32; - let (pos_x, pos_y) = (pos.0 as i32, pos.1 as i32); + let (pos_x, pos_y) = (target_cell_pos.0 as i32, target_cell_pos.1 as i32); - for x in (pos_x - radius)..(pos_x + radius) { - for y in (pos_y - radius)..(pos_y + radius) { + for x in (pos_x - robot_cell_radius as i32)..(pos_x + robot_cell_radius as i32) { + for y in (pos_y - robot_cell_radius as i32)..(pos_y + robot_cell_radius as i32) { if !self.in_bounds((x, y)) { continue; } let nearby_cell = (x as usize, y as usize); + // if a cell is near the target AND not inside the robot AND and unknown then target cell is dangerous if - distance_between_tuples(nearby_cell, pos) <= self.current_robot_radius && + distance_between_tuples(nearby_cell, target_cell_pos) <= robot_cell_radius && + distance_between_tuples(nearby_cell, robot_cell_pos) > robot_cell_radius && !self.is_known(nearby_cell) { - return false; + return Err(nearby_cell); } } } - true + Ok(()) } - pub fn is_known(&self, pos: (usize, usize)) -> bool { - self.get_height(pos) != 0.0 + pub fn get_cell_state(&self, pos: (usize, usize)) -> CellState { + + match self.expanded_obstacle_map[Self::xy_to_index(pos)].occupied() { + true => CellState::RED, + false => { + match self.get_height(pos) == 0.0 { + true => CellState::UNKNOWN, + false => CellState::GREEN, + } + }, + } } - pub fn is_occupied(&self, pos: (usize, usize)) -> bool { - self.expanded_obstacle_map[Self::xy_to_index(pos)].occupied() + pub fn is_known(&self, pos: (usize, usize)) -> bool { + self.get_cell_state(pos) != CellState::UNKNOWN } pub fn get_height(&self, pos: (usize, usize)) -> f32 { @@ -145,6 +170,7 @@ impl PointsStorageChannel { } } + pub fn spawn_thalassic_pipeline( buffer: OwnedData, point_cloud_channels: Box<[Arc]>, @@ -170,7 +196,7 @@ pub fn spawn_thalassic_pipeline( NonZeroU32::new(128).unwrap(), NonZeroU32::new(256).unwrap(), ), - cell_size: 0.03125, + cell_size: THALASSIC_CELL_SIZE, max_point_count: NonZeroU32::new(max_point_count).unwrap(), max_triangle_count: NonZeroU32::new(max_triangle_count).unwrap(), }