Code Monkey home page Code Monkey logo

path-planning's Introduction

Path-Planning

This repo provides implementation for three Path-Planning Algorithms: A* with strict visited list, Lifelong Planning A* and D* Lite final version A* with strict visited list

Data structure:

q                          priority queue
max_q_size                 unsigned int
expansions                 hash table
final_path                 string
run_time                   long long
is_found                   bool

Pseudocode:

q.push start
while q is not empty and q.top != goal
  curr = q.pop
  if curr has not been visited
    expansions.insert curr.state
    foreach neighbour of curr
      if neighbour has not been visited
        if q does not contain neighbour
          q.push neighbour
        else
          update q with neighbour
if q.empty
  final_path = ""
else
  final_path = q.top.path

Lifelong Planning A*

Data structure:

matrix                     2D dynamic array
start, goal                tuple
hfunc                      function object
q                          priority queue
max_q_size                 unsigned int
expansions                 hash table
path                       string
run_time                   long long

Pseudocode:

initialize
  q.reset
  at(start).r = 0
  q.push start

update_vertex(s)
  if s.cell != start
    minimum = huge
    foreach neighbour of s.cell
      minimum = min(minimum, (at(neighbour).g + cost()))
    s.r = minimum
  q.remove s.cell
  if s.g != s.r 
    q.push s.cell
  
update_neighbours_of(cell)
  foreach neighbour of cell
    if !at(neighbour).bad
      update_vertex(at(neighbour));
      
compute_shortest_path
  while (!q.empty  and key(at(q.top)) < key(at(goal)) or at(goal).r != at(goal).g
    c = q.pop
    if at(c).g > at(c).r
      at(c).g = at(c).r
    else
      at(c).g = huge 
      update_vertex at c
    update_neighbours_of c
    max_q_size = max(max_q_size, q.size())
    expansions.insert c
  path = build_path
  
plan
  initialize
  compute_shortest_path
  
replan(cells_to_toggle)
  foreach cell of cells_to_toggle
    at(cell).bad = !at(cell).bad
    if !at(cell).bad
      update_vertex at cell
    else
      at(cell).g = at(cell).r = huge
      update_neighbours_of cell
  compute_shortest_path

D* Lite final version

Data structure:

matrix                     2D dynamic array
start, goal                tuple
hfunc                      function object
km                         int
q                          priority queue
old_keys                   hash table
max_q_size                 unsigned int
expansions                 hash table
path                       string
run_time                   long long

Pseudocode:

initialize
  q.reset
  km = at(goal).r = 0
  q.push goal
  old_keys.insert key(goal, { at(goal), km } )
  
update_vertex(s)
  if s.cell != goal
    minimum = huge
    foreach neighbour of s.cell
      minimum = min(minimum, (at(neighbour).g + cost()))
      s.r = minimum
  q.remove s.cell
  if s.g != s.)
    q.push s.cell 
    old_keys.update_with tuple({ s.cell, Key{ s, km })
    
update_neighbours_of(cell)
  foreach neighbour of cell
    if !at(neighbour).bad
      update_vertex(at(neighbour))
      
compute_shortest_path
  while !q.empty() && ((key(at(q.top)) < key(at(start)) || at(start).r != at(start).g))
    c = q.pop
    if old_keys.at(c) < key(at(c), km)
      q.push c
      old_keys.update_with tuple( c, Key{ at(c), km })
    else if at(c).g > at(c).r
      at(c).g = at(c).r
      update_neighbours_of c
    else
      at(c).g = huge
      update_vertex at c
      update_neighbours_of c
    max_q_size = max(max_q_size, q.size)
    expansions.insert c
    
initial_plan
  initialize
  compute_shortest_path
  
plan(changes, move_to, use_path)
  initial_plan
  last = start
  curr = start
  i = 0
  while curr != goal
    curr = min(neighbours of curr)
    move_to curr
    if i != changes.length
      km += hfunc(this_loop.last, this_loop.curr)
      last = curr
      foreach cell of changes[i]
        at(cell).bad = !at(cell).bad
        if !at(cell).bad
          update_vertex at cell
        else
          at(cell).g = at(cell).r = huge
        update_neighbours_of cell
      ++i
      compute_shortest_path
    use_path build_path(this_loop.curr, goal)

path-planning's People

Contributors

mooophy avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

path-planning's Issues

Test LpState with PriorityQueue

  • Make sure the ProrityQueue<LpState> works as expected.
  • Check if the ProrityQueue<LpState> has covered all features needed in LPA*

Add unorded_set<Coordinate> as Blockeds.

To pass blocked coordinates from frond end to algorithm module.

Precondition and plan:

  • Members to_string and to_hash for Coordinate. -- done.
  • std::hash<Coordinate> -- done.
  • Member Contains(Coordinate) -> bool in this unorded_set<Coordinate>. -- use std::unordered_set::count instead.
  • Unit tests needed.

Result of DSatr re-planning

{[r=0,c=0]|g:10000|r:5|h:1|b:f} {[r=0,c=1]|g:10000|r:5|h:1|b:f}         {[r=0,c=2]|g:10000|r:10000|h:2|b:f}+++
{[r=1,c=0]|g:4|r:4|h:0|b:f}     {[r=1,c=1]|g:10000|r:10000|h:1|b:t}     {[r=1,c=2]|g:10000|r:3|h:2|b:f}+++
{[r=2,c=0]|g:3|r:3|h:1|b:f}     {[r=2,c=1]|g:10000|r:10000|h:1|b:t}     {[r=2,c=2]|g:2|r:2|h:2|b:f}+++
{[r=3,c=0]|g:2|r:2|h:2|b:f}     {[r=3,c=1]|g:10000|r:10000|h:2|b:t}     {[r=3,c=2]|g:1|r:1|h:2|b:f}+++
{[r=4,c=0]|g:10000|r:2|h:3|b:f} {[r=4,c=1]|g:1|r:1|h:3|b:f}             {[r=4,c=2]|g:0|r:0|h:3|b:f}+++
  • Note:g and r are the same as the given example. But h values are different, because in example h values have been recalculated for each moving. It seems unnecessary. Currently, not implemented like this. Just leave an Issue here for memo.

Refactor ideas for LpAstar

  • Use at() -> LpState& to replace matrix.at() -> LpState&
  • plan() and replan() instead of run and rerun
  • infinity() to huge()

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    ๐Ÿ–– Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. ๐Ÿ“Š๐Ÿ“ˆ๐ŸŽ‰

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google โค๏ธ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.