Grid planner code complete

.gitignore

@@ -1,3 +1,212 @@
 *.o
 *.gch
 test
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# Prerequisites
+*.d
+
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don’t work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# celery beat schedule file
+celerybeat-schedule
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/

dijkstra.cpp

@@ -3,7 +3,7 @@
 // All rights reserved.
 //
 // Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
+// modification, are permitted provided that g following conditions are met:
 //
 //     1. Redistributions of source code must retain the above copyright notice
 //        this list of conditions and the following disclaimer.
@@ -27,7 +27,7 @@
 // POSSIBILITY OF SUCH DAMAGE.
 ////////////////////////////////////////////////////////////////////////////////
 
-#include "dijkstra.h"
+#include "./dijkstra.h"
 #include <set>
 #include <cassert>
 #include <algorithm>
@@ -39,29 +39,62 @@ 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) {
 
-    // While the queue is not empty
+    CostMap map;
+    ChildToParentMap child_to_parent_map;
+    CostMapComparator comparator(map);
+    std::set<int, CostMapComparator> Q(comparator);
+    std::vector<int> path_ids;
+    Q.insert(start_id);
+    map[start_id] = 0;
     while (!Q.empty()) {
-        // Pop and expand the next node in the priority queue
         (*num_expansions)++;
-
-        // YOUR CODE HERE
+        int curr_state = *(Q.begin());
+        Q.erase(Q.begin());
+        Q.erase(curr_state);
+        if (curr_state  == goal_id) {
+            extract_path(child_to_parent_map, start_id, goal_id, &path_ids);
+            m_graph.get_path_coordinates(path_ids, path);
+            return;
+        }
+        std::vector<int> succesor_ids;
+        std::vector<double> costs;
+        m_graph.get_succs(curr_state, &succesor_ids, &costs);
+        for (int idx = 0; idx < succesor_ids.size(); ++idx) {
+            int succ_id = succesor_ids[idx];
+            double new_cost = map[curr_state] + costs[idx];
+            if (map.find(succ_id) == map.end() || map[succ_id] > new_cost) {
+                child_to_parent_map[succ_id] = curr_state;
+                map[succ_id] = new_cost;
+                assert(Q.find(curr_state) == Q.end());
+                Q.erase(succ_id);
+                Q.insert(succ_id);
+            }
+        }
+        assert(Q.find(curr_state) == Q.end());
     }
+    extract_path(child_to_parent_map, start_id, goal_id, &path_ids);
+    m_graph.get_path_coordinates(path_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;
+    }
 
+    assert(child_to_parent_map.find(goal_id) != child_to_parent_map.end());
+    auto parent = child_to_parent_map.find(goal_id);
+    path_state_ids->push_back(goal_id);
+    while (parent != child_to_parent_map.end()) {
+        path_state_ids->push_back(parent->second);
+        parent = child_to_parent_map.find(parent->second);
+    }
+    std::reverse(path_state_ids->begin(), path_state_ids->end());
 }
-}
+}  // namespace planners
+}  // namespace grid_planner

dijkstra.h

@@ -27,10 +27,10 @@
 // POSSIBILITY OF SUCH DAMAGE.
 ////////////////////////////////////////////////////////////////////////////////
 
-#ifndef DIJKSTRAS_H_
+#ifndef DIJKSTRA_H_
 #define DIJKSTRAS_H_
 
-#include "graph.h"
+#include "./graph.h"
 #include <vector>
 #include <utility>
 #include <unordered_map>
@@ -53,8 +53,9 @@ 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;
+
+        return cost_map_.find(state_1)->second <= 
+        cost_map_.find(state_2)->second;
     }
 
  private:
@@ -64,11 +65,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 +87,8 @@ class Dijkstras {
         std::vector<int> *path_state_ids);
 
     const graphs::Graph m_graph;
-
 };
+}  // namespace planners
+}  // namespace grid_planner
 
-}
-}
-
-#endif
+#endif  // DIJKSTRA_H_