mooophy / path-planning Goto Github PK

View Code? Open in Web Editor NEW

90.0 11.0 36.0 4.6 MB

Implementation for A* with stricted visited list, Lifelong Planning A* and D* Lite final version

License: MIT License

C++ 93.46% C 6.09% Makefile 0.44%

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

Stargazers

Watchers

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*

Refactor ideas for LpAstar

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

Is coordinate in LpState needed?

The position of the 2d vectors holds this information already.

GetAsyncKeyState may need debouncing.

Potential problem in AStarSel::Result

In lines, the type Result is using reference to pass result.Such design may cause potential security problem when trying to keep its object.

filter out the invalid coordinates in neighbors.

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.

Hush function needed for Node

Found such requirement in 9cad5dd .

Change git remote to use https for git tool in vs2015

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.

Are succ and pred all just children?

Currently, I'm implementing succ and pred as a single children as that in AStar.

change (y, x) back to (x, y) for State

Bug : something wrong with priority queue

Add a auxiliary data structure to track old keys

Perhaps :

HashTable< Tuple<Cell, key> > old_keys;

Maintain :

Insert new tuple into this hash table when pushing any Cell into q.
Delete tuple when popping.

To add path building.

Pointer to LpState or Coordinate are what should be held by q, rather than LpState

Found this when implementing compute_shortest_path().

Since currently LpState is the type held by q, I use it's member ,that is LpState::Coordinate, as the abstract pointer for addressing LpState in matrix.

This may increase a little bit more time to run.