diff --git a/.dijkstra.cpp.un~ b/.dijkstra.cpp.un~ new file mode 100644 index 0000000..27fc761 Binary files /dev/null and b/.dijkstra.cpp.un~ differ diff --git a/.dijkstra.h.un~ b/.dijkstra.h.un~ new file mode 100644 index 0000000..89f84b2 Binary files /dev/null and b/.dijkstra.h.un~ differ diff --git a/.gitignore b/.gitignore index 14635d6..4143bf2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ *.o *.gch test +*.swp diff --git a/.graph.cpp.un~ b/.graph.cpp.un~ new file mode 100644 index 0000000..6c95332 Binary files /dev/null and b/.graph.cpp.un~ differ diff --git a/dijkstra.cpp b/dijkstra.cpp index 35e5e47..783c463 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -1,4 +1,4 @@ -//////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////// // Copyright (c) 2019, Vinitha Ranganeni // All rights reserved. // @@ -31,7 +31,7 @@ #include #include #include -#include + namespace grid_planner { namespace planners { @@ -39,29 +39,81 @@ 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) { + + CostMap cost_map; + CostMapComparator cost_map_comparator(cost_map); + std::set Q(cost_map_comparator); + + cost_map[start_id] = 0; + Q.insert(start_id); + + ChildToParentMap child_to_parent_map; + + std::vector 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 succ_ids; + std::vector 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 *path_state_ids) -{ - // YOUR CODE HERE -} + std::vector *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 diff --git a/dijkstra.h b/dijkstra.h index ec24453..c0b5256 100644 --- a/dijkstra.h +++ b/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 #include #include +#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 *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..9cf455d 100644 --- a/graph.cpp +++ b/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 *succ_ids, - std::vector *costs) const -{ + std::vector *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& path_state_ids, - std::vector > *path_coordinates) const -{ - // YOUR CODE HERE + std::vector > *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 diff --git a/graph.h b/graph.h index 0d3b62d..4de6c34 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 @@ -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_ diff --git a/student_solutions/test_1.solution b/student_solutions/test_1.solution new file mode 100644 index 0000000..cd54402 --- /dev/null +++ b/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 diff --git a/student_solutions/test_2.solution b/student_solutions/test_2.solution new file mode 100644 index 0000000..f36abe9 --- /dev/null +++ b/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 diff --git a/student_solutions/test_3.solution b/student_solutions/test_3.solution new file mode 100644 index 0000000..6f82ffd --- /dev/null +++ b/student_solutions/test_3.solution @@ -0,0 +1,68 @@ +3 17 +3 18 +3 19 +3 20 +3 21 +3 22 +4 23 +4 24 +5 25 +6 26 +7 27 +8 28 +8 29 +8 30 +8 31 +8 32 +8 33 +8 34 +8 35 +8 36 +9 37 +10 38 +11 39 +12 40 +13 41 +14 42 +15 43 +16 44 +17 45 +18 46 +19 47 +20 48 +21 49 +22 49 +23 50 +23 51 +23 52 +23 53 +23 54 +23 55 +23 56 +24 57 +25 58 +26 59 +27 60 +28 61 +29 62 +30 63 +31 64 +32 65 +33 66 +34 67 +35 68 +36 69 +37 69 +38 69 +39 69 +40 69 +41 69 +42 70 +43 71 +44 72 +45 73 +46 74 +47 75 +48 76 +49 77 +50 78 diff --git a/student_solutions/test_4.solution b/student_solutions/test_4.solution new file mode 100644 index 0000000..ea85b9d --- /dev/null +++ b/student_solutions/test_4.solution @@ -0,0 +1,76 @@ +45 78 +45 77 +45 76 +45 75 +45 74 +45 73 +45 72 +44 71 +43 70 +43 69 +43 68 +43 67 +43 66 +43 65 +43 64 +43 63 +42 62 +42 61 +42 60 +41 59 +40 58 +39 57 +38 56 +37 55 +36 54 +35 53 +35 52 +35 51 +35 50 +35 49 +35 48 +35 47 +34 46 +33 45 +32 44 +31 43 +30 42 +29 41 +28 40 +27 39 +26 38 +25 37 +24 36 +23 35 +22 34 +21 34 +20 33 +20 32 +20 31 +20 30 +20 29 +20 28 +20 27 +20 26 +20 25 +19 24 +18 23 +17 22 +16 21 +15 20 +14 19 +13 18 +12 17 +11 16 +10 15 +9 14 +8 13 +7 12 +6 11 +5 10 +5 9 +4 8 +3 7 +2 7 +1 7 +0 7 diff --git a/student_solutions/test_5.solution b/student_solutions/test_5.solution new file mode 100644 index 0000000..d78f672 --- /dev/null +++ b/student_solutions/test_5.solution @@ -0,0 +1,404 @@ +10 70 +10 71 +10 72 +10 73 +10 74 +10 75 +10 76 +11 77 +12 78 +12 79 +12 80 +12 81 +12 82 +12 83 +12 84 +12 85 +13 86 +14 87 +15 88 +16 89 +17 90 +18 91 +19 92 +20 93 +21 94 +22 94 +23 95 +23 96 +23 97 +24 98 +25 99 +26 100 +27 101 +28 102 +29 103 +30 104 +31 105 +32 106 +33 107 +34 108 +35 109 +36 110 +37 111 +38 112 +38 113 +38 114 +38 115 +38 116 +38 117 +38 118 +38 119 +38 120 +38 121 +38 122 +38 123 +38 124 +38 125 +39 126 +40 127 +41 128 +42 129 +43 130 +44 131 +45 132 +46 133 +47 134 +48 135 +49 136 +50 137 +51 138 +52 138 +53 139 +53 140 +53 141 +53 142 +53 143 +53 144 +53 145 +54 146 +55 147 +56 148 +57 149 +58 150 +59 151 +60 152 +61 153 +62 154 +63 155 +64 156 +65 157 +66 158 +67 159 +68 160 +68 161 +69 162 +70 163 +71 164 +72 165 +73 166 +74 167 +75 168 +76 169 +77 170 +77 171 +77 172 +77 173 +77 174 +77 175 +77 176 +77 177 +77 178 +77 179 +77 180 +77 181 +77 182 +77 183 +77 184 +77 185 +77 186 +77 187 +77 188 +77 189 +77 190 +77 191 +77 192 +77 193 +77 194 +77 195 +77 196 +77 197 +77 198 +77 199 +77 200 +77 201 +77 202 +77 203 +77 204 +77 205 +77 206 +77 207 +77 208 +77 209 +77 210 +77 211 +77 212 +77 213 +77 214 +77 215 +77 216 +77 217 +77 218 +77 219 +77 220 +77 221 +77 222 +77 223 +77 224 +77 225 +77 226 +77 227 +77 228 +77 229 +77 230 +77 231 +77 232 +77 233 +77 234 +77 235 +77 236 +77 237 +77 238 +77 239 +77 240 +77 241 +77 242 +77 243 +77 244 +77 245 +77 246 +77 247 +77 248 +77 249 +77 250 +77 251 +77 252 +77 253 +77 254 +77 255 +77 256 +77 257 +77 258 +77 259 +77 260 +77 261 +77 262 +77 263 +77 264 +77 265 +77 266 +77 267 +77 268 +77 269 +77 270 +77 271 +77 272 +77 273 +77 274 +77 275 +77 276 +77 277 +77 278 +77 279 +77 280 +77 281 +77 282 +77 283 +77 284 +77 285 +77 286 +77 287 +77 288 +77 289 +77 290 +77 291 +77 292 +77 293 +77 294 +77 295 +77 296 +77 297 +77 298 +77 299 +77 300 +77 301 +77 302 +77 303 +77 304 +77 305 +77 306 +77 307 +77 308 +77 309 +77 310 +77 311 +77 312 +77 313 +77 314 +77 315 +77 316 +77 317 +77 318 +77 319 +77 320 +77 321 +77 322 +77 323 +77 324 +77 325 +77 326 +77 327 +77 328 +77 329 +77 330 +77 331 +77 332 +77 333 +77 334 +77 335 +77 336 +77 337 +77 338 +77 339 +77 340 +77 341 +77 342 +77 343 +77 344 +77 345 +77 346 +77 347 +77 348 +77 349 +77 350 +77 351 +77 352 +77 353 +77 354 +77 355 +77 356 +77 357 +77 358 +77 359 +77 360 +77 361 +77 362 +77 363 +77 364 +77 365 +77 366 +77 367 +77 368 +77 369 +77 370 +77 371 +77 372 +77 373 +77 374 +77 375 +77 376 +77 377 +77 378 +77 379 +77 380 +77 381 +77 382 +77 383 +77 384 +77 385 +77 386 +77 387 +77 388 +77 389 +77 390 +77 391 +77 392 +77 393 +77 394 +77 395 +77 396 +77 397 +77 398 +77 399 +77 400 +77 401 +77 402 +77 403 +77 404 +77 405 +77 406 +77 407 +77 408 +77 409 +77 410 +77 411 +77 412 +77 413 +77 414 +77 415 +77 416 +77 417 +77 418 +77 419 +77 420 +77 421 +77 422 +77 423 +77 424 +77 425 +77 426 +77 427 +77 428 +77 429 +77 430 +77 431 +77 432 +77 433 +77 434 +77 435 +77 436 +77 437 +77 438 +77 439 +77 440 +77 441 +77 442 +77 443 +77 444 +77 445 +77 446 +77 447 +77 448 +77 449 +77 450 +77 451 +77 452 +77 453 +77 454 +77 455 +77 456 +78 457 +79 458 +80 459 +81 460 +82 460 +83 461 +83 462 +83 463 +84 464 +85 465 +86 466 +87 467 +88 468 +89 469 +90 470 diff --git a/tags b/tags new file mode 100644 index 0000000..a3cc0f1 --- /dev/null +++ b/tags @@ -0,0 +1,52 @@ +!_TAG_FILE_FORMAT 2 /extended format; --format=1 will not append ;" to lines/ +!_TAG_FILE_SORTED 1 /0=unsorted, 1=sorted, 2=foldcase/ +!_TAG_PROGRAM_AUTHOR Darren Hiebert /dhiebert@users.sourceforge.net/ +!_TAG_PROGRAM_NAME Exuberant Ctags // +!_TAG_PROGRAM_URL http://ctags.sourceforge.net /official site/ +!_TAG_PROGRAM_VERSION 5.9~svn20110310 // +ChildToParentMap dijkstra.h /^typedef std::unordered_map ChildToParentMap;$/;" t namespace:grid_planner::planners +CostMap dijkstra.h /^typedef std::unordered_map CostMap;$/;" t namespace:grid_planner::planners +CostMapComparator dijkstra.h /^ explicit CostMapComparator(const CostMap& cost_map): cost_map_(cost_map) {}$/;" f class:grid_planner::planners::CostMapComparator +CostMapComparator dijkstra.h /^class CostMapComparator {$/;" c namespace:grid_planner::planners +DIJKSTRAS_H_ dijkstra.h 31;" d +Dijkstras dijkstra.h /^ Dijkstras($/;" f class:grid_planner::planners::Dijkstras +Dijkstras dijkstra.h /^class Dijkstras {$/;" c namespace:grid_planner::planners +GRAPH_H_ graph.h 31;" d +Graph graph.h /^class Graph {$/;" c namespace:grid_planner::graphs +cost_map_ dijkstra.h /^ const CostMap& cost_map_;$/;" m class:grid_planner::planners::CostMapComparator +extract_path dijkstra.cpp /^void Dijkstras::extract_path($/;" f class:grid_planner::planners::Dijkstras +get_action_cost graph.cpp /^double Graph::get_action_cost($/;" f class:grid_planner::graphs::Graph +get_coord_from_state_id graph.cpp /^bool Graph::get_coord_from_state_id(const int& state_id, int* x, int* y) const$/;" f class:grid_planner::graphs::Graph +get_map visualize.py /^def get_map(filename):$/;" f +get_path_coordinates graph.cpp /^void Graph::get_path_coordinates($/;" f class:grid_planner::graphs::Graph +get_state_id graph.cpp /^int Graph::get_state_id(const int& x, const int& y) const$/;" f class:grid_planner::graphs::Graph +get_succs graph.cpp /^void Graph::get_succs($/;" f class:grid_planner::graphs::Graph +graphs graph.cpp /^namespace graphs {$/;" n namespace:grid_planner file: +graphs graph.h /^namespace graphs {$/;" n namespace:grid_planner +grid_planner dijkstra.cpp /^namespace grid_planner {$/;" n file: +grid_planner dijkstra.h /^namespace grid_planner {$/;" n +grid_planner graph.cpp /^namespace grid_planner {$/;" n file: +grid_planner graph.h /^namespace grid_planner {$/;" n +is_valid_state graph.cpp /^bool Graph::is_valid_state(const int& x, const int& y) const$/;" f class:grid_planner::graphs::Graph +m_goal_id graph.h /^ int m_goal_id;$/;" m class:grid_planner::graphs::Graph +m_graph dijkstra.h /^ const graphs::Graph m_graph;$/;" m class:grid_planner::planners::Dijkstras +m_height graph.h /^ m_height(height) {}$/;" f class:grid_planner::graphs::Graph +m_height graph.h /^ const int m_height;$/;" m class:grid_planner::graphs::Graph +m_occupancy_grid graph.h /^ const std::vector m_occupancy_grid;$/;" m class:grid_planner::graphs::Graph +m_start_id graph.h /^ int m_start_id;$/;" m class:grid_planner::graphs::Graph +m_width graph.h /^ const int m_width;$/;" m class:grid_planner::graphs::Graph +main test_planner.cpp /^int main() {$/;" f +map_values visualize.py /^ map_values = get_map(sys.argv[1])$/;" v +operator () dijkstra.h /^ bool operator()(const int& state_1,$/;" f class:grid_planner::planners::CostMapComparator +planners dijkstra.cpp /^namespace planners {$/;" n namespace:grid_planner file: +planners dijkstra.h /^namespace planners {$/;" n namespace:grid_planner +plot_path visualize.py /^def plot_path(filename, ax):$/;" f +print_path test_planner.cpp /^void print_path(const std::vector>& path)$/;" f +read_environment test_planner.cpp /^void read_environment($/;" f +read_solution test_planner.cpp /^void read_solution($/;" f +run_planner dijkstra.cpp /^void Dijkstras::run_planner($/;" f class:grid_planner::planners::Dijkstras +set_goal_state graph.cpp /^int Graph::set_goal_state(const int& x, const int& y)$/;" f class:grid_planner::graphs::Graph +set_start_state graph.cpp /^int Graph::set_start_state(const int& x, const int& y)$/;" f class:grid_planner::graphs::Graph +write_solution test_planner.cpp /^void write_solution($/;" f +~Dijkstras dijkstra.h /^ ~Dijkstras() {};$/;" f class:grid_planner::planners::Dijkstras +~Graph graph.h /^ ~Graph() {};$/;" f class:grid_planner::graphs::Graph