diff --git a/.gitignore b/.gitignore index 14635d6..a221e3b 100644 --- a/.gitignore +++ b/.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/ diff --git a/dijkstra.cpp b/dijkstra.cpp index 35e5e47..f20f5fb 100644 --- a/dijkstra.cpp +++ b/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 #include #include @@ -39,29 +39,62 @@ void Dijkstras::run_planner( const int& start_id, const int& goal_id, int* num_expansions, - std::vector> *path) -{ - // 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::vector> *path) { - // While the queue is not empty + CostMap map; + ChildToParentMap child_to_parent_map; + CostMapComparator comparator(map); + std::set Q(comparator); + std::vector 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 succesor_ids; + std::vector 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 *path_state_ids) -{ - // YOUR CODE HERE -} + std::vector *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 diff --git a/dijkstra.h b/dijkstra.h index ec24453..544170c 100644 --- a/dijkstra.h +++ b/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 #include #include @@ -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 *path_state_ids); const graphs::Graph m_graph; - }; +} // namespace planners +} // namespace grid_planner -} -} - -#endif +#endif // DIJKSTRA_H_ diff --git a/graph.cpp b/graph.cpp index 9f6272b..6ab3e43 100644 --- a/graph.cpp +++ b/graph.cpp @@ -27,75 +27,98 @@ // POSSIBILITY OF SUCH DAMAGE. //////////////////////////////////////////////////////////////////////////////// -#include "graph.h" +#include "./graph.h" #include #include namespace grid_planner { namespace graphs { -int Graph::set_start_state(const int& x, const int& y) -{ - // YOUR CODE HERE +int Graph::set_start_state(const int& x, const int& y) { + if (is_valid_state(x, y)) { + m_start_id = get_state_id(x, y); + return m_start_id; + } return -1; } -int Graph::set_goal_state(const int& x, const int& y) -{ - // YOUR CODE HERE +int Graph::set_goal_state(const int& x, const int& y) { + if (is_valid_state(x, y)) { + m_goal_id = get_state_id(x, y); + return m_goal_id; + } return -1; } 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()); - - // YOUR CODE HERE + int x_so, y_so; + get_coord_from_state_id(source_state_id, &x_so, &y_so); + for (int i = -1; i <= 1; i++) { + for (int j = -1; j <= 1; j++) { + if (!(i == 0 && j == 0)) { + int x_succ = x_so + i; + int y_succ = y_so + j; + if (is_valid_state(x_succ, y_succ)) { + succ_ids->push_back(get_state_id(x_succ, y_succ)); + double cost = get_action_cost(x_so, y_so, x_succ, y_succ); + costs->push_back(cost); + } + } + } + } } void Graph::get_path_coordinates( const std::vector& path_state_ids, - std::vector > *path_coordinates) const -{ - // YOUR CODE HERE + std::vector > *path_coordinates) const { + for (int i = 0; i < path_state_ids.size(); ++i) { + int state_id = path_state_ids[i]; + int x, y; + if (get_coord_from_state_id(state_id, &x, &y)) { + 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 (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()); + int y_val = state_id / m_width; + int x_val = state_id - y_val * m_width; + *x = x_val; + *y = y_val; - // YOUR CODE HERE - return true; + 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 { + if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) { + return false; + } + int state_id = get_state_id(x, y); + if (m_occupancy_grid[state_id] == 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 { + return sqrt(pow((succ_x - source_x), 2) + pow((succ_y - source_y), 2)); } - } // namespace graphs } // namespace grid_planner diff --git a/graph.h b/graph.h index 0d3b62d..48a1b1c 100644 --- a/graph.h +++ b/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 @@ -101,7 +101,7 @@ class Graph { int m_goal_id; }; -} -} +} // namespace graphs +} // namespace grid_planner -#endif +#endif // GRAPH_H_