From e78269354840826430b74cb8132148e61e1af94e Mon Sep 17 00:00:00 2001 From: Cazadorro Date: Tue, 19 Apr 2016 20:46:07 -0500 Subject: [PATCH 1/2] create navigation package --- ros_ws/src/navigation/CMakeLists.txt | 47 ++ .../src/navigation/include/navigation/astar.h | 25 + .../navigation/include/navigation/astar.hpp | 170 +++++++ .../navigation/euclidiandistancehueristic.h | 32 ++ .../navigation/include/navigation/minheap.h | 113 +++++ .../navigation/include/navigation/minheap.hpp | 434 ++++++++++++++++++ .../navigation/include/navigation/typewkey.h | 49 ++ .../include/navigation/updatepriorityqueue.h | 173 +++++++ .../navigation/updatepriorityqueue.hpp | 279 +++++++++++ ros_ws/src/navigation/package.xml | 58 +++ .../navigation/src/PathFinderTutorials.cpp | 165 +++++++ .../src/euclidiandistancehueristic.cpp | 28 ++ 12 files changed, 1573 insertions(+) create mode 100644 ros_ws/src/navigation/CMakeLists.txt create mode 100644 ros_ws/src/navigation/include/navigation/astar.h create mode 100644 ros_ws/src/navigation/include/navigation/astar.hpp create mode 100644 ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h create mode 100644 ros_ws/src/navigation/include/navigation/minheap.h create mode 100644 ros_ws/src/navigation/include/navigation/minheap.hpp create mode 100644 ros_ws/src/navigation/include/navigation/typewkey.h create mode 100644 ros_ws/src/navigation/include/navigation/updatepriorityqueue.h create mode 100644 ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp create mode 100644 ros_ws/src/navigation/package.xml create mode 100644 ros_ws/src/navigation/src/PathFinderTutorials.cpp create mode 100644 ros_ws/src/navigation/src/euclidiandistancehueristic.cpp diff --git a/ros_ws/src/navigation/CMakeLists.txt b/ros_ws/src/navigation/CMakeLists.txt new file mode 100644 index 0000000..bbb072c --- /dev/null +++ b/ros_ws/src/navigation/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.8.3) +project(navigation) + +set (NAVSOURCES src/euclidiandistancehueristic.cpp +src/PathFinderTutorials.cpp) + + + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + sensor_msgs + std_msgs +) + + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS include + LIBRARIES navigation + CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs + DEPENDS system_lib +) + +########### +## Build ## +########### + + +include_directories( + ${catkin_INCLUDE_DIRS} +include/navigation/ +) + + + add_executable(navigation_node ${NAVSOURCES}) + + + add_dependencies(navigation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(navigation_node + ${catkin_LIBRARIES} + ) + diff --git a/ros_ws/src/navigation/include/navigation/astar.h b/ros_ws/src/navigation/include/navigation/astar.h new file mode 100644 index 0000000..4466a54 --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/astar.h @@ -0,0 +1,25 @@ +/** + * @file astar.h + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + +#ifndef ASTAR_H_ +#define ASTAR_H_ + +#include "updatepriorityqueue.h" +#include "typewkey.h" + +template +std::vector> Astar(occupancyGrid[], + TypeWkey start, TypeWkey goal, const Heuristic& heuristic); +template +const std::vector>& reconstruct_path( + std::vector>& moves, + const unordered_map, TypeWkey >& parents, + const TypeWkey& current, size_t width, size_t hieght); + +#include "astar.hpp" +#endif /* ASTAR_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/astar.hpp b/ros_ws/src/navigation/include/navigation/astar.hpp new file mode 100644 index 0000000..1c5b2e1 --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/astar.hpp @@ -0,0 +1,170 @@ +/** + * @file astar.hpp + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + + +template +std::vector> Astar(occupancyGrid[], + TypeWkey start, TypeWkey goal, const Heuristic& heuristic) +{ + //set of moves to be returned + std::vector> moves; + //occupancy grid width + size_t width = occupancyGrid.width(); + //occupancy grid height + size_t height = occupancyGrid.height(); + //scale cost of a single cardinal move + const double scale = 1; + //path cost for start + g_cost start_g = 0; + //heuristic cost for start + h_cost start_h = heuristic(start, goal); + //open priority queue, with removal and updating + UpdatePriorityQueue, size_t> openPQ; + //closed set map, maps the key for the type and the f cost for that type + unordered_map, f_cost> closedSet; + //map that maps parent with given key index + unordered_map, TypeWkey > 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]) + + //u = current node + TypeWkey u; + //v = next node + TypeWkey v; + //set of adjacent moves to u + std::vector, g_cost> > adj_moves; + + //while the priority queue is not empty + while (openPQ.size()) + { + //total score, g_score + h_score for u + f_cost top_score = openPQ.getTotalVal(openPQ.top()); + //poping off u from the priority queue + u = openPQ.pop(); // Source node in first case + if (u == goal) + { + //break and return current move list; + return reconstruct_path(moves, parents, u, width, height); + } + //if it isn't the goal we move the position to the closed set + closedSet.insert(std::make_pair(u, top_score)); + + //clear previous adjacent moves + adj_moves.clear(); + //NORTH + typekey temp_key = u.getKey(); + //Cardinal direction keys + typekey north_key = temp_key - width; + typekey south_key = temp_key + width; + typekey east_key = temp_key + 1; + typekey west_key = temp_key - 1; + //Diagonal direction keys + typekey north_w_key = north_key - 1; + typekey north_e_key = north_key + 1; + typekey south_w_key = south_key - 1; + typekey south_e_key = south_key + 1; + //diagonal distance + g_cost ddist = openPQ.getCostVal(u) + (scale * sqrt(2)); + //straight distance + g_cost sdist = openPQ.getCostVal(u) + scale; + //north lookup + if (temp_key >= width) + { + adj_moves.push_back(TypeWkey(north_key), sdist); + //north west lookup + if (temp_key % width > 0) + { + adj_moves.push_back(TypeWkey(north_w_key), ddist); + } + //north east lookup + if (temp_key % width < (width - 1)) + { + adj_moves.push_back(TypeWkey(north_e_key), ddist); + } + } + //south + if (temp_key <= (width - 1) * (height)) + { + adj_moves.push_back(TypeWkey(south_key), sdist); + //south west lookup + if (temp_key % width > 0) + { + adj_moves.push_back(TypeWkey(south_w_key), ddist); + } + //south east lookup + if (temp_key % width < (width - 1)) + { + adj_moves.push_back(TypeWkey(south_e_key), ddist); + } + } + + // west lookup + if (temp_key % width > 0) + { + adj_moves.push_back(TypeWkey(west_key), sdist); + } + // east lookup + if (temp_key % width < (width - 1)) + { + adj_moves.push_back(TypeWkey(east_key), sdist); + } + + //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; + //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) + { + //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); + + //if it wasn't already in the openset + if (!openPQ.find(v)) + { + //we insert it + 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); + //update the value + openPQ.update(v, path_cost, h_val); + } + } + } + } + } + return moves; +} + +template +const std::vector>& reconstruct_path( + std::vector>& moves, + const unordered_map, TypeWkey >& parents, + const TypeWkey& current, size_t width, size_t hieght) +{ + do + { + moves.push_back(current.getKey() % width, (current.getKey() / width)); + current = parents.at(current.getKey()); + } while (parents.find(current) != parents.end()); + return moves; +} + + diff --git a/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h b/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h new file mode 100644 index 0000000..7de015d --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h @@ -0,0 +1,32 @@ +/** + * @file euclidiandistancehueristic.h + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + +#ifndef EUCLIDIANDISTANCEHUERISTIC_H_ +#define EUCLIDIANDISTANCEHUERISTIC_H_ + +#include "updatepriorityqueue.h" +#include + +class EuclidianDistanceHueristic +{ + private: + size_t m_width; + size_t m_height; + size_t m_scale; + public: + EuclidianDistanceHueristic(const size_t width, const size_t height, + const double scale) : + m_width(width), m_height(height), m_scale(scale) + { + } + h_cost operator ()(const TypeWkey& start, + const TypeWkey& goal) const; + virtual ~EuclidianDistanceHueristic(); +}; + +#endif /* EUCLIDIANDISTANCEHUERISTIC_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/minheap.h b/ros_ws/src/navigation/include/navigation/minheap.h new file mode 100644 index 0000000..c6a42f1 --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/minheap.h @@ -0,0 +1,113 @@ +/** + * @file minheap.h + * @author shae, CS5201 Section A + * @date Feb 17, 2016 + * @brief Description: + * @details Details: + */ + +#ifndef MINHEAP_H_ +#define MINHEAP_H_ + +#include +#include +using namespace std; + +template +class MinHeap; + +template +ostream& operator <<(ostream& os, const MinHeap& rhs); + +template +void swap(MinHeap& first, MinHeap& second); + +template +class MinHeap +{ + public: + MinHeap(); + MinHeap(comp_T& compare); + MinHeap(comp_T&& compare); + MinHeap(MinHeap& heap); + MinHeap(MinHeap && heap); + const MinHeap& operator =(MinHeap heap); + + friend void swap(MinHeap& first, MinHeap& second); + + friend ostream& operator <<(ostream& os, const MinHeap& rhs); + + ~MinHeap(); + + + size_t heapifyUp(size_t i); + size_t heapifyDown(size_t i); + size_t insert(T item); + void remove(size_t i); + T pop(); + + size_t heapifyUp(size_t i, unordered_map& m_id); + size_t heapifyDown(size_t i, unordered_map& m_id); + size_t insert(T item, unordered_map& m_id); + void remove(size_t i, unordered_map& m_id); + T pop(unordered_map& m_id); + + T top() const; + size_t size()const; + comp_T getCompareObject(); + bool useCompareObject(const size_t lhs, const size_t rhs); + bool valid_heap()const; + const T& operator [](const size_t index)const; + + private: + T * m_heap; + size_t m_size; + size_t m_end; + size_t m_length; + comp_T m_compare; + + void resizeLength(size_t length); + +}; + +size_t parent(size_t i); +size_t leftChild(size_t i); +size_t rightChild(size_t i); + +template +class default_compare +{ + /** + * bool to reverse the comparison + */ + bool reverse; + + public: + /** + * paramaterized constructor + * @param goal = m_goal, + * @param revparam = reverse + */ + default_compare(const bool revparam = false) + { + reverse = revparam; + } + /** + * () operator to return the comparison of the hueristic of lhs and rhs + * @param lhs left hand side ColorGraphState + * @param rhs right hand side ColorGraphState + * @return returns true if the hueristic of lhs is > hueristic of rhs if not reversed + */ + bool operator()(const T& lhs, const T& rhs) const + { + if (reverse) + return (lhs > rhs); + else + return (lhs < rhs); + } +}; + +#include "minheap.hpp" +/* namespace std */ + +#endif /* MINHEAP_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/minheap.hpp b/ros_ws/src/navigation/include/navigation/minheap.hpp new file mode 100644 index 0000000..ea43e59 --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/minheap.hpp @@ -0,0 +1,434 @@ +/** + * @file minheap.hpp + * @author shae, CS5201 Section A + * @date Feb 17, 2016 + * @brief Description: + * @details Details: + */ + +template +MinHeap::MinHeap() +{ + m_size = 0; + m_end = 0; + m_length = 0; + m_heap = nullptr; + m_compare = comp_T(); +} + +template +MinHeap::MinHeap(comp_T& compare) +{ + m_size = 0; + m_end = 0; + m_length = 0; + m_heap = nullptr; + m_compare = compare; +} + +template +MinHeap::MinHeap(comp_T&& compare) +{ + m_size = 0; + m_end = 0; + m_length = 0; + m_heap = nullptr; + m_compare = compare; +} + +template +MinHeap::MinHeap(MinHeap& heap) +{ + m_size = heap.m_size; + m_end = heap.m_end; + m_length = heap.m_length; + std::copy(m_heap, m_heap + m_size, heap.m_heap); + m_compare = heap.m_compare; +} + +template +MinHeap::MinHeap(MinHeap && heap) : + MinHeap() +{ + swap(*this, heap); +} + +template +const MinHeap& MinHeap::operator =( + MinHeap heap) +{ + swap(*this, heap); + return *this; +} + +template +MinHeap::~MinHeap() +{ + delete[] m_heap; + m_heap = nullptr; +} + +template +size_t MinHeap::heapifyUp(size_t i) +{ + size_t j = 0; + while (i > 0) + { + j = parent(i); + if (m_compare(m_heap[i], m_heap[j])) + { + std::swap(m_heap[i], m_heap[j]); + i = j; + } + else + { + return i; + } + } + return i; +} + +template +size_t MinHeap::heapifyUp(size_t i, unordered_map& m_id) +{ + size_t j = 0; + while (i > 0) + { + j = parent(i); + if (m_compare(m_heap[i], m_heap[j])) + { + //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::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; + } + else + { + return i; + } + } + return i; +} + +template +size_t MinHeap::heapifyDown(size_t i, unordered_map& m_id) +{ + size_t j = 0; + + while (((2 * i) + 1) <= (m_end)) + { + + if (((2 * i) + 1) < (m_end)) + { + size_t left = leftChild(i); + size_t right = rightChild(i); + j = m_compare(m_heap[left], m_heap[right]) ? left : right; + } + else + { + j = (2 * i) + 1; + } + if (m_compare(m_heap[j], m_heap[i])) + { + //std::cout << "swapping at i = " << i << " and j = " << j << std::endl; + //std::cout << m_heap[i] <<" , " << m_heap[j] << std::endl; + //cout << "hello " << ((2 * i) + 1) << endl; + std::swap(m_heap[i], m_heap[j]); + std::swap(m_id.at(m_heap[i]), m_id.at(m_heap[j])); + //std::cout << m_heap[i] <<" , " << m_heap[j] << std::endl; + //std::cout << std::endl; + i = j; + } + else + { + return i; + } + } + return i; +} + +template +size_t MinHeap::heapifyDown(size_t i) +{ + size_t j = 0; + + while (((2 * i) + 1) <= (m_end)) + { + + if (((2 * i) + 1) < (m_end)) + { + size_t left = leftChild(i); + size_t right = rightChild(i); + j = m_compare(m_heap[left], m_heap[right]) ? left : right; + } + else + { + j = (2 * i) + 1; + } + if (m_compare(m_heap[j], m_heap[i])) + { + std::swap(m_heap[i], m_heap[j]); + i = j; + } + else + { + return i; + } + } + return i; +} + +template +size_t MinHeap::insert(T item) +{ + if ((m_size + 1) >= m_length) + { + resizeLength((m_size + 1) * 2); + } + m_end = m_size; + m_size += 1; + m_heap[m_end] = item; + + return heapifyUp(m_end); +} + +template +size_t MinHeap::insert(T item, unordered_map& m_id) +{ + if ((m_size + 1) >= m_length) + { + resizeLength((m_size + 1) * 2); + } + m_end = m_size; + m_size += 1; + m_heap[m_end] = item; + + return heapifyUp(m_end, m_id); +} + +template +void MinHeap::remove(size_t i, unordered_map& m_id) +{ + try + { + if (!(i < m_size)) + { + throw i; + } + std::swap(m_heap[i], m_heap[m_end]); + std::swap(m_id.at(m_heap[i]), m_id.at(m_heap[m_end])); + if (m_size > 0) + { + m_size -= 1; + m_end -= m_end ? 1 : 0; + } + i = heapifyUp(i, m_id); + heapifyDown(i, m_id); + + if (((m_size) <= m_length / 2)) + { + resizeLength(m_length / 2); + } + } + catch (size_t i) + { + cout << "error, i out of bounds, i = " << i << endl; + } +} + + + +template +void MinHeap::remove(size_t i) +{ + try + { + if (!(i < m_size)) + { + throw i; + } + std::swap(m_heap[i], m_heap[m_end]); + + if (m_size > 0) + { + m_size -= 1; + m_end -= m_end ? 1 : 0; + } + i = heapifyUp(i); + heapifyDown(i); + + if (((m_size) <= m_length / 2)) + { + resizeLength(m_length / 2); + } + } + catch (size_t i) + { + cout << "error, i out of bounds, i = " << i << endl; + } +} + +template +T MinHeap::pop() +{ + T val; + if (m_size > 0) + { + val = m_heap[0]; + remove(0); + } + else + { + cout << "NO TOP " << m_size << endl; + } + return val; +} + +template +T MinHeap::pop(unordered_map& m_id) +{ + T val; + if (m_size > 0) + { + val = m_heap[0]; + remove(0, m_id); + } + else + { + cout << "NO TOP " << m_size << endl; + } + return val; +} + +template +T MinHeap::top() const +{ + T val; + if (m_size > 0) + { + val = m_heap[0]; + } + else + { + cout << "NO TOP " << m_size << endl; + } + return val; +} + +template +size_t MinHeap::size() const +{ + return m_size; +} + +template +comp_T MinHeap::getCompareObject() +{ + return m_compare; +} + +template +void MinHeap::resizeLength(size_t length) +{ + T *temp = new T[length]; + for (size_t i = 0; i < m_size; i++) + { + temp[i] = m_heap[i]; + } + delete[] m_heap; + m_heap = temp; + m_length = length; +} + +template +bool MinHeap::valid_heap() const +{ + size_t j; + for (size_t i = 0; i < m_size - 1; i++) + { + + if (((2 * i) + 1) < (m_end)) + { + size_t left = leftChild(i); + size_t right = rightChild(i); + j = m_compare(m_heap[left], m_heap[right]) ? left : right; + } + else + { + j = ((2 * i) + 1 <= m_end) ? ((2 * i) + 1) : i; + } + if (m_compare(m_heap[j], m_heap[i])) + { + std::cout << m_compare(m_heap[j], m_heap[i]) << std::endl; + std::cout << "comparing i " << i << " with j " << j << std::endl; + std::cout << "comparing i " << m_heap[i] << " with j " << m_heap[j] + << std::endl; + for (size_t k = 0; k < m_size; k++) + { + std::cout << m_heap[k] << std::endl; + } + return false; + } + } + return true; +} + +template +bool MinHeap::useCompareObject(const size_t lhs, const size_t rhs) +{ + return m_compare(m_heap[lhs], m_heap[rhs]); +} + +template +const T& MinHeap::operator [](const size_t index) const +{ + try + { + if (!(index < size())) + { + throw index; + } + } + catch (size_t i) + { + cout << "error, i out of bounds, i = " << i << endl; + } + return m_heap[index]; +} + +template +void swap(MinHeap& first, MinHeap& second) +{ + std::swap(first.m_size, second.m_size); + std::swap(first.m_length, second.m_length); + std::swap(first.m_heap, second.m_heap); + std::swap(first.m_compare, second.m_compare); +} + +template +ostream& operator <<(ostream& os, const MinHeap& rhs) +{ + for (size_t i = 0; i < rhs.m_size; i++) + { + os << rhs.m_heap[i] << ", "; + } + 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; +} diff --git a/ros_ws/src/navigation/include/navigation/typewkey.h b/ros_ws/src/navigation/include/navigation/typewkey.h new file mode 100644 index 0000000..8bab93a --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/typewkey.h @@ -0,0 +1,49 @@ +/** + * @file typeWkey.h + * @author shae, CS5201 Section A + * @date Apr 15, 2016 + * @brief Description: + * @details Details: + */ + +#ifndef TYPEWKEY_H_ +#define TYPEWKEY_H_ + +template +struct TypeWkey +{ + public: + T m_data; + TypeWkey() + { + + } + TypeWkey(const TypeWkey& typewkey) + { + *this = typewkey; + } + + TypeWkey(const T& t) + { + m_data = t; + } + + const TypeWkey& operator =(const T& data) + { + m_data = data; + return *this; + } + + const TypeWkey& operator =( const TypeWkey& typewkey) + { + m_data = typewkey.m_data; + return *this; + } + + int getKey() const + { + return m_data; + } +}; + +#endif /* TYPEWKEY_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/updatepriorityqueue.h b/ros_ws/src/navigation/include/navigation/updatepriorityqueue.h new file mode 100644 index 0000000..e7ca201 --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/updatepriorityqueue.h @@ -0,0 +1,173 @@ +/** + * @file updatepriorityqueue.h + * @author shae, CS5201 Section A + * @date Feb 18, 2016 + * @brief Description: + * @details Details: + */ + +#ifndef UPDATEPRIORITYQUEUE_H_ +#define UPDATEPRIORITYQUEUE_H_ + +#include +#include "minheap.h" + +typedef double h_cost; +typedef double g_cost; +typedef double f_cost; + +template +class UpdatePriorityQueue; + +template +void swap(UpdatePriorityQueue& first, UpdatePriorityQueue& second); + +template +ostream& operator <<(ostream& os, const UpdatePriorityQueue& rhs); + + +//T = type, K = type of key of T +template +class UpdatePriorityQueue +{ + public: + + class compare; + + UpdatePriorityQueue(); + UpdatePriorityQueue(const UpdatePriorityQueue& priority_queue); + UpdatePriorityQueue(UpdatePriorityQueue && priority_queue); + const UpdatePriorityQueue& operator =( + const UpdatePriorityQueue priority_queue); + //~UpdatePriorityQueue(); + + void insert(const T& item, const g_cost& cost, const h_cost& hueristic); + bool updateHueristic(const T& item, const h_cost& hueristic); + bool updateCost(const T& item, const g_cost& cost); + const h_cost getHueristicVal(const T& item) const; + const g_cost getCostVal(const T& item) const; + const f_cost getTotalVal(const T& item) const; + bool update(const T& item, const g_cost& cost, const h_cost& hueristic); + bool remove(const T& item); + bool find(const T& item) const; + T pop(); + T top() const; + size_t size(); + bool valid_heap(); + + friend ostream& operator <<(ostream& os, + const UpdatePriorityQueue& rhs); + friend void swap(UpdatePriorityQueue& first, + UpdatePriorityQueue& second); + + private: + unordered_map m_g; + unordered_map m_h; + unordered_map m_id; + compare m_compare; + MinHeap::compare> m_min_heap; + +}; + +template +class UpdatePriorityQueue::compare +{ + /** + * bool to reverse the comparison + */ + bool reverse; + /** + * hueristic to be used in comparison + */ + public: + /** + * paramaterized constructor + * @param goal = m_goal, + * @param revparam = reverse + */ + + unordered_map* m_g; + unordered_map* m_h; + compare() + { + reverse = false; + m_g = nullptr; + m_h = nullptr; + } + + compare(UpdatePriorityQueue::compare& comp) + { + *this = comp; + } + + compare(UpdatePriorityQueue::compare&& comp) + { + *this = comp; + } + compare(unordered_map& g, unordered_map& h, + const bool revparam = false) + { + reverse = revparam; + m_g = &g; + m_h = &h; + } + const UpdatePriorityQueue::compare& operator =( + UpdatePriorityQueue::compare& comp) + { + reverse = comp.reverse; + m_g = comp.m_g; + m_h = comp.m_h; + return *this; + } + const UpdatePriorityQueue::compare& operator =( + UpdatePriorityQueue::compare&& comp) + { + reverse = comp.reverse; + m_g = comp.m_g; + m_h = comp.m_h; + comp.m_g = nullptr; + comp.m_h = nullptr; + return *this; + } + + ~compare() + { + //delete m_g; + //delete m_h; + m_g = nullptr; + m_h = nullptr; + } + /** + * () operator to return the comparison of the hueristic of lhs and rhs + * @param lhs left hand side ColorGraphState + * @param rhs right hand side ColorGraphState + * @return returns true if the hueristic of lhs is > hueristic of rhs if not reversed + */ + bool operator()(T& lhs, T& rhs) const + { + + size_t lhs_key = lhs.getKey(); + size_t rhs_key = rhs.getKey(); + size_t lhs_cost = (*m_g).at(lhs_key) + (*m_h).at(lhs_key); + size_t rhs_cost = (*m_g).at(rhs_key) + (*m_h).at(rhs_key); + if (reverse) + return (lhs_cost > rhs_cost); + else + return (lhs_cost < rhs_cost); + } + bool operator()(K& lhs_key, K& rhs_key) const + { + size_t lhs_cost = (*m_g).at(lhs_key) + (*m_h).at(lhs_key); + size_t rhs_cost = (*m_g).at(rhs_key) + (*m_h).at(rhs_key); + //std::cout << "lhs_cost of key " << lhs_key << " is " << lhs_cost; + //std::cout << "\n rhs_cost of key " << rhs_key << " is " << rhs_cost << std::endl; + if (reverse) + return (lhs_cost > rhs_cost); + else + return (lhs_cost < rhs_cost); + } +}; + +#include "updatepriorityqueue.hpp" + +#endif /* UPDATEPRIORITYQUEUE_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp b/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp new file mode 100644 index 0000000..a9a63e5 --- /dev/null +++ b/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp @@ -0,0 +1,279 @@ +/** + * @file updatepriorityqueue.hpp + * @author shae, CS5201 Section A + * @date Feb 18, 2016 + * @brief Description: + * @details Details: + */ + +template +UpdatePriorityQueue::UpdatePriorityQueue() +{ + m_g.clear(); + m_h.clear(); + m_id.clear(); + m_compare = UpdatePriorityQueue::compare(m_g, m_h, false); + m_min_heap = MinHeap::compare>(m_compare); +} + +template +UpdatePriorityQueue::UpdatePriorityQueue( + const UpdatePriorityQueue& priority_queue) +{ + m_g = priority_queue.m_g; + m_h = priority_queue.m_h; + m_id = priority_queue.m_id; + m_compare = priority_queue.m_compare; + m_min_heap = priority_queue.m_min_heap; +} + +template +UpdatePriorityQueue::UpdatePriorityQueue( + UpdatePriorityQueue && priority_queue) : + UpdatePriorityQueue() +{ + swap(*this, priority_queue); +} + +template +const UpdatePriorityQueue& UpdatePriorityQueue::operator =( + const UpdatePriorityQueue priority_queue) +{ + swap(*this, priority_queue); + return *this; +} + +//~UpdatePriorityQueue(); + +template +void UpdatePriorityQueue::insert(const T& item, const g_cost& cost, + const h_cost& hueristic) +{ + K key = item.getKey(); + //unordered_map map; + //map.insert(make_pair(key, cost)); + //cout << "map" << map[key] << endl; + + g_cost g = cost; + h_cost h = hueristic; + //cout << g << endl; + //cout << h << endl; + + m_g.insert(make_pair(key, g)); + //cout << "m_g" << m_g[key] << endl; + //(*m_g.find(key)).second = g; + //cout << "asdf " << make_pair(key, cost).first << "asdf " + // << make_pair(key, cost).second << endl; + + m_h.insert(make_pair(key, h)); + //(*m_h.find(key)).second = h; + + size_t heap_index = m_min_heap.insert(key, m_id); + m_id.insert(make_pair(key, heap_index)); + //cout << "H" << hueristic << endl; + //cout << "m_g" << m_g[key] << endl; + //cout << "m_g" << (*m_g.find(key)).second << endl; + //cout << "m_h" << m_h[key] << endl; + //cout << "m_id" << m_id[key] << endl; +} + +template +bool UpdatePriorityQueue::updateHueristic(const T& item, + const h_cost& hueristic) +{ + K key = item.getKey(); + try + { + m_h.at(key) = hueristic; + } + catch (const std::out_of_range& oor) + { + std::cerr << "Out of Range error: " << oor.what() << '\n'; + return false; + } + size_t& index_value = m_id.at(key); + index_value = m_min_heap.heapifyUp(index_value, m_id); + index_value = m_min_heap.heapifyDown(index_value, m_id); + m_id.at(key) = index_value; + return true; +} + +template +bool UpdatePriorityQueue::updateCost(const T& item, const g_cost& cost) +{ + K key = item.getKey(); + try + { + m_g.at(key) = cost; + } + catch (const std::out_of_range& oor) + { + std::cerr << "Out of Range error: " << oor.what() << '\n'; + return false; + } + size_t& index_value = m_id.at(key); + index_value = m_min_heap.heapifyUp(index_value, m_id); + index_value = m_min_heap.heapifyDown(index_value, m_id); + //m_id.at(key) = index_value; + return true; +} +template +const h_cost UpdatePriorityQueue::getHueristicVal(const T& item) const +{ + K key = item.getKey(); + return m_h.at(key); +} +template +const g_cost UpdatePriorityQueue::getCostVal(const T& item) const +{ + K key = item.getKey(); + return m_g.at(key); +} + +template +const f_cost UpdatePriorityQueue::getTotalVal(const T& item) const +{ + K key = item.getKey(); + return m_g.at(key) + m_h.at(key); +} + +template +bool UpdatePriorityQueue::update(const T& item, const g_cost& cost, + const h_cost& hueristic) + +{ + K key = item.getKey(); + //std::cout << "KEY " << key << std::endl; + try + { + m_g.at(key) = cost; + m_h.at(key) = hueristic; + } + catch (const std::out_of_range& oor) + { + std::cerr << "Out of Range error: " << oor.what() << '\n'; + return false; + } + //std::cout << "mid " << std::endl; + //std::cout << "index " << m_id.at(key) << std::endl; + size_t& index_value = m_id.at(key); + //std::cout << "fval "<< m_g.at(key) + m_h.at(key) << std::endl; + //std::cout << "index val "<< index_value << std::endl; + index_value = m_min_heap.heapifyUp(index_value, m_id); + //std::cout << "after up position " << m_id.at(key) << std::endl; + index_value = m_min_heap.heapifyDown(index_value, m_id); + //std::cout << "after down position " << m_id.at(key) << std::endl; + //std::cout << "endmid " << std::endl; + //m_id.at(key) = index_value; + return true; +} + +template +bool UpdatePriorityQueue::remove(const T& item) +{ + K key = item.getKey(); + size_t index_value; + try + { + index_value = m_id.at(key); + } + catch (const std::out_of_range& oor) + { + //std::cerr << "Out of Range error: " << oor.what() << '\n'; + return false; + } + m_min_heap.remove(index_value, m_id); + m_h.erase(key); + m_g.erase(key); + m_id.erase(key); + return true; +} + +template +bool UpdatePriorityQueue::find(const T& item) const +{ + K key = item.getKey(); + std::unordered_map::const_iterator got = m_id.find(key); + + return (got != m_id.end()) ? true : false; +} + +template +T UpdatePriorityQueue::pop() +{ + return m_min_heap.pop(m_id); +} + +template +T UpdatePriorityQueue::top() const +{ + return m_min_heap.top(); +} + +template +size_t UpdatePriorityQueue::size() +{ + return m_min_heap.size(); +} +template +bool UpdatePriorityQueue::valid_heap() +{ + size_t m_end = size() - 1; + size_t j; + compare t_compare = m_min_heap.getCompareObject(); + for (size_t i = 0; i < size(); i++) + { + + if (((2 * i) + 1) < (m_end)) + { + size_t left = leftChild(i); + size_t right = rightChild(i); + j = m_min_heap.useCompareObject(left, right) ? left : right; + } + else + { + j = ((2 * i) + 1 <= m_end) ? ((2 * i) + 1) : i; + } + if (m_min_heap.useCompareObject(j, i)) + { + 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; + 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; + } + return false; + } + } + return true; + return m_min_heap.valid_heap(); +} + +template +ostream& operator <<(ostream& os, const UpdatePriorityQueue& rhs) +{ + os << rhs.m_min_heap; + return os; +} + +template +void swap(UpdatePriorityQueue& first, UpdatePriorityQueue& second) +{ + swap(first.m_g, second.m_g); + swap(first.m_h, second.m_h); + swap(first.m_id, second.m_id); + swap(first.m_compare, second.m_compare); + swap(first.m_min_heap, second.m_min_heap); +} diff --git a/ros_ws/src/navigation/package.xml b/ros_ws/src/navigation/package.xml new file mode 100644 index 0000000..b828401 --- /dev/null +++ b/ros_ws/src/navigation/package.xml @@ -0,0 +1,58 @@ + + + navigation + 0.0.0 + The navigation package + + + + + shae + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + roscpp + sensor_msgs + std_msgs + geometry_msgs + roscpp + sensor_msgs + std_msgs + + + + + + + + \ No newline at end of file diff --git a/ros_ws/src/navigation/src/PathFinderTutorials.cpp b/ros_ws/src/navigation/src/PathFinderTutorials.cpp new file mode 100644 index 0000000..50b9e32 --- /dev/null +++ b/ros_ws/src/navigation/src/PathFinderTutorials.cpp @@ -0,0 +1,165 @@ +//============================================================================ +// Name : PathFinderTutorials.cpp +// Author : +// Version : +// Copyright : Your copyright notice +// Description : Hello World in C++, Ansi-style +//============================================================================ + +#include +#include "updatepriorityqueue.h" +#include /* srand, rand */ +#include /* time */ +#include "typewkey.h" +#include +#include "euclidiandistancehueristic.h" +#include "astar.h" + +#define CONST 10 +#define DONST CONST +using namespace std; + +struct intWkey +{ + public: + int m_data; + intWkey(int data = 0) + { + m_data = data; + } + + intWkey(intWkey& intwkey) + { + *this = intwkey; + } + + intWkey(const intWkey& intwkey) + { + *this = intwkey; + } + + const intWkey& operator =(int data) + { + m_data = data; + return *this; + } + + const intWkey& operator =(intWkey& intwkey) + { + m_data = intwkey.m_data; + return *this; + } + const intWkey& operator =(const intWkey& intwkey) + { + m_data = intwkey.m_data; + return *this; + } + + int getKey() const + { + return m_data; + } +}; + +int main() +{ + srand(time(NULL)); + default_compare comp(false); + /* + MinHeap > heap(comp); + for (int i = 0; i < CONST; i++) + { + //heap.insert((rand()%200) - 100); + heap.insert(i - (DONST / 2)); + } + cout << "!!!Hello World!!!" << endl; // prints !!!Hello World!!! + cout << heap << endl; + + for (int i = 0; i < CONST; i++) + { + cout << heap.pop() << endl; + cout << heap << endl; + } + + UpdatePriorityQueue pq; + intWkey keyint; + for (size_t i = 0; i < CONST; i++) + { + //pq.insert((rand()%200) - 100); + keyint = (i); //- (DONST / 2)); + pq.insert(keyint, i, i); + cout << pq << endl; + } + + size_t temps; + keyint = 0; + temps = 20; //rand() % 10; + cout << "temps" << temps << endl; + pq.updateCost(keyint, temps); + cout << pq << endl; + cout << "GOODG" << endl; + for (size_t i = 0; i < 10; i++) + { + keyint = (i); + temps = rand() % 10; + cout << "temps" << temps << endl; + pq.updateCost(keyint, temps); + cout << pq << endl; + } + for (size_t i = 0; i < CONST; i++) + { + //pq.insert((rand()%200) - 100); + //keyint = (i - (DONST / 2)); + cout << pq.pop().getKey() << endl; + cout << pq << endl; + } + + cout << "hello" << endl; + //unordered_map map; + unordered_map map; + for (size_t i = 0; i < 10; i++) + { + keyint = i; + map.insert(make_pair(keyint.getKey(), i)); + cout << (map[i]) << endl; + } + */ + /* + UpdatePriorityQueue rand_q; + intWkey intwkey; + for (size_t i = 0; i < 1000; i++) + { + intwkey = i; + rand_q.insert(intwkey, i, 0); + } + if (!rand_q.valid_heap()) + { + cout << "WHAT" << endl; + } + for (size_t i = 0; i < 1000; i++) + { + //std::cout << "editing " << i << std::endl; + intwkey = i; + int r = rand() % 1000; + int re = rand() % 1000; + //std::cout << "using new h val" << r << std::endl; + rand_q.update(intwkey, re, r); + if (!rand_q.valid_heap()) + { + cout << "WHAT" << endl; + return 0; + } + } + if (!rand_q.valid_heap()) + { + cout << "WHAT" << endl; + } + */ + EuclidianDistanceHueristic eh; + TypeWkey start; + TypeWkey goal; + //std::vector > move_list = Astar(occupancyGrid[], +// start, goal, eh); + + return 0; +} diff --git a/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp b/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp new file mode 100644 index 0000000..56c4569 --- /dev/null +++ b/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp @@ -0,0 +1,28 @@ +/** + * @file euclidiandistancehueristic.cpp + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + +#include "euclidiandistancehueristic.h" + +h_cost EuclidianDistanceHueristic::operator ()(const TypeWkey& start, + const TypeWkey& goal) const +{ + size_t start_x = start.getKey() % m_width; + size_t start_y = start.getKey() / m_width; + size_t goal_x = goal.getKey() % m_width; + size_t goal_y = goal.getKey() / m_width; + return m_scale + * sqrt( + ((goal_x - start_x) * (goal_x - start_x)) + + ((goal_y - start_y) * (goal_y - start_y))); +} + +EuclidianDistanceHueristic::~EuclidianDistanceHueristic() +{ + // TODO Auto-generated destructor stub +} + From 1f1f63d166db2a1795926d238195ae828f4ed901 Mon Sep 17 00:00:00 2001 From: Cazadorro Date: Fri, 29 Apr 2016 14:12:31 -0500 Subject: [PATCH 2/2] Fixed Astar, needs to be tested with moving robot in simulator first --- ros_ws/src/navigation/CMakeLists.txt | 9 +- .../src/navigation/include/navigation/astar.h | 6 +- .../navigation/include/navigation/astar.hpp | 60 ++++--- .../navigation/euclidiandistancehueristic.h | 3 +- .../navigation/include/navigation/minheap.h | 1 + .../navigation/include/navigation/minheap.hpp | 19 +- .../include/navigation/navigation.h | 30 ++++ .../navigation/include/navigation/typewkey.h | 14 ++ .../navigation/updatepriorityqueue.hpp | 28 +-- ros_ws/src/navigation/package.xml | 4 +- .../navigation/src/PathFinderTutorials.cpp | 165 ------------------ .../src/euclidiandistancehueristic.cpp | 16 +- ros_ws/src/navigation/src/minheap.cpp | 27 +++ ros_ws/src/navigation/src/navigation.cpp | 43 +++++ ros_ws/src/navigation/src/navigation_node.cpp | 28 +++ 15 files changed, 220 insertions(+), 233 deletions(-) create mode 100644 ros_ws/src/navigation/include/navigation/navigation.h delete mode 100644 ros_ws/src/navigation/src/PathFinderTutorials.cpp create mode 100644 ros_ws/src/navigation/src/minheap.cpp create mode 100644 ros_ws/src/navigation/src/navigation.cpp create mode 100644 ros_ws/src/navigation/src/navigation_node.cpp diff --git a/ros_ws/src/navigation/CMakeLists.txt b/ros_ws/src/navigation/CMakeLists.txt index bbb072c..4b24a77 100644 --- a/ros_ws/src/navigation/CMakeLists.txt +++ b/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 ) diff --git a/ros_ws/src/navigation/include/navigation/astar.h b/ros_ws/src/navigation/include/navigation/astar.h index 4466a54..b3c186e 100644 --- a/ros_ws/src/navigation/include/navigation/astar.h +++ b/ros_ws/src/navigation/include/navigation/astar.h @@ -9,16 +9,18 @@ #ifndef ASTAR_H_ #define ASTAR_H_ +#include #include "updatepriorityqueue.h" #include "typewkey.h" +#include template -std::vector> Astar(occupancyGrid[], +std::vector> Astar(nav_msgs::OccupancyGrid o_grid, TypeWkey start, TypeWkey goal, const Heuristic& heuristic); template const std::vector>& reconstruct_path( std::vector>& moves, - const unordered_map, TypeWkey >& parents, + const unordered_map& parents, const TypeWkey& current, size_t width, size_t hieght); #include "astar.hpp" diff --git a/ros_ws/src/navigation/include/navigation/astar.hpp b/ros_ws/src/navigation/include/navigation/astar.hpp index 1c5b2e1..6e2084d 100644 --- a/ros_ws/src/navigation/include/navigation/astar.hpp +++ b/ros_ws/src/navigation/include/navigation/astar.hpp @@ -8,15 +8,16 @@ template -std::vector> Astar(occupancyGrid[], +std::vector> Astar(nav_msgs::OccupancyGrid o_grid, TypeWkey start, TypeWkey goal, const Heuristic& heuristic) { + std::cout << "Starting Astar" << std::endl; //set of moves to be returned std::vector> 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> Astar(occupancyGrid[], //open priority queue, with removal and updating UpdatePriorityQueue, size_t> openPQ; //closed set map, maps the key for the type and the f cost for that type - unordered_map, f_cost> closedSet; + unordered_map closedSet; //map that maps parent with given key index - unordered_map, TypeWkey > parents; + unordered_map 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> Astar(occupancyGrid[], TypeWkey v; //set of adjacent moves to u std::vector, g_cost> > adj_moves; - + //std::cout << "HOLA" <(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,65 +83,68 @@ std::vector> Astar(occupancyGrid[], //north lookup if (temp_key >= width) { - adj_moves.push_back(TypeWkey(north_key), sdist); + adj_moves.push_back(std::make_pair(TypeWkey(north_key), sdist)); //north west lookup if (temp_key % width > 0) { - adj_moves.push_back(TypeWkey(north_w_key), ddist); + adj_moves.push_back(std::make_pair(TypeWkey(north_w_key), ddist)); } //north east lookup if (temp_key % width < (width - 1)) { - adj_moves.push_back(TypeWkey(north_e_key), ddist); + adj_moves.push_back(std::make_pair(TypeWkey(north_e_key), ddist)); } } //south if (temp_key <= (width - 1) * (height)) { - adj_moves.push_back(TypeWkey(south_key), sdist); + adj_moves.push_back(std::make_pair(TypeWkey(south_key), sdist)); //south west lookup if (temp_key % width > 0) { - adj_moves.push_back(TypeWkey(south_w_key), ddist); + adj_moves.push_back(std::make_pair(TypeWkey(south_w_key), ddist)); } //south east lookup if (temp_key % width < (width - 1)) { - adj_moves.push_back(TypeWkey(south_e_key), ddist); + adj_moves.push_back(std::make_pair(TypeWkey(south_e_key), ddist)); } } // west lookup if (temp_key % width > 0) { - adj_moves.push_back(TypeWkey(west_key), sdist); + adj_moves.push_back(std::make_pair(TypeWkey(west_key), sdist)); } // east lookup if (temp_key % width < (width - 1)) { - adj_moves.push_back(TypeWkey(east_key), sdist); + adj_moves.push_back(std::make_pair(TypeWkey(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 @@ -142,13 +152,14 @@ std::vector> Astar(occupancyGrid[], 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; } @@ -156,14 +167,17 @@ std::vector> Astar(occupancyGrid[], template const std::vector>& reconstruct_path( std::vector>& moves, - const unordered_map, TypeWkey >& parents, + const unordered_map& parents, const TypeWkey& current, size_t width, size_t hieght) { + TypeWkey 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; } diff --git a/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h b/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h index 7de015d..a59b7a5 100644 --- a/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h +++ b/ros_ws/src/navigation/include/navigation/euclidiandistancehueristic.h @@ -10,7 +10,9 @@ #define EUCLIDIANDISTANCEHUERISTIC_H_ #include "updatepriorityqueue.h" +#include "typewkey.h" #include +#include class EuclidianDistanceHueristic { @@ -26,7 +28,6 @@ class EuclidianDistanceHueristic } h_cost operator ()(const TypeWkey& start, const TypeWkey& goal) const; - virtual ~EuclidianDistanceHueristic(); }; #endif /* EUCLIDIANDISTANCEHUERISTIC_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/minheap.h b/ros_ws/src/navigation/include/navigation/minheap.h index c6a42f1..6ffb578 100644 --- a/ros_ws/src/navigation/include/navigation/minheap.h +++ b/ros_ws/src/navigation/include/navigation/minheap.h @@ -11,6 +11,7 @@ #include #include +#include using namespace std; template diff --git a/ros_ws/src/navigation/include/navigation/minheap.hpp b/ros_ws/src/navigation/include/navigation/minheap.hpp index ea43e59..036d77c 100644 --- a/ros_ws/src/navigation/include/navigation/minheap.hpp +++ b/ros_ws/src/navigation/include/navigation/minheap.hpp @@ -100,6 +100,10 @@ size_t MinHeap::heapifyUp(size_t i, unordered_map& 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& 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; -} diff --git a/ros_ws/src/navigation/include/navigation/navigation.h b/ros_ws/src/navigation/include/navigation/navigation.h new file mode 100644 index 0000000..34b915d --- /dev/null +++ b/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 +#include +#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_ */ diff --git a/ros_ws/src/navigation/include/navigation/typewkey.h b/ros_ws/src/navigation/include/navigation/typewkey.h index 8bab93a..9888b97 100644 --- a/ros_ws/src/navigation/include/navigation/typewkey.h +++ b/ros_ws/src/navigation/include/navigation/typewkey.h @@ -40,10 +40,24 @@ struct TypeWkey return *this; } + int getKey() const { return m_data; } }; +template +bool operator == (const TypeWkey& lhs, const TypeWkey& rhs) +{ + return lhs.getKey() == rhs.getKey(); +} + +template +bool operator != (const TypeWkey& lhs, const TypeWkey& rhs) +{ + return !(lhs == rhs); +} + + #endif /* TYPEWKEY_H_ */ diff --git a/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp b/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp index a9a63e5..4d0c39a 100644 --- a/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp +++ b/ros_ws/src/navigation/include/navigation/updatepriorityqueue.hpp @@ -67,9 +67,9 @@ void UpdatePriorityQueue::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 bool UpdatePriorityQueue::find(const T& item) const { K key = item.getKey(); - std::unordered_map::const_iterator got = m_id.find(key); + typename std::unordered_map::const_iterator got = m_id.find(key); return (got != m_id.end()) ? true : false; } @@ -239,20 +239,20 @@ bool UpdatePriorityQueue::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; } diff --git a/ros_ws/src/navigation/package.xml b/ros_ws/src/navigation/package.xml index b828401..354b7d6 100644 --- a/ros_ws/src/navigation/package.xml +++ b/ros_ws/src/navigation/package.xml @@ -44,10 +44,12 @@ roscpp sensor_msgs std_msgs + nav_msgs geometry_msgs roscpp sensor_msgs std_msgs + nav_msgs @@ -55,4 +57,4 @@ - \ No newline at end of file + diff --git a/ros_ws/src/navigation/src/PathFinderTutorials.cpp b/ros_ws/src/navigation/src/PathFinderTutorials.cpp deleted file mode 100644 index 50b9e32..0000000 --- a/ros_ws/src/navigation/src/PathFinderTutorials.cpp +++ /dev/null @@ -1,165 +0,0 @@ -//============================================================================ -// Name : PathFinderTutorials.cpp -// Author : -// Version : -// Copyright : Your copyright notice -// Description : Hello World in C++, Ansi-style -//============================================================================ - -#include -#include "updatepriorityqueue.h" -#include /* srand, rand */ -#include /* time */ -#include "typewkey.h" -#include -#include "euclidiandistancehueristic.h" -#include "astar.h" - -#define CONST 10 -#define DONST CONST -using namespace std; - -struct intWkey -{ - public: - int m_data; - intWkey(int data = 0) - { - m_data = data; - } - - intWkey(intWkey& intwkey) - { - *this = intwkey; - } - - intWkey(const intWkey& intwkey) - { - *this = intwkey; - } - - const intWkey& operator =(int data) - { - m_data = data; - return *this; - } - - const intWkey& operator =(intWkey& intwkey) - { - m_data = intwkey.m_data; - return *this; - } - const intWkey& operator =(const intWkey& intwkey) - { - m_data = intwkey.m_data; - return *this; - } - - int getKey() const - { - return m_data; - } -}; - -int main() -{ - srand(time(NULL)); - default_compare comp(false); - /* - MinHeap > heap(comp); - for (int i = 0; i < CONST; i++) - { - //heap.insert((rand()%200) - 100); - heap.insert(i - (DONST / 2)); - } - cout << "!!!Hello World!!!" << endl; // prints !!!Hello World!!! - cout << heap << endl; - - for (int i = 0; i < CONST; i++) - { - cout << heap.pop() << endl; - cout << heap << endl; - } - - UpdatePriorityQueue pq; - intWkey keyint; - for (size_t i = 0; i < CONST; i++) - { - //pq.insert((rand()%200) - 100); - keyint = (i); //- (DONST / 2)); - pq.insert(keyint, i, i); - cout << pq << endl; - } - - size_t temps; - keyint = 0; - temps = 20; //rand() % 10; - cout << "temps" << temps << endl; - pq.updateCost(keyint, temps); - cout << pq << endl; - cout << "GOODG" << endl; - for (size_t i = 0; i < 10; i++) - { - keyint = (i); - temps = rand() % 10; - cout << "temps" << temps << endl; - pq.updateCost(keyint, temps); - cout << pq << endl; - } - for (size_t i = 0; i < CONST; i++) - { - //pq.insert((rand()%200) - 100); - //keyint = (i - (DONST / 2)); - cout << pq.pop().getKey() << endl; - cout << pq << endl; - } - - cout << "hello" << endl; - //unordered_map map; - unordered_map map; - for (size_t i = 0; i < 10; i++) - { - keyint = i; - map.insert(make_pair(keyint.getKey(), i)); - cout << (map[i]) << endl; - } - */ - /* - UpdatePriorityQueue rand_q; - intWkey intwkey; - for (size_t i = 0; i < 1000; i++) - { - intwkey = i; - rand_q.insert(intwkey, i, 0); - } - if (!rand_q.valid_heap()) - { - cout << "WHAT" << endl; - } - for (size_t i = 0; i < 1000; i++) - { - //std::cout << "editing " << i << std::endl; - intwkey = i; - int r = rand() % 1000; - int re = rand() % 1000; - //std::cout << "using new h val" << r << std::endl; - rand_q.update(intwkey, re, r); - if (!rand_q.valid_heap()) - { - cout << "WHAT" << endl; - return 0; - } - } - if (!rand_q.valid_heap()) - { - cout << "WHAT" << endl; - } - */ - EuclidianDistanceHueristic eh; - TypeWkey start; - TypeWkey goal; - //std::vector > move_list = Astar(occupancyGrid[], -// start, goal, eh); - - return 0; -} diff --git a/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp b/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp index 56c4569..10d4aa8 100644 --- a/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp +++ b/ros_ws/src/navigation/src/euclidiandistancehueristic.cpp @@ -8,21 +8,17 @@ #include "euclidiandistancehueristic.h" -h_cost EuclidianDistanceHueristic::operator ()(const TypeWkey& start, - const TypeWkey& goal) const +h_cost EuclidianDistanceHueristic::operator ()(const TypeWkey& start, + const TypeWkey& goal) const { - size_t start_x = start.getKey() % m_width; - size_t start_y = start.getKey() / m_width; - size_t goal_x = goal.getKey() % m_width; - size_t goal_y = goal.getKey() / m_width; + std::size_t start_x = start.getKey() % m_width; + std::size_t start_y = start.getKey() / m_width; + std::size_t goal_x = goal.getKey() % m_width; + std::size_t goal_y = goal.getKey() / m_width; return m_scale * sqrt( ((goal_x - start_x) * (goal_x - start_x)) + ((goal_y - start_y) * (goal_y - start_y))); } -EuclidianDistanceHueristic::~EuclidianDistanceHueristic() -{ - // TODO Auto-generated destructor stub -} diff --git a/ros_ws/src/navigation/src/minheap.cpp b/ros_ws/src/navigation/src/minheap.cpp new file mode 100644 index 0000000..ea8b1cd --- /dev/null +++ b/ros_ws/src/navigation/src/minheap.cpp @@ -0,0 +1,27 @@ +/** + * @file minheap.cpp + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + +#include "minheap.h" + +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; +} + + diff --git a/ros_ws/src/navigation/src/navigation.cpp b/ros_ws/src/navigation/src/navigation.cpp new file mode 100644 index 0000000..6e7316b --- /dev/null +++ b/ros_ws/src/navigation/src/navigation.cpp @@ -0,0 +1,43 @@ +/** + * @file navigation.cpp + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + +#include "navigation.h" + +void Navigation::oMapCallback( + const nav_msgs::OccupancyGrid::ConstPtr& occupancy_grid) +{ + //size_t width = (*occupancy_grid).info.width; + //size_t height = (*occupancy_grid).info.height; + m_occupancy_grid = *occupancy_grid; + //std::cout << "Width " << width << " Height " << height << std::endl; + +} + +Navigation::Navigation() +{ + omap_sub = nh.subscribe ("map", 1, &Navigation::oMapCallback, this); +} + +void Navigation::update() +{ + size_t width = m_occupancy_grid.info.width; + size_t height = m_occupancy_grid.info.height; + if (width && height) + { + size_t x = 3; + size_t y = 4; + size_t xg = 100; + size_t yg = 70; + std::cout << "Width " << width << " Height " << height << std::endl; + TypeWkey start = (x % width) + (y * width); + TypeWkey goal = (xg % width) + (yg * width); + EuclidianDistanceHueristic eh(width, height, 1.0); + Astar(m_occupancy_grid, start, goal, + eh); + } +} diff --git a/ros_ws/src/navigation/src/navigation_node.cpp b/ros_ws/src/navigation/src/navigation_node.cpp new file mode 100644 index 0000000..10980f7 --- /dev/null +++ b/ros_ws/src/navigation/src/navigation_node.cpp @@ -0,0 +1,28 @@ +/** + * @file navigation_node.cpp + * @author shae, CS5201 Section A + * @date Apr 19, 2016 + * @brief Description: + * @details Details: + */ + + + +#include +#include "navigation.h" + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "navigation_node"); + Navigation nav_node; + ros::Rate loop_rate(1); + std::cout << "asdfasd" << std::endl; + while(ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + nav_node.update(); + } +} +