403 lines
12 KiB
Rust
403 lines
12 KiB
Rust
//! Motion planning: A\* grid search and potential-field velocity commands.
|
|
//!
|
|
//! Operates on the [`OccupancyGrid`](crate::bridge::OccupancyGrid) type from
|
|
//! the bridge module. Two planners are provided:
|
|
//!
|
|
//! - [`astar`]: discrete A\* on the occupancy grid returning a cell path.
|
|
//! - [`potential_field`]: continuous-space repulsive/attractive field producing
|
|
//! a velocity command.
|
|
|
|
use crate::bridge::OccupancyGrid;
|
|
|
|
use serde::{Deserialize, Serialize};
|
|
use std::cmp::Ordering;
|
|
use std::collections::{BinaryHeap, HashMap, HashSet};
|
|
|
|
/// A 2-D grid cell coordinate.
|
|
pub type Cell = (usize, usize);
|
|
|
|
/// Result of an A\* search.
|
|
#[derive(Debug, Clone, Serialize, Deserialize)]
|
|
pub struct GridPath {
|
|
/// Sequence of `(x, y)` cells from start to goal (inclusive).
|
|
pub cells: Vec<Cell>,
|
|
/// Total traversal cost.
|
|
pub cost: f64,
|
|
}
|
|
|
|
/// Errors from planning operations.
|
|
#[derive(Debug, thiserror::Error)]
|
|
pub enum PlanningError {
|
|
#[error("start cell ({0}, {1}) is out of bounds or occupied")]
|
|
InvalidStart(usize, usize),
|
|
#[error("goal cell ({0}, {1}) is out of bounds or occupied")]
|
|
InvalidGoal(usize, usize),
|
|
#[error("no feasible path found")]
|
|
NoPath,
|
|
}
|
|
|
|
pub type Result<T> = std::result::Result<T, PlanningError>;
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// A* search
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/// Occupancy value above which a cell is considered blocked.
|
|
const OCCUPIED_THRESHOLD: f32 = 0.5;
|
|
|
|
#[derive(PartialEq)]
|
|
struct AStarEntry {
|
|
cell: Cell,
|
|
f: f64,
|
|
}
|
|
impl Eq for AStarEntry {}
|
|
impl Ord for AStarEntry {
|
|
fn cmp(&self, other: &Self) -> Ordering {
|
|
other.f.partial_cmp(&self.f).unwrap_or(Ordering::Equal)
|
|
}
|
|
}
|
|
impl PartialOrd for AStarEntry {
|
|
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
|
|
Some(self.cmp(other))
|
|
}
|
|
}
|
|
|
|
/// Run A\* on `grid`, returning the shortest [`GridPath`] from `start` to
|
|
/// `goal`. Cells with occupancy >= 0.5 are treated as impassable.
|
|
///
|
|
/// Diagonal moves cost √2, cardinal moves cost 1.
|
|
pub fn astar(
|
|
grid: &OccupancyGrid,
|
|
start: Cell,
|
|
goal: Cell,
|
|
) -> Result<GridPath> {
|
|
if !cell_free(grid, start) {
|
|
return Err(PlanningError::InvalidStart(start.0, start.1));
|
|
}
|
|
if !cell_free(grid, goal) {
|
|
return Err(PlanningError::InvalidGoal(goal.0, goal.1));
|
|
}
|
|
if start == goal {
|
|
return Ok(GridPath { cells: vec![start], cost: 0.0 });
|
|
}
|
|
|
|
let mut g_score: HashMap<Cell, f64> = HashMap::with_capacity(128);
|
|
let mut came_from: HashMap<Cell, Cell> = HashMap::with_capacity(128);
|
|
let mut open = BinaryHeap::new();
|
|
let mut closed: HashSet<Cell> = HashSet::with_capacity(128);
|
|
let mut neighbor_buf: Vec<(usize, usize, f64)> = Vec::with_capacity(8);
|
|
|
|
g_score.insert(start, 0.0);
|
|
open.push(AStarEntry { cell: start, f: heuristic(start, goal) });
|
|
|
|
while let Some(AStarEntry { cell, .. }) = open.pop() {
|
|
if cell == goal {
|
|
return Ok(reconstruct_path(&came_from, goal, &g_score));
|
|
}
|
|
|
|
// Skip already-expanded nodes (avoids re-expansion from stale heap entries).
|
|
if !closed.insert(cell) {
|
|
continue;
|
|
}
|
|
|
|
let current_g = g_score[&cell];
|
|
|
|
neighbors_into(grid, cell, &mut neighbor_buf);
|
|
for &(nx, ny, step_cost) in &neighbor_buf {
|
|
let neighbor = (nx, ny);
|
|
if closed.contains(&neighbor) {
|
|
continue;
|
|
}
|
|
let tentative_g = current_g + step_cost;
|
|
if tentative_g < *g_score.get(&neighbor).unwrap_or(&f64::INFINITY) {
|
|
g_score.insert(neighbor, tentative_g);
|
|
came_from.insert(neighbor, cell);
|
|
open.push(AStarEntry {
|
|
cell: neighbor,
|
|
f: tentative_g + heuristic(neighbor, goal),
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
Err(PlanningError::NoPath)
|
|
}
|
|
|
|
#[inline]
|
|
fn cell_free(grid: &OccupancyGrid, (x, y): Cell) -> bool {
|
|
grid.get(x, y).is_some_and(|v| v < OCCUPIED_THRESHOLD)
|
|
}
|
|
|
|
#[inline]
|
|
fn heuristic(a: Cell, b: Cell) -> f64 {
|
|
let dx = (a.0 as f64 - b.0 as f64).abs();
|
|
let dy = (a.1 as f64 - b.1 as f64).abs();
|
|
// Octile distance.
|
|
let (min, max) = if dx < dy { (dx, dy) } else { (dy, dx) };
|
|
min * std::f64::consts::SQRT_2 + (max - min)
|
|
}
|
|
|
|
/// Write neighbours of `cell` into `out`, reusing the buffer to avoid
|
|
/// per-expansion heap allocation.
|
|
#[inline]
|
|
fn neighbors_into(grid: &OccupancyGrid, (cx, cy): Cell, out: &mut Vec<(usize, usize, f64)>) {
|
|
out.clear();
|
|
for dx in [-1_i64, 0, 1] {
|
|
for dy in [-1_i64, 0, 1] {
|
|
if dx == 0 && dy == 0 {
|
|
continue;
|
|
}
|
|
let nx = cx as i64 + dx;
|
|
let ny = cy as i64 + dy;
|
|
if nx < 0 || ny < 0 {
|
|
continue;
|
|
}
|
|
let (nx, ny) = (nx as usize, ny as usize);
|
|
if cell_free(grid, (nx, ny)) {
|
|
let cost = if dx != 0 && dy != 0 {
|
|
std::f64::consts::SQRT_2
|
|
} else {
|
|
1.0
|
|
};
|
|
out.push((nx, ny, cost));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn reconstruct_path(
|
|
came_from: &HashMap<Cell, Cell>,
|
|
goal: Cell,
|
|
g_score: &HashMap<Cell, f64>,
|
|
) -> GridPath {
|
|
let mut cells = vec![goal];
|
|
let mut current = goal;
|
|
while let Some(&prev) = came_from.get(¤t) {
|
|
cells.push(prev);
|
|
current = prev;
|
|
}
|
|
cells.reverse();
|
|
let cost = g_score.get(&goal).copied().unwrap_or(0.0);
|
|
GridPath { cells, cost }
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Potential field
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/// Output of the potential field planner: a 3-D velocity command.
|
|
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
|
|
pub struct VelocityCommand {
|
|
pub vx: f64,
|
|
pub vy: f64,
|
|
pub vz: f64,
|
|
}
|
|
|
|
/// Configuration for the potential field planner.
|
|
#[derive(Debug, Clone, Serialize, Deserialize)]
|
|
pub struct PotentialFieldConfig {
|
|
/// Attractive gain toward the goal.
|
|
pub attractive_gain: f64,
|
|
/// Repulsive gain away from obstacles.
|
|
pub repulsive_gain: f64,
|
|
/// Influence range for obstacles (metres).
|
|
pub obstacle_influence: f64,
|
|
/// Maximum output speed (m/s).
|
|
pub max_speed: f64,
|
|
}
|
|
|
|
impl Default for PotentialFieldConfig {
|
|
fn default() -> Self {
|
|
Self {
|
|
attractive_gain: 1.0,
|
|
repulsive_gain: 100.0,
|
|
obstacle_influence: 3.0,
|
|
max_speed: 2.0,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Compute a velocity command using attractive + repulsive potential fields.
|
|
///
|
|
/// * `robot` — current robot position `[x, y, z]`.
|
|
/// * `goal` — target position `[x, y, z]`.
|
|
/// * `obstacles` — positions of nearby obstacles.
|
|
pub fn potential_field(
|
|
robot: &[f64; 3],
|
|
goal: &[f64; 3],
|
|
obstacles: &[[f64; 3]],
|
|
config: &PotentialFieldConfig,
|
|
) -> VelocityCommand {
|
|
// Attractive force: linear pull toward goal.
|
|
let mut fx = config.attractive_gain * (goal[0] - robot[0]);
|
|
let mut fy = config.attractive_gain * (goal[1] - robot[1]);
|
|
let mut fz = config.attractive_gain * (goal[2] - robot[2]);
|
|
|
|
// Repulsive force: push away from each obstacle within influence range.
|
|
for obs in obstacles {
|
|
let dx = robot[0] - obs[0];
|
|
let dy = robot[1] - obs[1];
|
|
let dz = robot[2] - obs[2];
|
|
let dist = (dx * dx + dy * dy + dz * dz).sqrt().max(0.01);
|
|
|
|
if dist < config.obstacle_influence {
|
|
let strength =
|
|
config.repulsive_gain * (1.0 / dist - 1.0 / config.obstacle_influence) / (dist * dist);
|
|
fx += strength * dx / dist;
|
|
fy += strength * dy / dist;
|
|
fz += strength * dz / dist;
|
|
}
|
|
}
|
|
|
|
// Clamp to max speed.
|
|
let speed = (fx * fx + fy * fy + fz * fz).sqrt();
|
|
if speed > config.max_speed {
|
|
let s = config.max_speed / speed;
|
|
fx *= s;
|
|
fy *= s;
|
|
fz *= s;
|
|
}
|
|
|
|
VelocityCommand { vx: fx, vy: fy, vz: fz }
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Path smoothing
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/// Convert a [`GridPath`] (grid cells) to world-space waypoints using the
|
|
/// grid resolution and origin.
|
|
pub fn path_to_waypoints(path: &GridPath, resolution: f64, origin: &[f64; 3]) -> Vec<[f64; 3]> {
|
|
path.cells
|
|
.iter()
|
|
.map(|&(x, y)| {
|
|
[
|
|
origin[0] + x as f64 * resolution,
|
|
origin[1] + y as f64 * resolution,
|
|
origin[2],
|
|
]
|
|
})
|
|
.collect()
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
fn free_grid(w: usize, h: usize) -> OccupancyGrid {
|
|
OccupancyGrid::new(w, h, 1.0)
|
|
}
|
|
|
|
#[test]
|
|
fn test_astar_straight_line() {
|
|
let grid = free_grid(10, 10);
|
|
let path = astar(&grid, (0, 0), (5, 0)).unwrap();
|
|
assert_eq!(*path.cells.first().unwrap(), (0, 0));
|
|
assert_eq!(*path.cells.last().unwrap(), (5, 0));
|
|
assert!((path.cost - 5.0).abs() < 1e-6);
|
|
}
|
|
|
|
#[test]
|
|
fn test_astar_diagonal() {
|
|
let grid = free_grid(10, 10);
|
|
let path = astar(&grid, (0, 0), (3, 3)).unwrap();
|
|
assert_eq!(*path.cells.last().unwrap(), (3, 3));
|
|
// Pure diagonal = 3 * sqrt(2) ≈ 4.24
|
|
assert!((path.cost - 3.0 * std::f64::consts::SQRT_2).abs() < 1e-6);
|
|
}
|
|
|
|
#[test]
|
|
fn test_astar_same_cell() {
|
|
let grid = free_grid(5, 5);
|
|
let path = astar(&grid, (2, 2), (2, 2)).unwrap();
|
|
assert_eq!(path.cells.len(), 1);
|
|
assert!((path.cost).abs() < 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn test_astar_around_wall() {
|
|
let mut grid = free_grid(10, 10);
|
|
// Vertical wall at x=3 from y=0 to y=4.
|
|
for y in 0..5 {
|
|
grid.set(3, y, 1.0);
|
|
}
|
|
let path = astar(&grid, (1, 2), (5, 2)).unwrap();
|
|
assert_eq!(*path.cells.last().unwrap(), (5, 2));
|
|
// Path must go around the wall, so cost > 4 (straight line).
|
|
assert!(path.cost > 4.0);
|
|
}
|
|
|
|
#[test]
|
|
fn test_astar_blocked() {
|
|
let mut grid = free_grid(5, 5);
|
|
// Full wall across the grid.
|
|
for y in 0..5 {
|
|
grid.set(2, y, 1.0);
|
|
}
|
|
let result = astar(&grid, (0, 2), (4, 2));
|
|
assert!(result.is_err());
|
|
}
|
|
|
|
#[test]
|
|
fn test_astar_invalid_start() {
|
|
let grid = free_grid(5, 5);
|
|
let result = astar(&grid, (10, 10), (2, 2));
|
|
assert!(result.is_err());
|
|
}
|
|
|
|
#[test]
|
|
fn test_potential_field_towards_goal() {
|
|
let cmd = potential_field(
|
|
&[0.0, 0.0, 0.0],
|
|
&[5.0, 0.0, 0.0],
|
|
&[],
|
|
&PotentialFieldConfig::default(),
|
|
);
|
|
assert!(cmd.vx > 0.0);
|
|
assert!(cmd.vy.abs() < 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn test_potential_field_obstacle_repulsion() {
|
|
let cmd = potential_field(
|
|
&[0.0, 0.0, 0.0],
|
|
&[5.0, 0.0, 0.0],
|
|
&[[1.0, 0.0, 0.0]],
|
|
&PotentialFieldConfig::default(),
|
|
);
|
|
// Obstacle directly ahead — repulsion should reduce forward velocity.
|
|
let cmd_no_obs = potential_field(
|
|
&[0.0, 0.0, 0.0],
|
|
&[5.0, 0.0, 0.0],
|
|
&[],
|
|
&PotentialFieldConfig::default(),
|
|
);
|
|
assert!(cmd.vx < cmd_no_obs.vx);
|
|
}
|
|
|
|
#[test]
|
|
fn test_potential_field_max_speed() {
|
|
let config = PotentialFieldConfig { max_speed: 1.0, ..Default::default() };
|
|
let cmd = potential_field(
|
|
&[0.0, 0.0, 0.0],
|
|
&[100.0, 100.0, 0.0],
|
|
&[],
|
|
&config,
|
|
);
|
|
let speed = (cmd.vx * cmd.vx + cmd.vy * cmd.vy + cmd.vz * cmd.vz).sqrt();
|
|
assert!((speed - 1.0).abs() < 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn test_path_to_waypoints() {
|
|
let path = GridPath {
|
|
cells: vec![(0, 0), (1, 0), (2, 0)],
|
|
cost: 2.0,
|
|
};
|
|
let wps = path_to_waypoints(&path, 0.5, &[0.0, 0.0, 0.0]);
|
|
assert_eq!(wps.len(), 3);
|
|
assert!((wps[1][0] - 0.5).abs() < 1e-9);
|
|
assert!((wps[2][0] - 1.0).abs() < 1e-9);
|
|
}
|
|
}
|