From dc1e2d8eb28ae468bd3949550caf50e4a97bdc73 Mon Sep 17 00:00:00 2001 From: JHLee0513 Date: Sun, 28 Apr 2019 22:31:15 -0700 Subject: [PATCH 1/4] started working --- .gitignore | 209 +++++++++++++++++++++++++++++++++++++++++++++++++++++ graph.cpp | 14 +++- 2 files changed, 221 insertions(+), 2 deletions(-) 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/graph.cpp b/graph.cpp index 9f6272b..b97ef59 100644 --- a/graph.cpp +++ b/graph.cpp @@ -36,13 +36,21 @@ namespace graphs { int Graph::set_start_state(const int& x, const int& y) { - // YOUR CODE HERE + if (x >= 0 && y>= 0) + { + m_start_id = (x,y); + return m_start_id; + } + return -1; } int Graph::set_goal_state(const int& x, const int& y) { - // YOUR CODE HERE + if (x >= 0 && y>= 0){ + m_goal_id = (x,y); + return m_goal_id; + } return -1; } @@ -54,6 +62,8 @@ void Graph::get_succs( assert(source_state_id < m_occupancy_grid.size()); // YOUR CODE HERE + + } void Graph::get_path_coordinates( From 950b45c297dbfdcd594b958c76242f40927bcc1d Mon Sep 17 00:00:00 2001 From: JHLee0513 Date: Fri, 17 May 2019 23:12:08 -0700 Subject: [PATCH 2/4] almost complete, debugging dijkstra before cpplint --- dijkstra.cpp | 70 ++++++++++++++++++++++++++++++++++++++++++++++++---- dijkstra.h | 4 +++ graph.cpp | 65 +++++++++++++++++++++++++++++++++++++----------- 3 files changed, 119 insertions(+), 20 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 35e5e47..b613526 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. @@ -39,18 +39,71 @@ void Dijkstras::run_planner( const int& start_id, const int& goal_id, int* num_expansions, + //pair(x,y) std::vector> *path) { + + //define costmap + CostMap map; + ChildToParentMap child_to_parent_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 - + CostMapComparator comparator(map); + std::set Q(comparator); // You will need to change this line + // While the queue is not empty + while (!Q.empty()) { // Pop and expand the next node in the priority queue (*num_expansions)++; - // YOUR CODE HERE + std::set::iterator curr_state = Q.begin(); + std::vector *succesor_ids; + std::vector *costs; + + + if (*curr_state == start_id) + { + map.insert(std::pair (start_id, 0)); + } + if (*curr_state == goal_id) { + + std::vector *path_ids; + extract_path(child_to_parent_map, start_id, goal_id, path_ids); + for (int i = 0; i < path_ids->size(); i++) { + //get_coordinate from state_id at each iteration + //convert coordinate into std::pair + //insert pair into *path + m_graph.get_path_coordinates(*path_ids,path); + + } + break; + } + m_graph.get_succs(*curr_state, succesor_ids, costs); + //for each succesor: + // check if in costmap: yes -> compare, no -> add to map with cost + for (int i = 0; i < succesor_ids->size(); i++) + { + int curr_id = succesor_ids->at(i); + //add to map (succesor, currrent state) + //child: succesor, parent: currr + //hence child to parent map + child_to_parent_map.insert(std::make_pair(curr_id, *curr_state)); + + CostMap::iterator map_cost = map.find(curr_id); + double succ_cost = costs->at(i); + double tmp_cost = succ_cost = map_cost->second + succ_cost; + if (map.find(curr_id) != map.end()) { + + if (succ_cost < map_cost->second) { + map.insert(std::pair(curr_id,succ_cost)); + } + } else { + map.insert(std::pair(curr_id, succ_cost + map_cost->second)); + } + } + ++curr_state; + } } @@ -60,7 +113,14 @@ void Dijkstras::extract_path( const int& goal_id, std::vector *path_state_ids) { - // YOUR CODE HERE + //will return path from start to goal + int start = goal_id; + path_state_ids->push_back(start); + while(start != start_id){ + int child = child_to_parent_map.at(start); + path_state_ids->push_back(child); + start =child; + } } } diff --git a/dijkstra.h b/dijkstra.h index ec24453..5c1d3a6 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -54,6 +54,10 @@ class CostMapComparator { // Given two states you need to write a comparator that determines // how to order them // YOUR CODE HERE (replace line below) + if (cost_map_.at(state_1) > cost_map_.at(state_2)) + { + return false; + } return true; } diff --git a/graph.cpp b/graph.cpp index b97ef59..f9cd2f0 100644 --- a/graph.cpp +++ b/graph.cpp @@ -36,19 +36,18 @@ namespace graphs { int Graph::set_start_state(const int& x, const int& y) { - if (x >= 0 && y>= 0) + if (is_valid_state(x,y)) { - m_start_id = (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) { - if (x >= 0 && y>= 0){ - m_goal_id = (x,y); + if (is_valid_state(x,y)){ + m_goal_id = get_state_id(x,y); return m_goal_id; } return -1; @@ -61,16 +60,40 @@ void Graph::get_succs( { assert(source_state_id < m_occupancy_grid.size()); - // YOUR CODE HERE - + int *x_so = 0; + int *y_so = 0; + 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++) + { + int* x_co = 0; + int* y_co = 0; + get_coord_from_state_id(source_state_id, x_co, y_co); + if (is_valid_state(*x_co+i, *y_co+j)) + { + succ_ids->push_back(get_state_id(*x_co+i, *y_co+j)); + + double cost = get_action_cost(*x_so, *y_so, *x_co, *y_co); + costs->push_back(cost); + } + } + } } void Graph::get_path_coordinates( const std::vector& path_state_ids, std::vector > *path_coordinates) const { - // YOUR CODE HERE + for(int i=0; i < path_state_ids.size(); i++) + { + int state_id = path_state_ids[i]; + int* x = 0; + int* y = 0; + 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 @@ -78,21 +101,33 @@ 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 { assert(state_id < m_occupancy_grid.size()); - // YOUR CODE HERE - return true; + int x_val = state_id % m_width; + int y_val = (state_id - x_val) / m_width; + *x = x_val; + *y = y_val; + + return (is_valid_state(x_val, y_val)); + } bool Graph::is_valid_state(const int& x, const int& y) const { - // YOUR CODE HERE + if (x < 0 || x > m_width || y < 0 || y > m_height) + { + return false; + } + //get state id to index id for occ grid to chek 1 for obstacle + int state_id = get_state_id(x, y); + if (m_occupancy_grid[state_id] == 1) { + return false; + } return true; } @@ -102,8 +137,8 @@ double Graph::get_action_cost( const int& succ_x, const int& succ_y) const { - // YOUR CODE HERE - return 0; + //euclidean distance + return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); } } // namespace graphs From de47615ee2db9888903535c541530fbfd03c7aad Mon Sep 17 00:00:00 2001 From: JHLee0513 Date: Tue, 21 May 2019 09:25:35 -0700 Subject: [PATCH 3/4] It works my dude, time for cpplint --- dijkstra.cpp | 141 +++++++++++++++++++++++++++------------------------ dijkstra.h | 9 +++- graph.cpp | 68 +++++++++++++------------ graph.h | 2 +- 4 files changed, 119 insertions(+), 101 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index b613526..7c1ad74 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -39,89 +39,100 @@ void Dijkstras::run_planner( const int& start_id, const int& goal_id, int* num_expansions, - //pair(x,y) - std::vector> *path) -{ - - //define costmap + std::vector> *path) { + CostMap map; ChildToParentMap child_to_parent_map; - // Create priority queue; I suggest using a set with with the custom - // comparator defined in dijkstra.h as your priority queue CostMapComparator comparator(map); - std::set Q(comparator); // You will need to change this line - - // While the queue is not empty + std::set Q(comparator); + std::vector path_ids; + Q.insert(start_id); + map[start_id] = 0; + //std::cout << "initial" << '\n'; while (!Q.empty()) { - // Pop and expand the next node in the priority queue (*num_expansions)++; - - std::set::iterator curr_state = Q.begin(); - std::vector *succesor_ids; - std::vector *costs; + int curr_state = *(Q.begin()); + Q.erase(Q.begin()); + Q.erase(curr_state); - - if (*curr_state == start_id) - { - map.insert(std::pair (start_id, 0)); - } - if (*curr_state == goal_id) { - - std::vector *path_ids; - extract_path(child_to_parent_map, start_id, goal_id, path_ids); - for (int i = 0; i < path_ids->size(); i++) { - //get_coordinate from state_id at each iteration - //convert coordinate into std::pair - //insert pair into *path - m_graph.get_path_coordinates(*path_ids,path); - - } - break; + //std::cout << "goal condition" << '\n'; + 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); + + //std::cout << *num_expansions << '\n'; + return; } - m_graph.get_succs(*curr_state, succesor_ids, costs); - //for each succesor: - // check if in costmap: yes -> compare, no -> add to map with cost - for (int i = 0; i < succesor_ids->size(); i++) - { - int curr_id = succesor_ids->at(i); - //add to map (succesor, currrent state) - //child: succesor, parent: currr - //hence child to parent map - child_to_parent_map.insert(std::make_pair(curr_id, *curr_state)); - - CostMap::iterator map_cost = map.find(curr_id); - double succ_cost = costs->at(i); - double tmp_cost = succ_cost = map_cost->second + succ_cost; - if (map.find(curr_id) != map.end()) { + //std::cout << "condition passed" << '\n'; + std::vector succesor_ids; + std::vector costs; + m_graph.get_succs(curr_state, &succesor_ids, &costs); + + //std::cout << "expand" << '\n'; + //for (auto iter = Q.begin(); iter != Q.end(); ++iter) { + // std::cout << *iter % 15 << " <-x, y-> " << *iter / 15 << '\n'; + //} + + for (int idx = 0; idx < succesor_ids.size(); ++idx) { + int succ_id = succesor_ids[idx]; + //double curr_cost = map[curr_state]; + 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; + //std::cout << "succesor: " << '\n'; + //std::cout << succ_id / 15 << '\n'; + //std::cout << succ_id - (succ_id / 15) * 15 << '\n' << '\n'; + //std::cout << new_cost << '\n'; + assert(Q.find(curr_state) == Q.end()); - if (succ_cost < map_cost->second) { - map.insert(std::pair(curr_id,succ_cost)); - } - } else { - map.insert(std::pair(curr_id, succ_cost + map_cost->second)); + Q.erase(succ_id); + Q.insert(succ_id); } + } - ++curr_state; - + //std::cout << "===== step ======" << '\n'; + assert(Q.find(curr_state) == Q.end()); + //std::cout << "stop" << '\n'; + } + extract_path(child_to_parent_map, start_id, goal_id, &path_ids); + + m_graph.get_path_coordinates(path_ids,path); + //std::cout << *num_expansions << '\n'; } void Dijkstras::extract_path( const ChildToParentMap& child_to_parent_map, const int& start_id, const int& goal_id, - std::vector *path_state_ids) -{ - //will return path from start to goal - int start = goal_id; - path_state_ids->push_back(start); - while(start != start_id){ - int child = child_to_parent_map.at(start); - path_state_ids->push_back(child); - start =child; + 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); + + //debugging + //check if anything being added + //std::cout << parent->second << '\n'; + + + parent = child_to_parent_map.find(parent->second); + } + std::reverse(path_state_ids->begin(), path_state_ids->end()); + + //debugging + //std::cout << child_to_parent_map.size() << '\n'; + //std::cout << path_state_ids->size() << '\n'; } -} + +} +} diff --git a/dijkstra.h b/dijkstra.h index 5c1d3a6..7b5852a 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -53,12 +53,17 @@ 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) - if (cost_map_.at(state_1) > cost_map_.at(state_2)) + + /* + if (*cost_map_.find(state_1) > *cost_map_.find(state_2)) { return false; } return true; + /**/ + + return cost_map_.find(state_1)->second <= cost_map_.find(state_2)->second; + } private: diff --git a/graph.cpp b/graph.cpp index f9cd2f0..68011e1 100644 --- a/graph.cpp +++ b/graph.cpp @@ -30,6 +30,8 @@ #include "graph.h" #include #include +//below for debugging +#include namespace grid_planner { namespace graphs { @@ -60,24 +62,23 @@ void Graph::get_succs( { assert(source_state_id < m_occupancy_grid.size()); - - int *x_so = 0; - int *y_so = 0; - 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++) - { - int* x_co = 0; - int* y_co = 0; - get_coord_from_state_id(source_state_id, x_co, y_co); - if (is_valid_state(*x_co+i, *y_co+j)) - { - succ_ids->push_back(get_state_id(*x_co+i, *y_co+j)); - - double cost = get_action_cost(*x_so, *y_so, *x_co, *y_co); - costs->push_back(cost); - } + 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; + //get_coord_from_state_id(source_state_id, x_co, y_co); + if (is_valid_state(x_succ, y_succ)) { + //std::cout << "succesor added" << '\n'; + 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); + //std::cout << x_co << '\n'; + } + } } } } @@ -86,13 +87,13 @@ void Graph::get_path_coordinates( const std::vector& path_state_ids, std::vector > *path_coordinates) const { - for(int i=0; i < path_state_ids.size(); i++) - { + for(int i=0; i < path_state_ids.size(); ++i) { int state_id = path_state_ids[i]; - int* x = 0; - int* y = 0; - get_coord_from_state_id(state_id, x, y); - path_coordinates->push_back(std::make_pair(*x, *y)); + int x, y; + if (get_coord_from_state_id(state_id, &x, &y)) { + path_coordinates->push_back(std::make_pair(x, y)); + } + } } @@ -101,34 +102,35 @@ int Graph::get_state_id(const int& x, const int& y) const assert(x < m_width); assert(y < m_height); - return y * m_width + x; + return (y * m_width) + x; } bool Graph::get_coord_from_state_id(const int& state_id, int* x, int* y) const { assert(state_id < m_occupancy_grid.size()); - int x_val = state_id % m_width; - int y_val = (state_id - x_val) / m_width; + //int x_val = state_id % m_width; + int y_val = state_id / m_width; + int x_val = state_id - y_val * m_width; *x = x_val; *y = y_val; - return (is_valid_state(x_val, y_val)); + return (is_valid_state(*x, *y)); } bool Graph::is_valid_state(const int& x, const int& y) const { - if (x < 0 || x > m_width || y < 0 || y > m_height) + if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) { return false; } //get state id to index id for occ grid to chek 1 for obstacle int state_id = get_state_id(x, y); - if (m_occupancy_grid[state_id] == 1) { - return false; + if (m_occupancy_grid[state_id] == 0) { + return true; } - return true; + return false; } double Graph::get_action_cost( @@ -138,7 +140,7 @@ double Graph::get_action_cost( const int& succ_y) const { //euclidean distance - return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); + return sqrt(pow((double)(succ_x - source_x), 2) + pow((double)(succ_y - source_y), 2)); } } // namespace graphs diff --git a/graph.h b/graph.h index 0d3b62d..f46be66 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 From fddbfa4d1fe8fd9dc45a349809fa84a85a801eef Mon Sep 17 00:00:00 2001 From: JHLee0513 Date: Tue, 21 May 2019 21:57:03 -0700 Subject: [PATCH 4/4] cpplint check --- dijkstra.cpp | 50 ++++++---------------------------------------- dijkstra.h | 27 ++++++++----------------- graph.cpp | 56 +++++++++++++++------------------------------------- graph.h | 6 +++--- 4 files changed, 33 insertions(+), 106 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 7c1ad74..f20f5fb 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -27,7 +27,7 @@ // POSSIBILITY OF SUCH DAMAGE. //////////////////////////////////////////////////////////////////////////////// -#include "dijkstra.h" +#include "./dijkstra.h" #include #include #include @@ -46,62 +46,36 @@ void Dijkstras::run_planner( CostMapComparator comparator(map); std::set Q(comparator); std::vector path_ids; - Q.insert(start_id); map[start_id] = 0; - //std::cout << "initial" << '\n'; while (!Q.empty()) { (*num_expansions)++; int curr_state = *(Q.begin()); Q.erase(Q.begin()); Q.erase(curr_state); - - //std::cout << "goal condition" << '\n'; 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); - - //std::cout << *num_expansions << '\n'; + m_graph.get_path_coordinates(path_ids, path); return; } - //std::cout << "condition passed" << '\n'; std::vector succesor_ids; std::vector costs; m_graph.get_succs(curr_state, &succesor_ids, &costs); - - //std::cout << "expand" << '\n'; - //for (auto iter = Q.begin(); iter != Q.end(); ++iter) { - // std::cout << *iter % 15 << " <-x, y-> " << *iter / 15 << '\n'; - //} - for (int idx = 0; idx < succesor_ids.size(); ++idx) { int succ_id = succesor_ids[idx]; - //double curr_cost = map[curr_state]; 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; - //std::cout << "succesor: " << '\n'; - //std::cout << succ_id / 15 << '\n'; - //std::cout << succ_id - (succ_id / 15) * 15 << '\n' << '\n'; - //std::cout << new_cost << '\n'; assert(Q.find(curr_state) == Q.end()); - Q.erase(succ_id); Q.insert(succ_id); } - } - //std::cout << "===== step ======" << '\n'; assert(Q.find(curr_state) == Q.end()); - //std::cout << "stop" << '\n'; - } extract_path(child_to_parent_map, start_id, goal_id, &path_ids); - - m_graph.get_path_coordinates(path_ids,path); - //std::cout << *num_expansions << '\n'; + m_graph.get_path_coordinates(path_ids, path); } void Dijkstras::extract_path( @@ -114,25 +88,13 @@ void Dijkstras::extract_path( } 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()){ + while (parent != child_to_parent_map.end()) { path_state_ids->push_back(parent->second); - - //debugging - //check if anything being added - //std::cout << parent->second << '\n'; - - parent = child_to_parent_map.find(parent->second); } std::reverse(path_state_ids->begin(), path_state_ids->end()); - - //debugging - //std::cout << child_to_parent_map.size() << '\n'; - //std::cout << path_state_ids->size() << '\n'; } - -} -} +} // namespace planners +} // namespace grid_planner diff --git a/dijkstra.h b/dijkstra.h index 7b5852a..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 @@ -54,16 +54,8 @@ class CostMapComparator { // Given two states you need to write a comparator that determines // how to order them - /* - if (*cost_map_.find(state_1) > *cost_map_.find(state_2)) - { - return false; - } - return true; - /**/ - - return cost_map_.find(state_1)->second <= cost_map_.find(state_2)->second; - + return cost_map_.find(state_1)->second <= + cost_map_.find(state_2)->second; } private: @@ -73,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 @@ -96,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 68011e1..6ab3e43 100644 --- a/graph.cpp +++ b/graph.cpp @@ -27,29 +27,24 @@ // POSSIBILITY OF SUCH DAMAGE. //////////////////////////////////////////////////////////////////////////////// -#include "graph.h" +#include "./graph.h" #include #include -//below for debugging -#include namespace grid_planner { namespace graphs { -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); +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) -{ - if (is_valid_state(x,y)){ - m_goal_id = get_state_id(x,y); +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; @@ -58,10 +53,8 @@ int Graph::set_goal_state(const int& x, const int& y) 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()); - int x_so, y_so; get_coord_from_state_id(source_state_id, &x_so, &y_so); for (int i = -1; i <= 1; i++) { @@ -69,14 +62,10 @@ void Graph::get_succs( if (!(i == 0 && j == 0)) { int x_succ = x_so + i; int y_succ = y_so + j; - //get_coord_from_state_id(source_state_id, x_co, y_co); if (is_valid_state(x_succ, y_succ)) { - //std::cout << "succesor added" << '\n'; 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); - //std::cout << x_co << '\n'; } } } @@ -85,47 +74,37 @@ void Graph::get_succs( void Graph::get_path_coordinates( const std::vector& path_state_ids, - std::vector > *path_coordinates) const -{ - for(int i=0; i < path_state_ids.size(); ++i) { + 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); 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 x_val = state_id % m_width; int y_val = state_id / m_width; int x_val = state_id - y_val * m_width; *x = x_val; *y = y_val; return (is_valid_state(*x, *y)); - } -bool Graph::is_valid_state(const int& x, const int& y) const -{ - if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) - { +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; } - //get state id to index id for occ grid to chek 1 for obstacle int state_id = get_state_id(x, y); if (m_occupancy_grid[state_id] == 0) { return true; @@ -137,12 +116,9 @@ double Graph::get_action_cost( const int& source_x, const int& source_y, const int& succ_x, - const int& succ_y) const -{ - //euclidean distance - return sqrt(pow((double)(succ_x - source_x), 2) + pow((double)(succ_y - source_y), 2)); + 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 f46be66..48a1b1c 100644 --- a/graph.h +++ b/graph.h @@ -101,7 +101,7 @@ class Graph { int m_goal_id; }; -} -} +} // namespace graphs +} // namespace grid_planner -#endif +#endif // GRAPH_H_