Pull Request

.gitignore

@@ -1,3 +1,4 @@
 *.o
 *.gch
 test
+*.swp

dijkstra.cpp

@@ -1,4 +1,4 @@
-////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////
 // Copyright (c) 2019, Vinitha Ranganeni
 // All rights reserved.
 //
@@ -31,37 +31,89 @@
 #include <set>
 #include <cassert>
 #include <algorithm>
-#include <iostream>
+
 namespace grid_planner {
 namespace planners {
 
 void Dijkstras::run_planner(
     const int& start_id,
     const int& goal_id,
     int* num_expansions,
-    std::vector<std::pair<int, int>> *path)
-{
-    // 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::vector<std::pair<int, int>> *path) {
+
+    CostMap cost_map;
+    CostMapComparator cost_map_comparator(cost_map);
+    std::set<int, CostMapComparator> Q(cost_map_comparator);
+
+    cost_map[start_id] = 0;
+    Q.insert(start_id);
+
+    ChildToParentMap child_to_parent_map;
+
+    std::vector<int> path_state_ids;
 
     // While the queue is not empty
     while (!Q.empty()) {
-        // Pop and expand the next node in the priority queue
         (*num_expansions)++;
 
-        // YOUR CODE HERE
+        const int parent_id = *(Q.begin());
+        Q.erase(Q.begin());
+
+        if (parent_id == goal_id) {
+            extract_path(child_to_parent_map, start_id, goal_id,
+                         &path_state_ids);
+            m_graph.get_path_coordinates(path_state_ids, path);
+            return;
+        }
+
+        std::vector<int> succ_ids;
+        std::vector<double> costs;
+        m_graph.get_succs(parent_id, &succ_ids, &costs);
+
+        assert(succ_ids.size() == costs.size());
+
+        for (int index = 0; index < succ_ids.size(); ++index) {
+            const int succ_state_id = succ_ids[index];
+            const double transition_cost = costs[index];
+            const double g_value = cost_map[parent_id] + transition_cost;
+            if (cost_map.find(succ_state_id) == cost_map.end() ||
+                g_value < cost_map[succ_state_id]) {
+                cost_map[succ_state_id] = g_value;
+                Q.erase(succ_state_id);
+                Q.insert(succ_state_id);
+                child_to_parent_map[succ_state_id] = parent_id;
+            }
+        }
     }
+
+    // if Q is empty, we need to call extract_path
+    extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids);
+
+    // set path parameter
+    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) {
 
+    if (goal_id == start_id) {
+        return;
+    }
+
+    (*path_state_ids).push_back(goal_id);
+
+    auto path_iter = child_to_parent_map.find(goal_id);
+    // loop till we find start or we reach end of map
+    while (path_iter != child_to_parent_map.end()) {
+        (*path_state_ids).push_back(path_iter->second);
+        path_iter = child_to_parent_map.find(path_iter->second);
+    }
+
+    std::reverse(path_state_ids->begin(), path_state_ids->end());
 }
-}
+}  // namespace planners
+
+}  // namespace grid_planner

dijkstra.h

@@ -27,13 +27,13 @@
 // POSSIBILITY OF SUCH DAMAGE.
 ////////////////////////////////////////////////////////////////////////////////
 
-#ifndef DIJKSTRAS_H_
-#define DIJKSTRAS_H_
+#ifndef DIJKSTRA_H_
+#define DIJKSTRA_H_
 
-#include "graph.h"
 #include <vector>
 #include <utility>
 #include <unordered_map>
+#include "graph.h"
 
 namespace grid_planner {
 namespace planners {
@@ -51,10 +51,7 @@ class CostMapComparator {
 
     bool operator()(const int& state_1,
                     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;
+        return cost_map_.find(state_1)->second <= cost_map_.find(state_2)->second;
     }
 
  private:
@@ -64,11 +61,10 @@ class CostMapComparator {
 // This class implements dijkstra's algorithm
 class Dijkstras {
  public:
-
     Dijkstras(
         const graphs::Graph& graph) : m_graph(graph) {}
 
-    ~Dijkstras() {};
+    ~Dijkstras() {}
 
     // Runs the planner from the start ID to the goal ID and fills in the
     // final path states into the path vector
@@ -87,10 +83,9 @@ class Dijkstras {
         std::vector<int> *path_state_ids);
 
     const graphs::Graph m_graph;
-
 };
 
-}
-}
+}  // namespace planners
+}  // namespace grid_planner
 
-#endif
+#endif  // DIJKSTRA_H_

graph.cpp

@@ -34,66 +34,104 @@
 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) {
+    m_start_id = get_state_id(x, y);
+    if (is_valid_state(x, y)) {
+        return m_start_id;  // valid state
+    }
+
+    return -1;  // invalid state
 }
 
-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) {
+    m_goal_id = get_state_id(x, y);
+    if (is_valid_state(x, y)) {
+        return m_goal_id;  // valid state
+    } else {
+        return -1;  // invalid state
+    }
 }
 
 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());
 
-    // YOUR CODE HERE
+    int x_source, y_source;
+    get_coord_from_state_id(source_state_id, &x_source, &y_source);
+
+    for (int i = -1; i <= 1; i++) {
+        for (int j = -1; j <= 1; j++) {
+            if (i == 0 && j == 0) {  // current state, not a successor
+                continue;
+            }
+
+            const int x_succ = x_source + i;
+            const int y_succ = y_source + j;
+
+            if (!is_valid_state(x_succ, y_succ)) {
+                continue;  // successor is not valid
+            }
+            const int succ_state_id = get_state_id(x_succ, y_succ);
+            (*succ_ids).push_back(succ_state_id);
+
+            // transition cost i.e. cost from parent to successor
+            const double succ_cost = get_action_cost(x_source, y_source,
+                                                     x_succ, y_succ);
+            (*costs).push_back(succ_cost);
+        }
+    }
 }
 
 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 {
+    for (int index = 0; index < path_state_ids.size(); ++index) {
+        const int path_state_id = path_state_ids[index];
+        int x, y;
+        if (get_coord_from_state_id(path_state_id, &x, &y)) {
+            // coordinates are valid
+            (*path_coordinates).push_back(std::make_pair(x, y));
+        }
+    }
 }
 
-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 x + y * m_width;
 }
 
-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());
 
-    // YOUR CODE HERE
-    return true;
+    *y = state_id / m_width;
+    *x = state_id - *y * m_width;
+
+    return 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 {
+    // check bounds (i.e. check value is valid)
+    if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) {
+        return false;
+    }
+    // check occupancy grid to see if cell is free
+    if (m_occupancy_grid[get_state_id(x, y)] == 0) {
+        return true;
+    }
+    return false;
 }
 
 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 {
+    // Calculate Euclidean distance between the 2 points
+    return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2));
 }
 
 }  // namespace graphs

graph.h

@@ -50,7 +50,7 @@ class Graph {
           m_width(width),
           m_height(height) {}
 
-    ~Graph() {};
+    ~Graph() {}
 
     // Sets and returns the start state ID (m_start_id)
     // Returns -1 if the state is not valid
@@ -97,11 +97,12 @@ class Graph {
     const int m_width;
     const int m_height;
 
+    // start and goal state ID
     int m_start_id;
     int m_goal_id;
 };
 
-}
-}
+}  // namespace graphs
+}  // namespace grid_planner
 
-#endif
+#endif  // GRAPH_H_

student_solutions/test_1.solution

@@ -0,0 +1,22 @@
+0 0
+1 0
+2 0
+3 1
+4 2
+5 3
+6 4
+7 4
+8 4
+9 4
+10 4
+11 5
+12 6
+12 7
+12 8
+12 9
+12 10
+12 11
+12 12
+12 13
+13 14
+14 15

student_solutions/test_2.solution

@@ -0,0 +1,18 @@
+13 14
+13 13
+13 12
+13 11
+13 10
+13 9
+13 8
+13 7
+12 6
+11 5
+10 6
+10 7
+10 8
+10 9
+10 10
+10 11
+9 12
+8 13