Fixed Astar, needs to be tested with moving robot in simulator first

Author: Cazadorro

ros_ws/src/navigation/CMakeLists.txt

@@ -2,7 +2,11 @@ cmake_minimum_required(VERSION 2.8.3)
 project(navigation)
 
 set (NAVSOURCES src/euclidiandistancehueristic.cpp
-src/PathFinderTutorials.cpp)
+src/navigation_node.cpp src/navigation.cpp src/minheap.cpp include/navigation/astar.hpp include/navigation/minheap.hpp include/navigation/updatepriorityqueue.hpp include/navigation/astar.h 
+include/navigation/euclidiandistancehueristic.h include/navigation/minheap.h include/navigation/navigation.h include/navigation/typewkey.h
+include/navigation/updatepriorityqueue.h)
+
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g")
 
 
 
@@ -11,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
   std_msgs
+  nav_msgs
 )
 
 
@@ -21,7 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES navigation
-  CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs
+  CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs nav_msgs
   DEPENDS system_lib
 )
 

ros_ws/src/navigation/include/navigation/astar.h

@@ -9,16 +9,18 @@
 #ifndef ASTAR_H_
 #define ASTAR_H_
 
+#include <nav_msgs/OccupancyGrid.h>
 #include "updatepriorityqueue.h"
 #include "typewkey.h"
+#include <vector>
 
 template<typename typekey, typename Heuristic>
-std::vector<std::pair<size_t, size_t>> Astar(occupancyGrid[],
+std::vector<std::pair<size_t, size_t>> Astar(nav_msgs::OccupancyGrid o_grid,
     TypeWkey<typekey> start, TypeWkey<typekey> goal, const Heuristic& heuristic);
 template<typename typekey>
 const std::vector<std::pair<size_t, size_t>>& reconstruct_path(
     std::vector<std::pair<size_t, size_t>>& moves,
-    const unordered_map<TypeWkey<typekey>, TypeWkey<typekey> >& parents,
+    const unordered_map<typekey, typekey >& parents,
     const TypeWkey<typekey>& current, size_t width, size_t hieght);
 
 #include "astar.hpp"

ros_ws/src/navigation/include/navigation/astar.hpp

@@ -8,15 +8,16 @@
 
 
 template<typename typekey, typename Heuristic>
-std::vector<std::pair<size_t, size_t>> Astar(occupancyGrid[],
+std::vector<std::pair<size_t, size_t>> Astar(nav_msgs::OccupancyGrid o_grid,
     TypeWkey<typekey> start, TypeWkey<typekey> goal, const Heuristic& heuristic)
 {
+  std::cout << "Starting Astar" << std::endl;
   //set of moves to be returned
   std::vector<std::pair<size_t, size_t>> moves;
   //occupancy grid width
-  size_t width = occupancyGrid.width();
+  size_t width = o_grid.info.width;
   //occupancy grid height
-  size_t height = occupancyGrid.height();
+  size_t height = o_grid.info.height;
   //scale cost of a single cardinal move
   const double scale = 1;
   //path cost for start
@@ -26,9 +27,9 @@ std::vector<std::pair<size_t, size_t>> Astar(occupancyGrid[],
   //open priority queue, with removal and updating
   UpdatePriorityQueue<TypeWkey<typekey>, size_t> openPQ;
   //closed set map, maps the key for the type and the f cost for that type
-  unordered_map<TypeWkey<typekey>, f_cost> closedSet;
+  unordered_map<typekey, f_cost> closedSet;
   //map that maps parent with given key index
-  unordered_map<TypeWkey<typekey>, TypeWkey<typekey> > parents;
+  unordered_map<typekey, typekey > parents;
   // parents.insert(std::make_pair(start, start)); //parents[start] = undefined, we set the parent as itself
   //inserts the first item
   openPQ.insert(start, start_g, start_h); //openQ.add(start, f_score[start])
@@ -39,24 +40,30 @@ std::vector<std::pair<size_t, size_t>> Astar(occupancyGrid[],
   TypeWkey<typekey> v;
   //set of adjacent moves to u
   std::vector<std::pair<TypeWkey<typekey>, g_cost> > adj_moves;
-
+  //std::cout << "HOLA" <<std::endl;
   //while the priority queue is not empty
   while (openPQ.size())
   {
+    //std::cout << "While Priority Queue is Not Empty, PriorityQueue size " << openPQ.size() << std::endl;
     //total score, g_score + h_score for u
     f_cost top_score = openPQ.getTotalVal(openPQ.top());
+   // std::cout << "we got the top score " << top_score << endl;
     //poping off u from the priority queue
     u = openPQ.pop();  // Source node in first case
+   // std::cout << "popped " << top_score << endl;
     if (u == goal)
     {
       //break and return current move list;
-      return reconstruct_path(moves, parents, u, width, height);
+      return reconstruct_path<typekey>(moves, parents, u, width, height);
     }
+    //std::cout << "not goal yet " << std::endl;
     //if it isn't the goal we move the position to the closed set
-    closedSet.insert(std::make_pair(u, top_score));
+    closedSet.insert(std::make_pair(u.getKey(), top_score));
 
+   // std::cout << "Inserting done " << std::endl;
     //clear previous adjacent moves
     adj_moves.clear();
+   // std::cout << "clearing moves " << std::endl;
     //NORTH
     typekey temp_key = u.getKey();
     //Cardinal direction keys
@@ -76,94 +83,101 @@ std::vector<std::pair<size_t, size_t>> Astar(occupancyGrid[],
     //north lookup
     if (temp_key >= width)
     {
-      adj_moves.push_back(TypeWkey<typekey>(north_key), sdist);
+      adj_moves.push_back(std::make_pair(TypeWkey<typekey>(north_key), sdist));
       //north west lookup
       if (temp_key % width > 0)
       {
-        adj_moves.push_back(TypeWkey<typekey>(north_w_key), ddist);
+        adj_moves.push_back(std::make_pair(TypeWkey<typekey>(north_w_key), ddist));
       }
       //north east lookup
       if (temp_key % width < (width - 1))
       {
-        adj_moves.push_back(TypeWkey<typekey>(north_e_key), ddist);
+        adj_moves.push_back(std::make_pair(TypeWkey<typekey>(north_e_key), ddist));
       }
     }
     //south
     if (temp_key <= (width - 1) * (height))
     {
-      adj_moves.push_back(TypeWkey<typekey>(south_key), sdist);
+      adj_moves.push_back(std::make_pair(TypeWkey<typekey>(south_key), sdist));
       //south west lookup
       if (temp_key % width > 0)
       {
-        adj_moves.push_back(TypeWkey<typekey>(south_w_key), ddist);
+        adj_moves.push_back(std::make_pair(TypeWkey<typekey>(south_w_key), ddist));
       }
       //south east lookup
       if (temp_key % width < (width - 1))
       {
-        adj_moves.push_back(TypeWkey<typekey>(south_e_key), ddist);
+        adj_moves.push_back(std::make_pair(TypeWkey<typekey>(south_e_key), ddist));
       }
     }
 
     // west lookup
     if (temp_key % width > 0)
     {
-      adj_moves.push_back(TypeWkey<typekey>(west_key), sdist);
+      adj_moves.push_back(std::make_pair(TypeWkey<typekey>(west_key), sdist));
     }
     // east lookup
     if (temp_key % width < (width - 1))
     {
-      adj_moves.push_back(TypeWkey<typekey>(east_key), sdist);
+      adj_moves.push_back(std::make_pair(TypeWkey<typekey>(east_key), sdist));
     }
 
+    //std::cout << "Done generating Adjacency Moves" << std::endl;
     //going through all possible adjacent moves
     for (size_t i = 0; i < adj_moves.size(); i++)     // where v is still in PQ.
     {
       //adjacent move
       v = adj_moves[i].first;
+      //std::cout << "Going through adjacent moves " << i << std::endl;
       //if it is inside the closed set we can ignore it. Otherwise continue
       if (closedSet.find(v.getKey()) == closedSet.end())
       {
         //if the probability of blockage in the probability grid is below 2% (1%, 0%, -1 (not explored)%)
-        if (occupancyGrid[v.getkey()] < 2)
+        if (o_grid.data[v.getKey()] < 2)
         {
           //path cost of v
           g_cost path_cost = adj_moves[i].second; // determined by edge length and weight
           //heuristic value of v
-          h_cost h_val = hueristic(v.getKey(), goal);
+          h_cost h_val = heuristic(v.getKey(), goal);
 
           //if it wasn't already in the openset
           if (!openPQ.find(v))
           {
             //we insert it
+            parents.insert(std::make_pair(v.getKey(), u.getKey()));
             openPQ.insert(v, path_cost, h_val);
           }
           //if it was already in the openset, and we found that this path is shorter
           //than the one already stored
           else if (path_cost < openPQ.getCostVal(v)) // A shorter path to v has been found
           {
             //inserting parent (where v is the child and u is the parent)
-            parents.insert(v, u);
+            parents.insert(std::make_pair(v.getKey(), u.getKey()));
             //update the value
             openPQ.update(v, path_cost, h_val);
           }
         }
       }
     }
+    //std::cout << "Done assessing adjacent " << std::endl;
   }
   return moves;
 }
 
 template<typename typekey>
 const std::vector<std::pair<size_t, size_t>>& reconstruct_path(
     std::vector<std::pair<size_t, size_t>>& moves,
-    const unordered_map<TypeWkey<typekey>, TypeWkey<typekey> >& parents,
+    const unordered_map<typekey ,typekey >& parents,
     const TypeWkey<typekey>& current, size_t width, size_t hieght)
 {
+  TypeWkey<typekey> path_current = current;
+  std::cout << "finding parents" << std::endl;
   do
   {
-    moves.push_back(current.getKey() % width, (current.getKey() / width));
-    current = parents.at(current.getKey());
-  } while (parents.find(current) != parents.end());
+
+    moves.push_back(std::make_pair(path_current.getKey() % width, (path_current.getKey() / width)));
+    path_current = parents.at(path_current.getKey());
+  } while (parents.find(path_current.getKey()) != parents.end());
   return moves;
 }
 

ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h

@@ -10,7 +10,9 @@
 #define EUCLIDIANDISTANCEHUERISTIC_H_
 
 #include "updatepriorityqueue.h"
+#include "typewkey.h"
 #include <math.h>
+#include <cstdlib>
 
 class EuclidianDistanceHueristic
 {
@@ -26,7 +28,6 @@ class EuclidianDistanceHueristic
     }
     h_cost operator ()(const TypeWkey<size_t>& start,
         const TypeWkey<size_t>& goal) const;
-    virtual ~EuclidianDistanceHueristic();
 };
 
 #endif /* EUCLIDIANDISTANCEHUERISTIC_H_ */

ros_ws/src/navigation/include/navigation/minheap.h

@@ -11,6 +11,7 @@
 
 #include <utility>
 #include <iostream>
+#include <unordered_map>
 using namespace std;
 
 template<typename T, typename comp_T>

ros_ws/src/navigation/include/navigation/minheap.hpp

@@ -100,6 +100,10 @@ size_t MinHeap<T, comp_T>::heapifyUp(size_t i, unordered_map<T, size_t>& m_id)
       //std::cout << "swapping at i  = " << i << " and j = " << j << std::endl;
       //std::cout << m_heap[i] <<" , " << m_heap[j] << std::endl;
       std::swap(m_heap[i], m_heap[j]);
+      //std::cout << "i " << std::endl;
+      //std::cout << m_id.at(m_heap[i]) << std::endl;
+      //std::cout << "j " << std::endl;
+      //std::cout << m_id.at(m_heap[j]) << std::endl;
       std::swap(m_id.at(m_heap[i]), m_id.at(m_heap[j]));
       //std::cout << m_heap[i] <<" , " << m_heap[j] << std::endl;
       i = j;
@@ -417,18 +421,3 @@ ostream& operator <<(ostream& os, const MinHeap<T, comp_T>& rhs)
   return os;
 }
 
-size_t parent(size_t i)
-{
-
-  return (i ? (i - 1) : i) / 2;
-}
-
-size_t leftChild(size_t i)
-{
-  return (i * 2) + 1;
-}
-
-size_t rightChild(size_t i)
-{
-  return (i * 2) + 2;
-}

ros_ws/src/navigation/include/navigation/navigation.h

@@ -0,0 +1,30 @@
+/**
+ *  @file navigation.h
+ *  @author shae, CS5201 Section A
+ *  @date Apr 19, 2016
+ *  @brief Description:
+ *  @details Details:
+ */
+
+#ifndef NAVIGATION_INCLUDE_NAVIGATION_NAVIGATION_H_
+#define NAVIGATION_INCLUDE_NAVIGATION_NAVIGATION_H_
+#include <ros/ros.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include "astar.h"
+#include "euclidiandistancehueristic.h"
+
+class Navigation
+{
+  private:
+    ros::NodeHandle nh;
+    ros::Subscriber omap_sub;
+    nav_msgs::OccupancyGrid m_occupancy_grid;
+
+    void oMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& occupancy_grid);
+  public:
+    Navigation();
+    void update();
+};
+
+
+#endif /* NAVIGATION_INCLUDE_NAVIGATION_NAVIGATION_H_ */

ros_ws/src/navigation/include/navigation/typewkey.h

@@ -40,10 +40,24 @@ struct TypeWkey
       return *this;
     }
 
+
     int getKey() const
     {
       return m_data;
     }
 };
 
+template <typename T>
+bool operator == (const TypeWkey<T>& lhs, const TypeWkey<T>& rhs)
+{
+  return lhs.getKey() == rhs.getKey();
+}
+
+template <typename T>
+bool operator != (const TypeWkey<T>& lhs, const TypeWkey<T>& rhs)
+{
+  return !(lhs == rhs);
+}
+
+
 #endif /* TYPEWKEY_H_ */

ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp

@@ -67,9 +67,9 @@ void UpdatePriorityQueue<T, K>::insert(const T& item, const g_cost& cost,
 
   m_h.insert(make_pair(key, h));
   //(*m_h.find(key)).second = h;
-
+  m_id.insert(make_pair(key, size() - 1));
   size_t heap_index = m_min_heap.insert(key, m_id);
-  m_id.insert(make_pair(key, heap_index));
+  m_id[key] = heap_index;
   //cout << "H" << hueristic << endl;
   //cout << "m_g" << m_g[key] << endl;
   //cout << "m_g" << (*m_g.find(key)).second << endl;
@@ -193,7 +193,7 @@ template<typename T, typename K>
 bool UpdatePriorityQueue<T, K>::find(const T& item) const
 {
   K key = item.getKey();
-  std::unordered_map<K, size_t>::const_iterator got = m_id.find(key);
+  typename std::unordered_map<K, size_t>::const_iterator got = m_id.find(key);
 
   return (got != m_id.end()) ? true : false;
 }
@@ -239,20 +239,20 @@ bool UpdatePriorityQueue<T, K>::valid_heap()
       K key_i = m_min_heap[i];
       K key_j = m_min_heap[j];
 
-      std::cout << m_min_heap.useCompareObject(j, i) << std::endl;
-      std::cout << "comparing i position " << i << " with j position " << j << std::endl;
-      std::cout << "comparing i key " << m_min_heap[i] << " with j key"
-          << m_min_heap[j] << std::endl;
-      std::cout << "comparing h[i] " << m_h.at(key_i) << " with h[j] " << m_h.at(key_j)
-          << std::endl;
-      std::cout << "comparing g[i] " << m_g.at(key_i) << " with g[j] " << m_g.at(key_j)
-          << std::endl;
-      std::cout << "comparing f[i] " << m_g.at(key_i) + m_h.at(key_i) << " with f[j] "
-          << m_g.at(key_j) + m_h.at(key_j) << std::endl;
+      //std::cout << m_min_heap.useCompareObject(j, i) << std::endl;
+      //std::cout << "comparing i position " << i << " with j position " << j << std::endl;
+      //std::cout << "comparing i key " << m_min_heap[i] << " with j key"
+       //   << m_min_heap[j] << std::endl;
+      //std::cout << "comparing h[i] " << m_h.at(key_i) << " with h[j] " << m_h.at(key_j)
+       //   << std::endl;
+      //std::cout << "comparing g[i] " << m_g.at(key_i) << " with g[j] " << m_g.at(key_j)
+       //   << std::endl;
+      //std::cout << "comparing f[i] " << m_g.at(key_i) + m_h.at(key_i) << " with f[j] "
+        //  << m_g.at(key_j) + m_h.at(key_j) << std::endl;
       for (size_t k = 0; k < size(); k++)
       {
         K key_k = m_min_heap[k];
-        std::cout << m_min_heap[k] << " : " << m_g.at(key_k) + m_h.at(key_k) << std::endl;
+        //std::cout << m_min_heap[k] << " : " << m_g.at(key_k) + m_h.at(key_k) << std::endl;
       }
       return false;
     }