From 9d2b41459c5bc883a53f729679140f96506f8590 Mon Sep 17 00:00:00 2001 From: Andressa Chan Date: Fri, 18 Oct 2019 14:40:53 -0700 Subject: [PATCH] Finished grid planner --- dijkstra.cpp | 51 +++++++++++++++++++++++++---- dijkstra.h | 8 +++-- graph.cpp | 91 ++++++++++++++++++++++++++++++++++------------------ 3 files changed, 110 insertions(+), 40 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 35e5e47..d361bc2 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #include namespace grid_planner { namespace planners { @@ -39,28 +40,64 @@ void Dijkstras::run_planner( const int& start_id, const int& goal_id, int* num_expansions, - std::vector> *path) -{ + std::vector> *path) { + CostMap cost_map; + cost_map[start_id] = 0; + ChildToParentMap child_to_parent_map; + CostMapComparator cost_map_comparator(cost_map); + // Create priority queue; I suggest using a set with with the custom // comparator defined in dijkstra.h as your priority queue - std::set Q; // You will need to change this line + std::set Q(cost_map_comparator); + std::set visited; + Q.insert(start_id); // While the queue is not empty while (!Q.empty()) { // Pop and expand the next node in the priority queue (*num_expansions)++; + std::set::iterator it = Q.begin(); + Q.erase(it); + int node = *it; + visited.insert(node); + if (node == goal_id) { + break; + } - // YOUR CODE HERE + std::vector succ_ids; + std::vector costs; + m_graph.get_succs(node, &succ_ids, &costs); + for (int i = 0; i != succ_ids.size(); i++) { + double updatedDistance = cost_map[node] + costs[i]; + bool seenNode = visited.find(succ_ids[i]) != visited.end(); + if (!seenNode || (updatedDistance < cost_map[succ_ids[i]])) { + cost_map[succ_ids[i]] = updatedDistance; + child_to_parent_map[succ_ids[i]] = node; + Q.insert(succ_ids[i]); + visited.insert(succ_ids[i]); + } + } } + + std::vector path_state_ids; + extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); + m_graph.get_path_coordinates(path_state_ids, path); } void Dijkstras::extract_path( const ChildToParentMap& child_to_parent_map, const int& start_id, const int& goal_id, - std::vector *path_state_ids) -{ - // YOUR CODE HERE + std::vector *path_state_ids) { + std::vector path; + int parent = goal_id; + while (parent != start_id) { + parent = child_to_parent_map.at(parent); + path.push_back(parent); + } + std::reverse(path.begin(), path.end()); + path.push_back(goal_id); + *path_state_ids = path; } } diff --git a/dijkstra.h b/dijkstra.h index ec24453..5d48aac 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -53,8 +53,12 @@ class CostMapComparator { const int& state_2) const { // Given two states you need to write a comparator that determines // how to order them - // YOUR CODE HERE (replace line below) - return true; + // int costDiff = cost_map_.at(state_1) - cost_map_.at(state_2); + // if (costDiff == 0) { + // return state_1 < state_2; + // } + // return costDiff < 0; + return cost_map_.at(state_1) <= cost_map_.at(state_2); } private: diff --git a/graph.cpp b/graph.cpp index 9f6272b..f78773e 100644 --- a/graph.cpp +++ b/graph.cpp @@ -34,66 +34,95 @@ namespace grid_planner { namespace graphs { -int Graph::set_start_state(const int& x, const int& y) -{ - // YOUR CODE HERE - return -1; +int Graph::set_start_state(const int& x, const int& y) { + if (x < 0 || x >= m_width || y < 0 || y >= m_height) { + return -1; + } + m_start_id = get_state_id(x, y); + if (!is_valid_state(x, y)) { + return -1; + } + return m_start_id; } -int Graph::set_goal_state(const int& x, const int& y) -{ - // YOUR CODE HERE - return -1; +int Graph::set_goal_state(const int& x, const int& y) { + if (x < 0 || x >= m_width || y < 0 || y >= m_height) { + return -1; + } + m_goal_id = get_state_id(x, y); + if (!is_valid_state(x, y)) { + return -1; + } + return m_goal_id; } void Graph::get_succs( const int& source_state_id, std::vector *succ_ids, - std::vector *costs) const -{ + std::vector *costs) const { assert(source_state_id < m_occupancy_grid.size()); + std::vector succ_temp; + std::vector costs_temp; + int x, y; + Graph::get_coord_from_state_id(source_state_id, &x, &y); + std::vector x_coords{x - 1, x, x + 1}; + std::vector y_coords{y - 1, y, y + 1}; + for (auto i = x_coords.begin(); i != x_coords.end(); i++) { + for (auto j = y_coords.begin(); j != y_coords.end(); j++) { + if (Graph::is_valid_state(*i, *j)) { + succ_temp.push_back(get_state_id(*i, *j)); + costs_temp.push_back(get_action_cost(x, y, *i, *j)); + } + } + } - // YOUR CODE HERE + *succ_ids = succ_temp; + *costs = costs_temp; } void Graph::get_path_coordinates( const std::vector& path_state_ids, - std::vector > *path_coordinates) const -{ - // YOUR CODE HERE + std::vector > *path_coordinates) const { + std::vector> coordinates; + for (int i = 0; i < path_state_ids.size(); i++) { + int x, y; + bool valid = get_coord_from_state_id(path_state_ids[i], &x, &y); + assert(valid); + std::pair coord(x, y); + coordinates.push_back(coord); + } + *path_coordinates = coordinates; } -int Graph::get_state_id(const int& x, const int& y) const -{ +int Graph::get_state_id(const int& x, const int& y) const { assert(x < m_width); assert(y < m_height); - - // YOUR CODE HERE - return 0; + return (y * m_width) + x; } -bool Graph::get_coord_from_state_id(const int& state_id, int* x, int* y) const -{ +bool Graph::get_coord_from_state_id(const int& state_id, int* x, int* y) const { assert(state_id < m_occupancy_grid.size()); + *y = state_id / m_width; + *x = state_id - (*y * m_width); - // YOUR CODE HERE - return true; + return Graph::is_valid_state(*x, *y); } -bool Graph::is_valid_state(const int& x, const int& y) const -{ - // YOUR CODE HERE - return true; +bool Graph::is_valid_state(const int& x, const int& y) const { + if (x < 0 || x >= m_width || y < 0 || y >= m_height) { + return false; + } + int state_id = Graph::get_state_id(x, y); + return m_occupancy_grid[state_id] == 0; } double Graph::get_action_cost( const int& source_x, const int& source_y, const int& succ_x, - const int& succ_y) const -{ - // YOUR CODE HERE - return 0; + const int& succ_y) const { + return sqrt(pow(source_x - succ_x, 2) + + pow(source_y - succ_y, 2)); // euclidean distance } } // namespace graphs