Finished grid planner

dijkstra.cpp

@@ -31,6 +31,7 @@
 #include <set>
 #include <cassert>
 #include <algorithm>
+#include <float.h>
 #include <iostream>
 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<std::pair<int, int>> *path)
-{
+    std::vector<std::pair<int, int>> *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<int> Q; // You will need to change this line
+    std::set<int, CostMapComparator> Q(cost_map_comparator);
+    std::set<int> 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<int>::iterator it = Q.begin();
+        Q.erase(it);
+        int node = *it;
+        visited.insert(node);
+        if (node == goal_id) {
+            break;
+        }
 
-        // YOUR CODE HERE
+        std::vector<int> succ_ids;
+        std::vector<double> 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<int> 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<int> *path_state_ids)
-{
-    // YOUR CODE HERE
+    std::vector<int> *path_state_ids) {
+    std::vector<int> 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;
 }
 
 }

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:

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<int> *succ_ids,
-    std::vector<double> *costs) const
-{
+    std::vector<double> *costs) const {
     assert(source_state_id < m_occupancy_grid.size());
+    std::vector<int> succ_temp;
+    std::vector<double> costs_temp;
+    int x, y;
+    Graph::get_coord_from_state_id(source_state_id, &x, &y);
+    std::vector<int> x_coords{x - 1, x, x + 1};
+    std::vector<int> 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<int>& path_state_ids,
-    std::vector<std::pair<int, int> > *path_coordinates) const
-{
-    // YOUR CODE HERE
+    std::vector<std::pair<int, int> > *path_coordinates) const {
+    std::vector<std::pair<int, int>> 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<int, int> 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