From 25ae45ec0b1c50a139bb8583b71e2993e437348d Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Wed, 30 Jan 2019 17:10:00 -0800 Subject: [PATCH 01/54] added comment --- graph.h | 1 + 1 file changed, 1 insertion(+) diff --git a/graph.h b/graph.h index 0d3b62d..5531e09 100644 --- a/graph.h +++ b/graph.h @@ -97,6 +97,7 @@ class Graph { const int m_width; const int m_height; + // start and goal state ID int m_start_id; int m_goal_id; }; From d63c903cbbfe2098438e6098eb30362966e87b4f Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 12 Feb 2019 10:17:13 -0800 Subject: [PATCH 02/54] Modified methods in graph.cpp --- graph.cpp | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/graph.cpp b/graph.cpp index 9f6272b..c228fe7 100644 --- a/graph.cpp +++ b/graph.cpp @@ -37,12 +37,31 @@ namespace graphs { int Graph::set_start_state(const int& x, const int& y) { // YOUR CODE HERE +/* + m_start_id = get_state_id(x, y); + + if (is_valid_state(m_start_id)) { + return m_start_id; // valid state + } else { + return -1; // invalid state + } +*/ + return -1; } int Graph::set_goal_state(const int& x, const int& y) { // YOUR CODE HERE +/* + m_goal_state = get_state_id(x, y); + + if (is_valid_state(m_goal_state)) { + return m_goal_state; // valid state + } else { + return -1; // invalid state + } +*/ return -1; } @@ -54,6 +73,14 @@ void Graph::get_succs( assert(source_state_id < m_occupancy_grid.size()); // YOUR CODE HERE +/* // HOW DO I GET THE SUCCESSORS? + std::vector::iterator iter; + for (iter = (*succ_ids).begin(); iter != (*succ_ids).end(); iter++) { + if (is_valid_state(*iter)) { // successor is valid + + } + } +*/ } void Graph::get_path_coordinates( @@ -61,6 +88,15 @@ void Graph::get_path_coordinates( std::vector > *path_coordinates) const { // YOUR CODE HERE +/* + std::vector::iterator iter; + for (iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { + int x, y; + if (get_coord_from_state_id(*iter, &x, &y) { //IS *iter CORRECT HERE? + (*path_coordinates).push_back(x, y); // coordinates are valid + } + } +*/ } int Graph::get_state_id(const int& x, const int& y) const @@ -69,6 +105,9 @@ int Graph::get_state_id(const int& x, const int& y) const assert(y < m_height); // YOUR CODE HERE +/* + +*/ return 0; } @@ -77,12 +116,18 @@ 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; } bool Graph::is_valid_state(const int& x, const int& y) const { // YOUR CODE HERE +/* + +*/ return true; } @@ -93,6 +138,10 @@ double Graph::get_action_cost( const int& succ_y) const { // YOUR CODE HERE +/* + // Are we calculating distance b/w the 2 points? + return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); +*/ return 0; } From ced85e7395107506b99a9f19b70c4ae75a2e5352 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 12 Feb 2019 12:04:39 -0800 Subject: [PATCH 03/54] Modified comparator in dijkstra.h and methods in dijkstra.cpp --- dijkstra.cpp | 51 ++++++++++++++++++++++++++++++++++++++++++++++++++- dijkstra.h | 7 +++++++ 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 35e5e47..d20637d 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -41,16 +41,24 @@ void Dijkstras::run_planner( int* num_expansions, std::vector> *path) { - // Create priority queue; I suggest using a set with with the custom + // Create priority queue; I suggest using a set with the custom // comparator defined in dijkstra.h as your priority queue + std::set Q; // You will need to change this line +/* + std::set Q; +*/ + // While the queue is not empty while (!Q.empty()) { // Pop and expand the next node in the priority queue (*num_expansions)++; // YOUR CODE HERE +/* + +*/ } } @@ -61,6 +69,47 @@ void Dijkstras::extract_path( std::vector *path_state_ids) { // YOUR CODE HERE +/* + int x, y; + +//I think I have to get the path backwards (i.e. from goal to start) + + if (goal_id == start_id) { + return; + } + + if (get coord_from_state_id(goal_id, &x, &y) { + // push goal_id + //check if x and why represents valid state? + (*path_state_ids).push_back(goal_id); + } + + //ChildToParentMap is typedef for std::unordered_map! + //maps child state id to parent state id + // what is the child_to parent_map doing exactly? + ChildToParentMap::iterator iter = child_to_parent_map.find(goal_id); + + while (1) { + if (iter != child_to_parent_map.end()) { + // Should I remove the mapping? + std::string value = iter->second; + + // do I need to check if the state id is valid? + (*path_state_ids).push_back(value); + child_to_parent_map.erase(iter); // removes mapping from map if key is found + + // will this work for the case when the start and goal are the same? + + if (value == start_id) { + break; + } + } else { + break; + } + + iter = child_to_parent_map.find(value); + } +*/ } } diff --git a/dijkstra.h b/dijkstra.h index ec24453..ccab7f2 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -54,6 +54,13 @@ class CostMapComparator { // Given two states you need to write a comparator that determines // how to order them // YOUR CODE HERE (replace line below) + +/* // FUNCTOR: a class that overloads the () operator so that it can be called like a function! + // What is the logic for this? + double state_1_cost = Graph::get_action_cost( + double state_2_cost = + return state_1_cost > state_2_cost; +*/ return true; } From 210617df8b26f9688bf73d4e54dc48faa2402622 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 12 Feb 2019 15:56:40 -0800 Subject: [PATCH 04/54] blah --- .gitignore | 1 + dijkstra.cpp | 4 +++- graph.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 5 deletions(-) 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/dijkstra.cpp b/dijkstra.cpp index d20637d..06462fa 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -74,6 +74,8 @@ void Dijkstras::extract_path( //I think I have to get the path backwards (i.e. from goal to start) +// Can end if we have expanded the goal or we have nothing in our priority list. +/ if (goal_id == start_id) { return; } @@ -89,7 +91,7 @@ void Dijkstras::extract_path( // what is the child_to parent_map doing exactly? ChildToParentMap::iterator iter = child_to_parent_map.find(goal_id); - while (1) { + while (1) { // we can put end condition here!!!!! if (iter != child_to_parent_map.end()) { // Should I remove the mapping? std::string value = iter->second; diff --git a/graph.cpp b/graph.cpp index c228fe7..2982dc5 100644 --- a/graph.cpp +++ b/graph.cpp @@ -126,7 +126,8 @@ bool Graph::is_valid_state(const int& x, const int& y) const { // YOUR CODE HERE /* - +// check bounds (i.e. value is valid) +// and check occupancy grid */ return true; } @@ -138,11 +139,10 @@ double Graph::get_action_cost( const int& succ_y) const { // YOUR CODE HERE -/* + // Are we calculating distance b/w the 2 points? return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); -*/ - return 0; + // return 0; } } // namespace graphs From 84ff411c5bdb79cb2a174a11c995633ad2826de7 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 12 Feb 2019 16:46:09 -0800 Subject: [PATCH 05/54] changed comment --- dijkstra.cpp | 6 ++++-- dijkstra.h | 5 +++-- graph.cpp | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 06462fa..be35dad 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -74,8 +74,10 @@ void Dijkstras::extract_path( //I think I have to get the path backwards (i.e. from goal to start) -// Can end if we have expanded the goal or we have nothing in our priority list. -/ +// Can stop if we have expanded the goal or we have nothing in our priority list. + +//do find and check if value is end before loop! + if (goal_id == start_id) { return; } diff --git a/dijkstra.h b/dijkstra.h index ccab7f2..b4bb25c 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -54,12 +54,13 @@ class CostMapComparator { // Given two states you need to write a comparator that determines // how to order them // YOUR CODE HERE (replace line below) - + + // return does state 1 have lower cost than state 2? /* // FUNCTOR: a class that overloads the () operator so that it can be called like a function! // What is the logic for this? double state_1_cost = Graph::get_action_cost( double state_2_cost = - return state_1_cost > state_2_cost; + return state_1_cost <= state_2_cost; */ return true; } diff --git a/graph.cpp b/graph.cpp index 2982dc5..d6a489f 100644 --- a/graph.cpp +++ b/graph.cpp @@ -140,7 +140,7 @@ double Graph::get_action_cost( { // YOUR CODE HERE - // Are we calculating distance b/w the 2 points? + // Calculate Euclidean distance between the 2 points return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); // return 0; } From 30ab40cff29a46d65ac4bad339aa2931580e03c8 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Sat, 23 Feb 2019 11:25:28 -0800 Subject: [PATCH 06/54] Modified methods --- dijkstra.cpp | 23 ++++++++----- dijkstra.h | 22 +++++++++---- graph.cpp | 91 ++++++++++++++++++++++++++++++++++++++++------------ 3 files changed, 102 insertions(+), 34 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index be35dad..7b81b9e 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -44,11 +44,11 @@ void Dijkstras::run_planner( // Create priority queue; I suggest using a set with the custom // comparator defined in dijkstra.h as your priority queue - std::set Q; // You will need to change this line +// std::set Q; // You will need to change this line -/* +// MY CODE BEGINS std::set Q; -*/ +// MY CODE ENDS // While the queue is not empty while (!Q.empty()) { @@ -56,9 +56,8 @@ void Dijkstras::run_planner( (*num_expansions)++; // YOUR CODE HERE -/* -*/ + // MY CODE ENDS } } @@ -69,7 +68,7 @@ void Dijkstras::extract_path( std::vector *path_state_ids) { // YOUR CODE HERE -/* + int x, y; //I think I have to get the path backwards (i.e. from goal to start) @@ -83,8 +82,8 @@ void Dijkstras::extract_path( } if (get coord_from_state_id(goal_id, &x, &y) { + // check if x and y represent valid state // push goal_id - //check if x and why represents valid state? (*path_state_ids).push_back(goal_id); } @@ -92,13 +91,20 @@ void Dijkstras::extract_path( //maps child state id to parent state id // what is the child_to parent_map doing exactly? ChildToParentMap::iterator iter = child_to_parent_map.find(goal_id); + (*path_state_ids).push_back(goal_id); + + // loop till we find start or we reach end of map + while (iter->second != start_id && iter != child_to_parent_map.end()) { + (*path_state_ids).push_back(iter->second); + iter = child_to_parent_map.find(iter->second); + } +/* OLD CODE while (1) { // we can put end condition here!!!!! if (iter != child_to_parent_map.end()) { // Should I remove the mapping? std::string value = iter->second; - // do I need to check if the state id is valid? (*path_state_ids).push_back(value); child_to_parent_map.erase(iter); // removes mapping from map if key is found @@ -114,6 +120,7 @@ void Dijkstras::extract_path( iter = child_to_parent_map.find(value); } */ + // END OF MY CODE } } diff --git a/dijkstra.h b/dijkstra.h index b4bb25c..ee86924 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -53,15 +53,25 @@ 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 does state 1 have lower cost than state 2? -/* // FUNCTOR: a class that overloads the () operator so that it can be called like a function! - // What is the logic for this? - double state_1_cost = Graph::get_action_cost( - double state_2_cost = - return state_1_cost <= state_2_cost; -*/ + // FUNCTOR: a class that overloads the () operator so that it can be called like a function! + + int source_x, source_y, succ_x, succ_y; + Graph::get_coord_from_state_id(start_id, &source_x, &source_y); + + Graph::get_coord_from_state_id(state_1, &succ_x, &succ_y); + double state_1_cost = Graph::get_action_cost(source_x, source_y, succ_x, succ_y); + + Graph::get_coord_from_state_id(state_2, &succ_x, &succ_y); + double state_2_cost = Graph::get_action_cost(source_x, source_y, succ_x, succ_y); + + return state_1_cost <= state_2_cost; + + // END OF MY CODE + return true; } diff --git a/graph.cpp b/graph.cpp index d6a489f..083cb24 100644 --- a/graph.cpp +++ b/graph.cpp @@ -37,7 +37,7 @@ namespace graphs { int Graph::set_start_state(const int& x, const int& y) { // YOUR CODE HERE -/* + m_start_id = get_state_id(x, y); if (is_valid_state(m_start_id)) { @@ -45,15 +45,16 @@ int Graph::set_start_state(const int& x, const int& y) } else { return -1; // invalid state } -*/ - return -1; + // END OF MY CODE + +// return -1; } int Graph::set_goal_state(const int& x, const int& y) { // YOUR CODE HERE -/* + m_goal_state = get_state_id(x, y); if (is_valid_state(m_goal_state)) { @@ -61,8 +62,10 @@ int Graph::set_goal_state(const int& x, const int& y) } else { return -1; // invalid state } -*/ - return -1; + + // END OF MY CODE + +// return -1; } void Graph::get_succs( @@ -73,14 +76,41 @@ void Graph::get_succs( assert(source_state_id < m_occupancy_grid.size()); // YOUR CODE HERE -/* // HOW DO I GET THE SUCCESSORS? + std::vector::iterator iter; for (iter = (*succ_ids).begin(); iter != (*succ_ids).end(); iter++) { if (is_valid_state(*iter)) { // successor is valid } } -*/ + + int x_source, y_source; + get_coord_from_state_id(source_state_id, x_source, y_source); + int x_succ, y_succ; + int succ_state_id; + double succ_cost; + + 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; + } + + x_succ = x_source + i; + y_succ = y_source + i; + succ_state_id = get_state_id(x_succ, y_succ); + + if (!is_valid_state(suc_state_id)) { + continue; // successor is not valid + } else { + (*succ_ids).pushback(succ_state_id); + succ_cost = get_action_cost(x_source, y_source); + (*costs).pushback(succ_cost); + } + } + } + + // END OF MY CODE } void Graph::get_path_coordinates( @@ -88,15 +118,17 @@ void Graph::get_path_coordinates( std::vector > *path_coordinates) const { // YOUR CODE HERE -/* + std::vector::iterator iter; for (iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { int x, y; - if (get_coord_from_state_id(*iter, &x, &y) { //IS *iter CORRECT HERE? + if (get_coord_from_state_id(*iter, &x, &y) { (*path_coordinates).push_back(x, y); // coordinates are valid } } -*/ + + // END OF MY CODE + } int Graph::get_state_id(const int& x, const int& y) const @@ -105,9 +137,11 @@ int Graph::get_state_id(const int& x, const int& y) const assert(y < m_height); // YOUR CODE HERE -/* - -*/ + + return x + y * m_width; + + // END OF MY CODE + return 0; } @@ -116,19 +150,34 @@ 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 -/* + + *y = state_id / m_width; + *x = state_id - *y * m_width -*/ + return is_valid_state(*x, *y); + + // END OF MY CODE + return true; } bool Graph::is_valid_state(const int& x, const int& y) const { // YOUR CODE HERE -/* -// check bounds (i.e. value is valid) -// and check occupancy grid -*/ + + // check bounds (i.e. value is valid) + (!(*x >= 0 && *x < m_width && *y >= 0 && *y < m_height)) { + return false; + } else { // check occupancy grid to see if cell is free + if (occupancy_grid[get_state_id(x, y)] == 0) { + return true; + } else { + return false; + } + } + + // END OF MY CODE + return true; } @@ -142,6 +191,8 @@ double Graph::get_action_cost( // Calculate Euclidean distance between the 2 points return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); + + // END OF MY CODE // return 0; } From 9f67a0801913e0a2a6a986e604ef217888070ab4 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Sat, 23 Feb 2019 12:36:05 -0800 Subject: [PATCH 07/54] Modified methods --- dijkstra.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 7b81b9e..4577083 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -56,7 +56,10 @@ void Dijkstras::run_planner( (*num_expansions)++; // YOUR CODE HERE + +// we can stop if we have expanded the goal or we have nothing in our priority queue + // MY CODE ENDS } } @@ -81,7 +84,7 @@ void Dijkstras::extract_path( return; } - if (get coord_from_state_id(goal_id, &x, &y) { + if (get coord_from_state_id(goal_id, &x, &y)) { // check if x and y represent valid state // push goal_id (*path_state_ids).push_back(goal_id); From bed0f76b079ab7f0491d4b6572cb6d4864a9a60b Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Sat, 23 Feb 2019 16:06:56 -0800 Subject: [PATCH 08/54] Modified comparator --- dijkstra.cpp | 29 ++++++++++++++++++++++++++--- dijkstra.h | 11 +---------- graph.cpp | 4 ++-- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 4577083..2c94aa9 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -49,17 +49,40 @@ void Dijkstras::run_planner( // MY CODE BEGINS std::set Q; // MY CODE ENDS - +/* // While the queue is not empty while (!Q.empty()) { // Pop and expand the next node in the priority queue (*num_expansions)++; // YOUR CODE HERE - + check if node popped is goal + call extract_path + break + + popped node which has optimal cost + + call get successors function + + //close list has nodes with optimal cost + // open list has everything that in in priority queue, and nodes that haven't been seen yet + + // check if sucessor node is in the priority queue + // if its not in the priority queue, we need to add it + // find cost from parent to successor by calling get_action _cost + // and add this to parent's cost to get g value + // + // else + // find cost here too + // + // update cost to optimal cost in priority queue + // + // + //at end of while loop, call extract path + // we can stop if we have expanded the goal or we have nothing in our priority queue - +*/ // MY CODE ENDS } } diff --git a/dijkstra.h b/dijkstra.h index ee86924..3f9a244 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -59,16 +59,7 @@ class CostMapComparator { // return does state 1 have lower cost than state 2? // FUNCTOR: a class that overloads the () operator so that it can be called like a function! - int source_x, source_y, succ_x, succ_y; - Graph::get_coord_from_state_id(start_id, &source_x, &source_y); - - Graph::get_coord_from_state_id(state_1, &succ_x, &succ_y); - double state_1_cost = Graph::get_action_cost(source_x, source_y, succ_x, succ_y); - - Graph::get_coord_from_state_id(state_2, &succ_x, &succ_y); - double state_2_cost = Graph::get_action_cost(source_x, source_y, succ_x, succ_y); - - return state_1_cost <= state_2_cost; + return cost_map_[state_1] <= cost_map_[state_2]; // END OF MY CODE diff --git a/graph.cpp b/graph.cpp index 083cb24..154182e 100644 --- a/graph.cpp +++ b/graph.cpp @@ -76,14 +76,14 @@ void Graph::get_succs( assert(source_state_id < m_occupancy_grid.size()); // YOUR CODE HERE - +/* std::vector::iterator iter; for (iter = (*succ_ids).begin(); iter != (*succ_ids).end(); iter++) { if (is_valid_state(*iter)) { // successor is valid } } - +*/ int x_source, y_source; get_coord_from_state_id(source_state_id, x_source, y_source); int x_succ, y_succ; From 3e07d4bed79f6a0286f7d38b744be527bb7abd39 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Sat, 23 Feb 2019 17:21:52 -0800 Subject: [PATCH 09/54] Modified run_planner --- dijkstra.cpp | 61 +++++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 56 insertions(+), 5 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 2c94aa9..fbdc4b0 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -47,20 +47,71 @@ void Dijkstras::run_planner( // std::set Q; // You will need to change this line // MY CODE BEGINS + // set of state IDs ordered by cost (rather then sorted in order of integers) std::set Q; + + std::vector path_state_ids; //close list? // MY CODE ENDS -/* + // 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 auto iterator node = Q.begin(); + + if (*node == goal_id) { + extract_path(ChildToParentMap, start_id, goal_id, &path_state_ids); + // set path parameter? + break; + } + + std::vector succ_ids; + std::vector costs; + get_succ(start_id, &succ_ids, &costs); + auto iterStateID = succ_ids.begin(); + auto iterCosts = costs.begin(); + while (iterStateID != succ_ids.end() && iterCosts != succ_ids.end()) { + // find cost + int x_parent, y_parent, x_succ, y_succ; + get_coord_from_state_id(*node, &x_parent, &y_parent); //gets parents coordinates + get_coord_from_state_id(*iterStateID, &x_succ, &y_succ); //gets succ coordinates + double gValue = get_action_cost(x_parent, y_parent, x_succ, y_succ); //finds cost from parent to successor + gValue += cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value + + // if node is not in the priority queue, we need to add it! + if (Q.find(node) == Q.end()) { + Q.insert(node); + } else { + // node is in priority queue, but update it to have optimal cost + // For Dikstras, optimal cost is the least cost + if (gValue < cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { + // update cost map? + cost_map_[node] = gValue; + } + } + } + } + + // if Q is empty, we need to call extract_path too + if (Q.empty()) { + extract_path(ChildToParentMap, start_id, goal_id, &path_state_ids); + } + + // set path parameter + for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); ++iter) { + int x, y; + get_coord_from_state_id(*iter, &x, &y); + path.pushback(std::make_pair(x, y)); + } + +/* check if node popped is goal call extract_path break - popped node which has optimal cost + //popped node has optimal cost call get successors function @@ -81,10 +132,10 @@ void Dijkstras::run_planner( //at end of while loop, call extract path // we can stop if we have expanded the goal or we have nothing in our priority queue - -*/ + */ + // MY CODE ENDS - } + } void Dijkstras::extract_path( From df0685f88689704bd3712658c99fea2cbc851bb7 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Sat, 2 Mar 2019 21:07:14 -0800 Subject: [PATCH 10/54] Added errors.txt which contains make errors --- errors.txt | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 errors.txt diff --git a/errors.txt b/errors.txt new file mode 100644 index 0000000..8ae2a66 --- /dev/null +++ b/errors.txt @@ -0,0 +1,2 @@ +g++ -std=c++11 -c test_planner.cpp graph.h graph.cpp dijkstra.h dijkstra.cpp +Makefile:2: recipe for target 'all' failed From 355ea19a142932728b62cc10cf10c85eba8c376d Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Sat, 2 Mar 2019 21:20:30 -0800 Subject: [PATCH 11/54] Fixed bug in dijkstra.h --- dijkstra.cpp | 12 ++++++------ dijkstra.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index fbdc4b0..a841f69 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -45,19 +45,19 @@ void Dijkstras::run_planner( // comparator defined in dijkstra.h as your priority queue // std::set Q; // You will need to change this line - +/* // MY CODE BEGINS // set of state IDs ordered by cost (rather then sorted in order of integers) std::set Q; std::vector path_state_ids; //close list? // MY CODE ENDS - +*/ // 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 auto iterator node = Q.begin(); @@ -105,7 +105,7 @@ void Dijkstras::run_planner( get_coord_from_state_id(*iter, &x, &y); path.pushback(std::make_pair(x, y)); } - +*/ /* check if node popped is goal call extract_path @@ -144,7 +144,7 @@ void Dijkstras::extract_path( const int& goal_id, std::vector *path_state_ids) { - // YOUR CODE HERE + /* // YOUR CODE HERE int x, y; @@ -175,7 +175,7 @@ void Dijkstras::extract_path( (*path_state_ids).push_back(iter->second); iter = child_to_parent_map.find(iter->second); } - +*/ /* OLD CODE while (1) { // we can put end condition here!!!!! if (iter != child_to_parent_map.end()) { diff --git a/dijkstra.h b/dijkstra.h index 3f9a244..f506214 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -59,7 +59,7 @@ class CostMapComparator { // return does state 1 have lower cost than state 2? // FUNCTOR: a class that overloads the () operator so that it can be called like a function! - return cost_map_[state_1] <= cost_map_[state_2]; + return cost_map_.find(state_1) <= cost_map_(state_2); // END OF MY CODE From 2ac9f355b149d9fd3c64daadd504ab845682c5fe Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:04:58 -0800 Subject: [PATCH 12/54] Squashed bug in comparator --- dijkstra.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/dijkstra.h b/dijkstra.h index f506214..025fe5c 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -58,12 +58,14 @@ class CostMapComparator { // return does state 1 have lower cost than state 2? // FUNCTOR: a class that overloads the () operator so that it can be called like a function! - - return cost_map_.find(state_1) <= cost_map_(state_2); + const auto state_1Iter = cost_map_.find(state_1); + const auto state_2Iter = cost_map_.find(state_2); + + return state_1Iter->second <= state_2Iter->second; // END OF MY CODE - return true; + // return true; } private: From 34c091910281976bd45cc06141d1b2be943dd71d Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:09:51 -0800 Subject: [PATCH 13/54] Fixed bug --- graph.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/graph.cpp b/graph.cpp index 154182e..2a94123 100644 --- a/graph.cpp +++ b/graph.cpp @@ -40,7 +40,7 @@ int Graph::set_start_state(const int& x, const int& y) m_start_id = get_state_id(x, y); - if (is_valid_state(m_start_id)) { + if (is_valid_state(x, y)) { return m_start_id; // valid state } else { return -1; // invalid state @@ -55,10 +55,10 @@ int Graph::set_goal_state(const int& x, const int& y) { // YOUR CODE HERE - m_goal_state = get_state_id(x, y); + m_goal_id = get_state_id(x, y); - if (is_valid_state(m_goal_state)) { - return m_goal_state; // valid state + if (is_valid_state(x, y)) { + return m_goal_id; // valid state } else { return -1; // invalid state } From 2d2be3aaee614e008b35d1e86236b917462981b6 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:15:05 -0800 Subject: [PATCH 14/54] Squashed more bugs --- graph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/graph.cpp b/graph.cpp index 2a94123..b8ca0ba 100644 --- a/graph.cpp +++ b/graph.cpp @@ -85,7 +85,7 @@ void Graph::get_succs( } */ int x_source, y_source; - get_coord_from_state_id(source_state_id, x_source, y_source); + get_coord_from_state_id(source_state_id, &x_source, &y_source); int x_succ, y_succ; int succ_state_id; double succ_cost; @@ -100,7 +100,7 @@ void Graph::get_succs( y_succ = y_source + i; succ_state_id = get_state_id(x_succ, y_succ); - if (!is_valid_state(suc_state_id)) { + if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid } else { (*succ_ids).pushback(succ_state_id); From fddae778f2c774bf00f9ea984681059233c1c118 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:20:42 -0800 Subject: [PATCH 15/54] Fixed bug --- graph.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/graph.cpp b/graph.cpp index b8ca0ba..4c35ff5 100644 --- a/graph.cpp +++ b/graph.cpp @@ -103,9 +103,9 @@ void Graph::get_succs( if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid } else { - (*succ_ids).pushback(succ_state_id); - succ_cost = get_action_cost(x_source, y_source); - (*costs).pushback(succ_cost); + (*succ_ids).push_back(succ_state_id); + succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); + (*costs).push_back(succ_cost); } } } @@ -119,8 +119,7 @@ void Graph::get_path_coordinates( { // YOUR CODE HERE - std::vector::iterator iter; - for (iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { + for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { int x, y; if (get_coord_from_state_id(*iter, &x, &y) { (*path_coordinates).push_back(x, y); // coordinates are valid From 40cd01877830fe4dbb87927efaa00bdc5e3974e1 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:23:56 -0800 Subject: [PATCH 16/54] Squashed bugs --- graph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/graph.cpp b/graph.cpp index 4c35ff5..1056d3b 100644 --- a/graph.cpp +++ b/graph.cpp @@ -121,7 +121,7 @@ void Graph::get_path_coordinates( for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { int x, y; - if (get_coord_from_state_id(*iter, &x, &y) { + if (get_coord_from_state_id(*iter, &x, &y)) { (*path_coordinates).push_back(x, y); // coordinates are valid } } @@ -151,7 +151,7 @@ bool Graph::get_coord_from_state_id(const int& state_id, int* x, int* y) const // YOUR CODE HERE *y = state_id / m_width; - *x = state_id - *y * m_width + *x = state_id - *y * m_width; return is_valid_state(*x, *y); @@ -165,7 +165,7 @@ bool Graph::is_valid_state(const int& x, const int& y) const // YOUR CODE HERE // check bounds (i.e. value is valid) - (!(*x >= 0 && *x < m_width && *y >= 0 && *y < m_height)) { + if (!(*x >= 0 && *x < m_width && *y >= 0 && *y < m_height)) { return false; } else { // check occupancy grid to see if cell is free if (occupancy_grid[get_state_id(x, y)] == 0) { From 5683deb0ad2d630a8172ef98f227324c38cc50ed Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:31:14 -0800 Subject: [PATCH 17/54] Squashed bugs --- graph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/graph.cpp b/graph.cpp index 1056d3b..3f00e46 100644 --- a/graph.cpp +++ b/graph.cpp @@ -122,7 +122,7 @@ void Graph::get_path_coordinates( for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { int x, y; if (get_coord_from_state_id(*iter, &x, &y)) { - (*path_coordinates).push_back(x, y); // coordinates are valid + (*path_coordinates).push_back(std::make_pair(x, y)); // coordinates are valid } } @@ -165,10 +165,10 @@ bool Graph::is_valid_state(const int& x, const int& y) const // YOUR CODE HERE // check bounds (i.e. value is valid) - if (!(*x >= 0 && *x < m_width && *y >= 0 && *y < m_height)) { + if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) { return false; } else { // check occupancy grid to see if cell is free - if (occupancy_grid[get_state_id(x, y)] == 0) { + if (m_occupancy_grid[get_state_id(x, y)] == 0) { return true; } else { return false; From 0886980fe483cfb028ae1b1ca29d12af14288a9f Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:34:42 -0800 Subject: [PATCH 18/54] Squashed bugs --- dijkstra.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index a841f69..c49b666 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -45,14 +45,14 @@ void Dijkstras::run_planner( // comparator defined in dijkstra.h as your priority queue // std::set Q; // You will need to change this line -/* + // MY CODE BEGINS // set of state IDs ordered by cost (rather then sorted in order of integers) std::set Q; std::vector path_state_ids; //close list? // MY CODE ENDS -*/ + // While the queue is not empty while (!Q.empty()) { // Pop and expand the next node in the priority queue @@ -144,7 +144,7 @@ void Dijkstras::extract_path( const int& goal_id, std::vector *path_state_ids) { - /* // YOUR CODE HERE +// YOUR CODE HERE int x, y; @@ -175,7 +175,7 @@ void Dijkstras::extract_path( (*path_state_ids).push_back(iter->second); iter = child_to_parent_map.find(iter->second); } -*/ + /* OLD CODE while (1) { // we can put end condition here!!!!! if (iter != child_to_parent_map.end()) { From b808be0c5c4090fdc7e0775b88b61023be9d513c Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:38:49 -0800 Subject: [PATCH 19/54] Squashed bugs --- dijkstra.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index c49b666..f50a8ba 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -198,7 +198,5 @@ void Dijkstras::extract_path( } */ // END OF MY CODE -} - -} + } } From 498d76b852ddbfff528d4323a4b10f0d14de292d Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 15:46:56 -0800 Subject: [PATCH 20/54] Squashed bugs --- dijkstra.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index f50a8ba..760bc0e 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -57,7 +57,7 @@ void Dijkstras::run_planner( while (!Q.empty()) { // Pop and expand the next node in the priority queue (*num_expansions)++; -/* + // YOUR CODE HERE const auto iterator node = Q.begin(); @@ -105,7 +105,7 @@ void Dijkstras::run_planner( get_coord_from_state_id(*iter, &x, &y); path.pushback(std::make_pair(x, y)); } -*/ + /* check if node popped is goal call extract_path @@ -200,3 +200,6 @@ void Dijkstras::extract_path( // END OF MY CODE } } + +} +} From 4f53e62c88d23c42ee0596b9b623677fc4294aa5 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:07:04 -0800 Subject: [PATCH 21/54] Squashed bugs --- dijkstra.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 760bc0e..2459fa8 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -51,6 +51,8 @@ void Dijkstras::run_planner( std::set Q; std::vector path_state_ids; //close list? + + ChildToParentMap child_to_parent_map; // empty map // MY CODE ENDS // While the queue is not empty @@ -59,10 +61,10 @@ void Dijkstras::run_planner( (*num_expansions)++; // YOUR CODE HERE - const auto iterator node = Q.begin(); + const auto node = Q.begin(); if (*node == goal_id) { - extract_path(ChildToParentMap, start_id, goal_id, &path_state_ids); + extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); // set path parameter? break; } @@ -77,6 +79,8 @@ void Dijkstras::run_planner( int x_parent, y_parent, x_succ, y_succ; get_coord_from_state_id(*node, &x_parent, &y_parent); //gets parents coordinates get_coord_from_state_id(*iterStateID, &x_succ, &y_succ); //gets succ coordinates + child_to_parent_map.insert(*iterStateID, *node); + double gValue = get_action_cost(x_parent, y_parent, x_succ, y_succ); //finds cost from parent to successor gValue += cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value @@ -96,7 +100,7 @@ void Dijkstras::run_planner( // if Q is empty, we need to call extract_path too if (Q.empty()) { - extract_path(ChildToParentMap, start_id, goal_id, &path_state_ids); + extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); } // set path parameter From 20a554a93fe214adc95d4092f203b06a3c351d4a Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:16:48 -0800 Subject: [PATCH 22/54] Squashed bugs --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 2459fa8..c08ed91 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -71,7 +71,7 @@ void Dijkstras::run_planner( std::vector succ_ids; std::vector costs; - get_succ(start_id, &succ_ids, &costs); + m_graph.get_succ(start_id, &succ_ids, &costs); auto iterStateID = succ_ids.begin(); auto iterCosts = costs.begin(); while (iterStateID != succ_ids.end() && iterCosts != succ_ids.end()) { From 1a33307feb8806ebe4bf5522708d8b1e3fb492e8 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:19:08 -0800 Subject: [PATCH 23/54] Bug --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index c08ed91..667ee5b 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -71,7 +71,7 @@ void Dijkstras::run_planner( std::vector succ_ids; std::vector costs; - m_graph.get_succ(start_id, &succ_ids, &costs); + m_graph.get_succs(start_id, &succ_ids, &costs); auto iterStateID = succ_ids.begin(); auto iterCosts = costs.begin(); while (iterStateID != succ_ids.end() && iterCosts != succ_ids.end()) { From 985061aec7cf0302dac1d412c792d78f646c627a Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:22:21 -0800 Subject: [PATCH 24/54] bug --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 667ee5b..1f3d650 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -74,7 +74,7 @@ void Dijkstras::run_planner( m_graph.get_succs(start_id, &succ_ids, &costs); auto iterStateID = succ_ids.begin(); auto iterCosts = costs.begin(); - while (iterStateID != succ_ids.end() && iterCosts != succ_ids.end()) { + while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { // find cost int x_parent, y_parent, x_succ, y_succ; get_coord_from_state_id(*node, &x_parent, &y_parent); //gets parents coordinates From 6e017beb5316396628fe330e63c557f66a1b03ed Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:25:23 -0800 Subject: [PATCH 25/54] bug --- dijkstra.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 1f3d650..1fbe041 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -77,11 +77,11 @@ void Dijkstras::run_planner( while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { // find cost int x_parent, y_parent, x_succ, y_succ; - get_coord_from_state_id(*node, &x_parent, &y_parent); //gets parents coordinates - get_coord_from_state_id(*iterStateID, &x_succ, &y_succ); //gets succ coordinates + m_graph.get_coord_from_state_id(*node, &x_parent, &y_parent); //gets parents coordinates + m_graph.get_coord_from_state_id(*iterStateID, &x_succ, &y_succ); //gets succ coordinates child_to_parent_map.insert(*iterStateID, *node); - double gValue = get_action_cost(x_parent, y_parent, x_succ, y_succ); //finds cost from parent to successor + double gValue = m_graph.get_action_cost(x_parent, y_parent, x_succ, y_succ); //finds cost from parent to successor gValue += cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value // if node is not in the priority queue, we need to add it! From 3d93d1843200bcf5f79b3021e9f898c7a8c58ea2 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:31:51 -0800 Subject: [PATCH 26/54] bug --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 1fbe041..e53f37f 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -81,7 +81,7 @@ void Dijkstras::run_planner( m_graph.get_coord_from_state_id(*iterStateID, &x_succ, &y_succ); //gets succ coordinates child_to_parent_map.insert(*iterStateID, *node); - double gValue = m_graph.get_action_cost(x_parent, y_parent, x_succ, y_succ); //finds cost from parent to successor + double gValue = *iterCosts; //finds cost from parent to successor gValue += cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value // if node is not in the priority queue, we need to add it! From ecbd953debdc73ff8f3c5f35b3f1dc662461d60c Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:33:39 -0800 Subject: [PATCH 27/54] bug --- dijkstra.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index e53f37f..a5c8cdd 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -75,10 +75,6 @@ void Dijkstras::run_planner( auto iterStateID = succ_ids.begin(); auto iterCosts = costs.begin(); while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { - // find cost - int x_parent, y_parent, x_succ, y_succ; - m_graph.get_coord_from_state_id(*node, &x_parent, &y_parent); //gets parents coordinates - m_graph.get_coord_from_state_id(*iterStateID, &x_succ, &y_succ); //gets succ coordinates child_to_parent_map.insert(*iterStateID, *node); double gValue = *iterCosts; //finds cost from parent to successor From 1aebbcd4479e6ffc66bf9da65dc46bbb7345e9df Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 16:39:36 -0800 Subject: [PATCH 28/54] bug --- dijkstra.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index a5c8cdd..9cc3e8a 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -78,7 +78,7 @@ void Dijkstras::run_planner( child_to_parent_map.insert(*iterStateID, *node); double gValue = *iterCosts; //finds cost from parent to successor - gValue += cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value + gValue += node->second; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value // if node is not in the priority queue, we need to add it! if (Q.find(node) == Q.end()) { @@ -86,9 +86,9 @@ void Dijkstras::run_planner( } else { // node is in priority queue, but update it to have optimal cost // For Dikstras, optimal cost is the least cost - if (gValue < cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { + if (gValue < node->second) { //cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { // update cost map? - cost_map_[node] = gValue; + node->second = gValue; // cost_map_[node] = gValue; } } } From 08fd0ae0c1f224d9ad5254b256acc1d689e7c9f3 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:01:26 -0800 Subject: [PATCH 29/54] bug --- dijkstra.cpp | 10 ++++++---- graph.cpp | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 9cc3e8a..4c6ea9e 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -48,7 +48,9 @@ void Dijkstras::run_planner( // MY CODE BEGINS // set of state IDs ordered by cost (rather then sorted in order of integers) - std::set Q; + CostMap cost_map; + CostMapComparator cost_map_comparator(cost_map); + std::set Q(cost_map_comparator); std::vector path_state_ids; //close list? @@ -78,7 +80,7 @@ void Dijkstras::run_planner( child_to_parent_map.insert(*iterStateID, *node); double gValue = *iterCosts; //finds cost from parent to successor - gValue += node->second; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value + gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value // if node is not in the priority queue, we need to add it! if (Q.find(node) == Q.end()) { @@ -86,9 +88,9 @@ void Dijkstras::run_planner( } else { // node is in priority queue, but update it to have optimal cost // For Dikstras, optimal cost is the least cost - if (gValue < node->second) { //cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { + if (gValue < cost_map[*node]) { //cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { // update cost map? - node->second = gValue; // cost_map_[node] = gValue; + cost_map[*node] = gValue; // cost_map_[node] = gValue; } } } diff --git a/graph.cpp b/graph.cpp index 3f00e46..fed06ef 100644 --- a/graph.cpp +++ b/graph.cpp @@ -104,6 +104,7 @@ void Graph::get_succs( continue; // successor is not valid } else { (*succ_ids).push_back(succ_state_id); + // transition cost i.e. cost from parent to successor succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); (*costs).push_back(succ_cost); } From 1683c90f41ff1325c27122cd484e9b0f54e03423 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:04:10 -0800 Subject: [PATCH 30/54] bug --- dijkstra.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 4c6ea9e..192dd0c 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -83,8 +83,8 @@ void Dijkstras::run_planner( gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value // if node is not in the priority queue, we need to add it! - if (Q.find(node) == Q.end()) { - Q.insert(node); + if (Q.find(*node) == Q.end()) { + Q.insert(*node); } else { // node is in priority queue, but update it to have optimal cost // For Dikstras, optimal cost is the least cost From 99221da9f0fee13b6a02ccdb085ca5d93e546a39 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:16:11 -0800 Subject: [PATCH 31/54] bug --- dijkstra.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 192dd0c..33b8fee 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -102,11 +102,7 @@ void Dijkstras::run_planner( } // set path parameter - for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); ++iter) { - int x, y; - get_coord_from_state_id(*iter, &x, &y); - path.pushback(std::make_pair(x, y)); - } + m_graph.get_path_coordinates(path_state_ids, &path); /* check if node popped is goal @@ -160,11 +156,7 @@ void Dijkstras::extract_path( return; } - if (get coord_from_state_id(goal_id, &x, &y)) { - // check if x and y represent valid state - // push goal_id - (*path_state_ids).push_back(goal_id); - } + (*path_state_ids).push_back(goal_id); //ChildToParentMap is typedef for std::unordered_map! //maps child state id to parent state id From db1ca17110a75d9fa6f198299fbb6c6f2a3ed7a5 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:19:07 -0800 Subject: [PATCH 32/54] bug --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 33b8fee..c80da75 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -102,7 +102,7 @@ void Dijkstras::run_planner( } // set path parameter - m_graph.get_path_coordinates(path_state_ids, &path); + m_graph.get_path_coordinates(path_state_ids, path); /* check if node popped is goal From d26e68e7ed532175dde2d745adcc250f1065b78f Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:22:59 -0800 Subject: [PATCH 33/54] bug --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index c80da75..a6d3fb3 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -161,7 +161,7 @@ void Dijkstras::extract_path( //ChildToParentMap is typedef for std::unordered_map! //maps child state id to parent state id // what is the child_to parent_map doing exactly? - ChildToParentMap::iterator iter = child_to_parent_map.find(goal_id); + auto iter = child_to_parent_map.find(goal_id); (*path_state_ids).push_back(goal_id); // loop till we find start or we reach end of map From 5a4ff5c51a6c64de576170484909f971ad25af23 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:28:03 -0800 Subject: [PATCH 34/54] bug --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index a6d3fb3..a6818f2 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -196,4 +196,4 @@ void Dijkstras::extract_path( } } -} + From 5987f482b349598d10f968017d9e12eab2c70d72 Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:33:27 -0800 Subject: [PATCH 35/54] used [] instead of insert for map --- dijkstra.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index a6818f2..87d6f5b 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -77,7 +77,7 @@ void Dijkstras::run_planner( auto iterStateID = succ_ids.begin(); auto iterCosts = costs.begin(); while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { - child_to_parent_map.insert(*iterStateID, *node); + child_to_parent_map[*iterStateID] = *node; double gValue = *iterCosts; //finds cost from parent to successor gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value From 8aacb0ce9f68d88b092e71aadce15a3164efa67e Mon Sep 17 00:00:00 2001 From: Ramya Ravichandran Asha Date: Tue, 5 Mar 2019 17:39:47 -0800 Subject: [PATCH 36/54] added getchar() to while loop for debugging --- dijkstra.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dijkstra.cpp b/dijkstra.cpp index 87d6f5b..a301ac5 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -59,6 +59,7 @@ void Dijkstras::run_planner( // While the queue is not empty while (!Q.empty()) { + getchar(); // Pop and expand the next node in the priority queue (*num_expansions)++; From 95b35b5bdc9786d874a6f81fa88dbf274153c1a4 Mon Sep 17 00:00:00 2001 From: Ramya Date: Wed, 3 Apr 2019 15:22:14 -0700 Subject: [PATCH 37/54] Did lots of debugging --- dijkstra.cpp | 111 +++++--- dijkstra.h | 9 + errors.txt | 6 + graph.cpp | 14 +- student_solutions/test_1.solution | 22 ++ student_solutions/test_2.solution | 18 ++ student_solutions/test_3.solution | 68 +++++ student_solutions/test_4.solution | 76 ++++++ student_solutions/test_5.solution | 404 ++++++++++++++++++++++++++++++ tags | 52 ++++ 10 files changed, 743 insertions(+), 37 deletions(-) create mode 100644 student_solutions/test_1.solution create mode 100644 student_solutions/test_2.solution create mode 100644 student_solutions/test_3.solution create mode 100644 student_solutions/test_4.solution create mode 100644 student_solutions/test_5.solution create mode 100644 tags diff --git a/dijkstra.cpp b/dijkstra.cpp index a301ac5..77c1972 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -1,4 +1,4 @@ -//////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////// // Copyright (c) 2019, Vinitha Ranganeni // All rights reserved. // @@ -41,6 +41,7 @@ void Dijkstras::run_planner( int* num_expansions, std::vector> *path) { + //std::cout << "in run planner" << std::endl; // Create priority queue; I suggest using a set with the custom // comparator defined in dijkstra.h as your priority queue @@ -51,6 +52,11 @@ void Dijkstras::run_planner( CostMap cost_map; CostMapComparator cost_map_comparator(cost_map); std::set Q(cost_map_comparator); + + cost_map[start_id] = 0; + //std::cout << start_id << " = " << cost_map[start_id] << std::endl; + Q.insert(start_id); + // cost_map[start_id] = 0; std::vector path_state_ids; //close list? @@ -59,43 +65,75 @@ void Dijkstras::run_planner( // While the queue is not empty while (!Q.empty()) { - getchar(); + //std::cout << "getchar" << std::endl; + //getchar(); // debugging technique that works on Vinitha's laptop! // Pop and expand the next node in the priority queue (*num_expansions)++; // YOUR CODE HERE - const auto node = Q.begin(); - - if (*node == goal_id) { - extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); - // set path parameter? - break; - } + const int parent = *(Q.begin()); + Q.erase(Q.begin()); + + if (parent == goal_id) { + 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); + return; + } std::vector succ_ids; std::vector costs; - m_graph.get_succs(start_id, &succ_ids, &costs); + m_graph.get_succs(parent, &succ_ids, &costs); auto iterStateID = succ_ids.begin(); - auto iterCosts = costs.begin(); - while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { - child_to_parent_map[*iterStateID] = *node; - - double gValue = *iterCosts; //finds cost from parent to successor - gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value - - // if node is not in the priority queue, we need to add it! - if (Q.find(*node) == Q.end()) { - Q.insert(*node); - } else { - // node is in priority queue, but update it to have optimal cost - // For Dikstras, optimal cost is the least cost - if (gValue < cost_map[*node]) { //cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { - // update cost map? - cost_map[*node] = gValue; // cost_map_[node] = gValue; + auto iterCosts = costs.begin(); + + while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { + //std::cout << "Succs in Planner function" << std::endl; + //std::cout << *iterStateID << " = " << *iterCosts << std::endl; + // child_to_parent_map[*iterStateID] = parent; + + double gValue = cost_map[parent] + *iterCosts; //finds cost from parent to successor + // gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value + + // if node is not in the priority queue, we need to add it + // check cost map instead of doing Q.find (which does binary search on the set so it uses the comparator, which leads to undefined behavior when the cost hasn't been defined for a stateID + if (cost_map.find(*iterStateID) == cost_map.end()) { + cost_map[*iterStateID] = gValue; + Q.insert(*iterStateID); + child_to_parent_map[*iterStateID] = parent; + } else if (gValue < cost_map[*iterStateID]) { + cost_map[*iterStateID] = gValue; + //std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; + Q.erase(*iterStateID); // do this because we can't insert a duplicate value! we need to erase the stateID and then insert it again with the new cost; the comparator will only be called when we do find() or insert() + Q.insert(*iterStateID); + child_to_parent_map[*iterStateID] = parent; + //std::cout << "==========================" << std::endl; + // cost_map[*iterStateID] = gValue; + } + + /* + if (Q.find(*iterStateID) == Q.end()) { + std::cout << "node not in priority queue" << std::endl; + //std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; + cost_map[*iterStateID] = gValue; + std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; + Q.insert(*iterStateID); + std::cout << "==========================" << std::endl; + // cost_map[*iterStateID] = gValue; + } else { + std::cout << "node is in priority queue" << std::endl; + // node is in priority queue, but update it to have optimal cost + // For Dikstras, optimal cost is the least cost + if (gValue < cost_map[*iterStateID]) { //cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { + // update cost map? + cost_map[*iterStateID] = gValue; // cost_map_[node] = gValue; + } + } + */ + ++iterStateID; + ++iterCosts; } - } - } - } + } // if Q is empty, we need to call extract_path too if (Q.empty()) { @@ -148,7 +186,6 @@ void Dijkstras::extract_path( int x, y; //I think I have to get the path backwards (i.e. from goal to start) - // Can stop if we have expanded the goal or we have nothing in our priority list. //do find and check if value is end before loop! @@ -157,20 +194,28 @@ void Dijkstras::extract_path( return; } + std::cout << "start_id = " << start_id << std::endl; + std::cout << "goal_id = " << goal_id << std::endl; + (*path_state_ids).push_back(goal_id); //ChildToParentMap is typedef for std::unordered_map! //maps child state id to parent state id // what is the child_to parent_map doing exactly? auto iter = child_to_parent_map.find(goal_id); - (*path_state_ids).push_back(goal_id); // loop till we find start or we reach end of map - while (iter->second != start_id && iter != child_to_parent_map.end()) { + while (iter != child_to_parent_map.end()) { + + //getchar(); + //std::cout << iter->second << std::endl; (*path_state_ids).push_back(iter->second); iter = child_to_parent_map.find(iter->second); } + //(*path_state_ids).push_back(start_id); + + /* OLD CODE while (1) { // we can put end condition here!!!!! if (iter != child_to_parent_map.end()) { @@ -192,6 +237,8 @@ void Dijkstras::extract_path( iter = child_to_parent_map.find(value); } */ + std::reverse(path_state_ids->begin(), path_state_ids->end()); + // END OF MY CODE } } diff --git a/dijkstra.h b/dijkstra.h index 025fe5c..c75499d 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -34,6 +34,7 @@ #include #include #include +#include namespace grid_planner { namespace planners { @@ -61,6 +62,14 @@ class CostMapComparator { const auto state_1Iter = cost_map_.find(state_1); const auto state_2Iter = cost_map_.find(state_2); + if (state_1Iter == cost_map_.end()) { + std::cout << state_1 << " -> FUCK 1" << std::endl; + } + + if (state_2Iter == cost_map_.end()) { + std::cout << state_2 << " -> FUCK 2" << std::endl; + } + return state_1Iter->second <= state_2Iter->second; // END OF MY CODE diff --git a/errors.txt b/errors.txt index 8ae2a66..a979d14 100644 --- a/errors.txt +++ b/errors.txt @@ -1,2 +1,8 @@ +B +A +B +B +A +A g++ -std=c++11 -c test_planner.cpp graph.h graph.cpp dijkstra.h dijkstra.cpp Makefile:2: recipe for target 'all' failed diff --git a/graph.cpp b/graph.cpp index fed06ef..485be42 100644 --- a/graph.cpp +++ b/graph.cpp @@ -30,6 +30,7 @@ #include "graph.h" #include #include +#include namespace grid_planner { namespace graphs { @@ -97,17 +98,20 @@ void Graph::get_succs( } x_succ = x_source + i; - y_succ = y_source + i; - succ_state_id = get_state_id(x_succ, y_succ); + y_succ = y_source + j; + // succ_state_id = get_state_id(x_succ, y_succ); if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid } else { - (*succ_ids).push_back(succ_state_id); + 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 succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); (*costs).push_back(succ_cost); - } + + // std::cout << succ_state_id << " = " << succ_cost << std::endl; + } } } @@ -142,7 +146,7 @@ int Graph::get_state_id(const int& x, const int& y) const // END OF MY CODE - return 0; + // return 0; } bool Graph::get_coord_from_state_id(const int& state_id, int* x, int* y) const 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 From 7ebbc6d14d424aca0ced5b6d7a16655d3f6c2d6d Mon Sep 17 00:00:00 2001 From: Ramya Date: Wed, 3 Apr 2019 15:33:24 -0700 Subject: [PATCH 38/54] Fixed minor cpplint issues --- graph.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/graph.h b/graph.h index 5531e09..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 @@ -102,7 +102,7 @@ class Graph { int m_goal_id; }; -} -} +} // namespace graphs +} // namespace grid_planner -#endif +#endif // GRAPH_H_ From c05d448aa98f7c67122a557ead2a6d80cb7438c4 Mon Sep 17 00:00:00 2001 From: Ramya Date: Wed, 3 Apr 2019 16:36:52 -0700 Subject: [PATCH 39/54] Fixed some cpplint errors --- dijkstra.cpp | 77 ++++++++++++++++++++++++---------------------------- dijkstra.h | 30 ++++++++++---------- graph.cpp | 66 ++++++++++++++++++++------------------------ 3 files changed, 79 insertions(+), 94 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 77c1972..7cc1138 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -39,12 +39,11 @@ void Dijkstras::run_planner( const int& start_id, const int& goal_id, int* num_expansions, - std::vector> *path) -{ - //std::cout << "in run planner" << std::endl; + std::vector> *path) { + // std::cout << "in run planner" << std::endl; // Create priority queue; I suggest using a set with the custom // comparator defined in dijkstra.h as your priority queue - + // std::set Q; // You will need to change this line // MY CODE BEGINS @@ -52,49 +51,49 @@ void Dijkstras::run_planner( CostMap cost_map; CostMapComparator cost_map_comparator(cost_map); std::set Q(cost_map_comparator); - + cost_map[start_id] = 0; - //std::cout << start_id << " = " << cost_map[start_id] << std::endl; + // std::cout << start_id << " = " << cost_map[start_id] << std::endl; Q.insert(start_id); // cost_map[start_id] = 0; - std::vector path_state_ids; //close list? + std::vector path_state_ids; // close list? - ChildToParentMap child_to_parent_map; // empty map + ChildToParentMap child_to_parent_map; // empty map // MY CODE ENDS // While the queue is not empty while (!Q.empty()) { - //std::cout << "getchar" << std::endl; - //getchar(); // debugging technique that works on Vinitha's laptop! + // std::cout << "getchar" << std::endl; + // getchar(); // debugging technique that works on Vinitha's laptop! // Pop and expand the next node in the priority queue (*num_expansions)++; // YOUR CODE HERE - const int parent = *(Q.begin()); + const int parent = *(Q.begin()); Q.erase(Q.begin()); - if (parent == goal_id) { + if (parent == goal_id) { extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); - // set path parameter? + // set path parameter? m_graph.get_path_coordinates(path_state_ids, path); return; - } + } std::vector succ_ids; std::vector costs; m_graph.get_succs(parent, &succ_ids, &costs); auto iterStateID = succ_ids.begin(); - auto iterCosts = costs.begin(); - + auto iterCosts = costs.begin(); + while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { - //std::cout << "Succs in Planner function" << std::endl; - //std::cout << *iterStateID << " = " << *iterCosts << std::endl; + // std::cout << "Succs in Planner function" << std::endl; + // std::cout << *iterStateID << " = " << *iterCosts << std::endl; // child_to_parent_map[*iterStateID] = parent; - double gValue = cost_map[parent] + *iterCosts; //finds cost from parent to successor + double gValue = cost_map[parent] + *iterCosts; // finds cost from parent to successor // gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value - + // if node is not in the priority queue, we need to add it // check cost map instead of doing Q.find (which does binary search on the set so it uses the comparator, which leads to undefined behavior when the cost hasn't been defined for a stateID if (cost_map.find(*iterStateID) == cost_map.end()) { @@ -103,14 +102,14 @@ void Dijkstras::run_planner( child_to_parent_map[*iterStateID] = parent; } else if (gValue < cost_map[*iterStateID]) { cost_map[*iterStateID] = gValue; - //std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; - Q.erase(*iterStateID); // do this because we can't insert a duplicate value! we need to erase the stateID and then insert it again with the new cost; the comparator will only be called when we do find() or insert() + // std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; + Q.erase(*iterStateID); // do this because we can't insert a duplicate value! we need to erase the stateID and then insert it again with the new cost; the comparator will only be called when we do find() or insert() Q.insert(*iterStateID); child_to_parent_map[*iterStateID] = parent; - //std::cout << "==========================" << std::endl; - // cost_map[*iterStateID] = gValue; + // std::cout << "==========================" << std::endl; + // cost_map[*iterStateID] = gValue; } - + /* if (Q.find(*iterStateID) == Q.end()) { std::cout << "node not in priority queue" << std::endl; @@ -132,7 +131,7 @@ void Dijkstras::run_planner( */ ++iterStateID; ++iterCosts; - } + } } // if Q is empty, we need to call extract_path too @@ -172,23 +171,21 @@ void Dijkstras::run_planner( */ // MY CODE ENDS - } void Dijkstras::extract_path( const ChildToParentMap& child_to_parent_map, const int& start_id, const int& goal_id, - std::vector *path_state_ids) -{ + std::vector *path_state_ids) { // YOUR CODE HERE int x, y; -//I think I have to get the path backwards (i.e. from goal to start) +// I think I have to get the path backwards (i.e. from goal to start) // Can stop if we have expanded the goal or we have nothing in our priority list. -//do find and check if value is end before loop! +// do find and check if value is end before loop! if (goal_id == start_id) { return; @@ -199,23 +196,22 @@ void Dijkstras::extract_path( (*path_state_ids).push_back(goal_id); - //ChildToParentMap is typedef for std::unordered_map! - //maps child state id to parent state id + // ChildToParentMap is typedef for std::unordered_map! + // maps child state id to parent state id // what is the child_to parent_map doing exactly? auto iter = child_to_parent_map.find(goal_id); // loop till we find start or we reach end of map while (iter != child_to_parent_map.end()) { - - //getchar(); - //std::cout << iter->second << std::endl; + // getchar(); + // std::cout << iter->second << std::endl; (*path_state_ids).push_back(iter->second); iter = child_to_parent_map.find(iter->second); } - //(*path_state_ids).push_back(start_id); + // (*path_state_ids).push_back(start_id); + - /* OLD CODE while (1) { // we can put end condition here!!!!! if (iter != child_to_parent_map.end()) { @@ -241,7 +237,6 @@ void Dijkstras::extract_path( // END OF MY CODE } -} - -} +} // namespace planners +} // namespace grid_planner diff --git a/dijkstra.h b/dijkstra.h index c75499d..aaa9aa6 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -27,14 +27,14 @@ // POSSIBILITY OF SUCH DAMAGE. //////////////////////////////////////////////////////////////////////////////// -#ifndef DIJKSTRAS_H_ -#define DIJKSTRAS_H_ +#ifndef DIJKSTRA_H_ +#define DIJKSTRA_H_ -#include "graph.h" #include #include #include #include +#include "graph.h" namespace grid_planner { namespace planners { @@ -54,14 +54,14 @@ 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 does state 1 have lower cost than state 2? + + // return does state 1 have lower cost than state 2? // FUNCTOR: a class that overloads the () operator so that it can be called like a function! - const auto state_1Iter = cost_map_.find(state_1); - const auto state_2Iter = cost_map_.find(state_2); - + const auto state_1Iter = cost_map_.find(state_1); + const auto state_2Iter = cost_map_.find(state_2); + if (state_1Iter == cost_map_.end()) { std::cout << state_1 << " -> FUCK 1" << std::endl; } @@ -70,7 +70,7 @@ class CostMapComparator { std::cout << state_2 << " -> FUCK 2" << std::endl; } - return state_1Iter->second <= state_2Iter->second; + return state_1Iter->second <= state_2Iter->second; // END OF MY CODE @@ -84,11 +84,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 @@ -107,10 +106,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 485be42..ff513cb 100644 --- a/graph.cpp +++ b/graph.cpp @@ -35,12 +35,11 @@ namespace grid_planner { namespace graphs { -int Graph::set_start_state(const int& x, const int& y) -{ +int Graph::set_start_state(const int& x, const int& y) { // YOUR CODE HERE m_start_id = get_state_id(x, y); - + if (is_valid_state(x, y)) { return m_start_id; // valid state } else { @@ -52,8 +51,7 @@ int Graph::set_start_state(const int& x, const int& y) // return -1; } -int Graph::set_goal_state(const int& x, const int& y) -{ +int Graph::set_goal_state(const int& x, const int& y) { // YOUR CODE HERE m_goal_id = get_state_id(x, y); @@ -72,8 +70,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()); // YOUR CODE HERE @@ -93,23 +91,23 @@ void Graph::get_succs( for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { - if (i == 0 && j == 0) { // current state, not a successor + if (i == 0 && j == 0) { // current state, not a successor continue; - } - - x_succ = x_source + i; - y_succ = y_source + j; + } + + x_succ = x_source + i; + y_succ = y_source + j; // succ_state_id = get_state_id(x_succ, y_succ); - if (!is_valid_state(x_succ, y_succ)) { + if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid - } else { - succ_state_id = get_state_id(x_succ, y_succ); + } else { + 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 - succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); - (*costs).push_back(succ_cost); - + // transition cost i.e. cost from parent to successor + succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); + (*costs).push_back(succ_cost); + // std::cout << succ_state_id << " = " << succ_cost << std::endl; } } @@ -120,8 +118,7 @@ void Graph::get_succs( void Graph::get_path_coordinates( const std::vector& path_state_ids, - std::vector > *path_coordinates) const -{ + std::vector > *path_coordinates) const { // YOUR CODE HERE for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { @@ -132,47 +129,43 @@ void Graph::get_path_coordinates( } // END OF MY CODE - } -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 x + y * m_width; + return x + y * m_width; // END OF MY CODE // return 0; } -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 - + *y = state_id / m_width; *x = state_id - *y * m_width; return is_valid_state(*x, *y); // END OF MY CODE - + return true; } -bool Graph::is_valid_state(const int& x, const int& y) const -{ +bool Graph::is_valid_state(const int& x, const int& y) const { // YOUR CODE HERE - + // check bounds (i.e. value is valid) if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) { return false; - } else { // check occupancy grid to see if cell is free + } else { // check occupancy grid to see if cell is free if (m_occupancy_grid[get_state_id(x, y)] == 0) { return true; } else { @@ -181,7 +174,7 @@ bool Graph::is_valid_state(const int& x, const int& y) const } // END OF MY CODE - + return true; } @@ -189,15 +182,14 @@ double Graph::get_action_cost( const int& source_x, const int& source_y, const int& succ_x, - const int& succ_y) const -{ + const int& succ_y) const { // YOUR CODE HERE // Calculate Euclidean distance between the 2 points return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); // END OF MY CODE - // return 0; + // return 0; } } // namespace graphs From e672f99edbdd6218cd5baf6c18da9ed3020ed04d Mon Sep 17 00:00:00 2001 From: Ramya Date: Wed, 3 Apr 2019 19:45:46 -0700 Subject: [PATCH 40/54] Fixed cpplint errors --- dijkstra.cpp | 156 +++++++++------------------------------------------ dijkstra.h | 26 +-------- graph.cpp | 124 ++++++++++++---------------------------- 3 files changed, 65 insertions(+), 241 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 7cc1138..6b9c421 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -31,7 +31,7 @@ #include #include #include -#include + namespace grid_planner { namespace planners { @@ -40,42 +40,31 @@ void Dijkstras::run_planner( const int& goal_id, int* num_expansions, std::vector> *path) { - // std::cout << "in run planner" << std::endl; - // Create priority queue; I suggest using a set with the custom - // comparator defined in dijkstra.h as your priority queue - -// std::set Q; // You will need to change this line -// MY CODE BEGINS - // set of state IDs ordered by cost (rather then sorted in order of integers) + // set of state IDs ordered by cost CostMap cost_map; CostMapComparator cost_map_comparator(cost_map); std::set Q(cost_map_comparator); cost_map[start_id] = 0; - // std::cout << start_id << " = " << cost_map[start_id] << std::endl; Q.insert(start_id); - // cost_map[start_id] = 0; - std::vector path_state_ids; // close list? + std::vector path_state_ids; // close list ChildToParentMap child_to_parent_map; // empty map -// MY CODE ENDS // While the queue is not empty while (!Q.empty()) { - // std::cout << "getchar" << std::endl; - // getchar(); // debugging technique that works on Vinitha's laptop! // Pop and expand the next node in the priority queue (*num_expansions)++; - // YOUR CODE HERE const int parent = *(Q.begin()); Q.erase(Q.begin()); if (parent == goal_id) { - extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); - // set path parameter? + 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); return; } @@ -83,94 +72,39 @@ void Dijkstras::run_planner( std::vector succ_ids; std::vector costs; m_graph.get_succs(parent, &succ_ids, &costs); - auto iterStateID = succ_ids.begin(); - auto iterCosts = costs.begin(); - - while (iterStateID != succ_ids.end() && iterCosts != costs.end()) { - // std::cout << "Succs in Planner function" << std::endl; - // std::cout << *iterStateID << " = " << *iterCosts << std::endl; - // child_to_parent_map[*iterStateID] = parent; + auto iterSuccStateID = succ_ids.begin(); + auto iterSuccCost = costs.begin(); - double gValue = cost_map[parent] + *iterCosts; // finds cost from parent to successor - // gValue += cost_map[*node]; //cost_map_[*node]; // adds above to parent's cost (i.e. from parent to start_id) to get g value + while (iterSuccStateID != succ_ids.end() && + iterSuccCost != costs.end()) { + // gValue is cost of parent + transition cost + // from parent to successor + double gValue = cost_map[parent] + *iterSuccCost; // if node is not in the priority queue, we need to add it - // check cost map instead of doing Q.find (which does binary search on the set so it uses the comparator, which leads to undefined behavior when the cost hasn't been defined for a stateID - if (cost_map.find(*iterStateID) == cost_map.end()) { - cost_map[*iterStateID] = gValue; - Q.insert(*iterStateID); - child_to_parent_map[*iterStateID] = parent; - } else if (gValue < cost_map[*iterStateID]) { - cost_map[*iterStateID] = gValue; - // std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; - Q.erase(*iterStateID); // do this because we can't insert a duplicate value! we need to erase the stateID and then insert it again with the new cost; the comparator will only be called when we do find() or insert() - Q.insert(*iterStateID); - child_to_parent_map[*iterStateID] = parent; - // std::cout << "==========================" << std::endl; - // cost_map[*iterStateID] = gValue; + if (cost_map.find(*iterSuccStateID) == cost_map.end()) { + cost_map[*iterSuccStateID] = gValue; + Q.insert(*iterSuccStateID); + child_to_parent_map[*iterSuccStateID] = parent; + } else if (gValue < cost_map[*iterSuccStateID]) { + cost_map[*iterSuccStateID] = gValue; + Q.erase(*iterSuccStateID); + Q.insert(*iterSuccStateID); + child_to_parent_map[*iterSuccStateID] = parent; } - /* - if (Q.find(*iterStateID) == Q.end()) { - std::cout << "node not in priority queue" << std::endl; - //std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; - cost_map[*iterStateID] = gValue; - std::cout << *iterStateID << " = " << cost_map[*iterStateID] << std::endl; - Q.insert(*iterStateID); - std::cout << "==========================" << std::endl; - // cost_map[*iterStateID] = gValue; - } else { - std::cout << "node is in priority queue" << std::endl; - // node is in priority queue, but update it to have optimal cost - // For Dikstras, optimal cost is the least cost - if (gValue < cost_map[*iterStateID]) { //cost_map_[node]) { //if (CostMapComparator(*iterStateID, *node)) { - // update cost map? - cost_map[*iterStateID] = gValue; // cost_map_[node] = gValue; - } - } - */ - ++iterStateID; - ++iterCosts; + ++iterSuccStateID; + ++iterSuccCost; } } - // if Q is empty, we need to call extract_path too + // if Q is empty, we need to call extract_path if (Q.empty()) { 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); - -/* - check if node popped is goal - call extract_path - break - - //popped node has optimal cost - - call get successors function - - //close list has nodes with optimal cost - // open list has everything that in in priority queue, and nodes that haven't been seen yet - - // check if sucessor node is in the priority queue - // if its not in the priority queue, we need to add it - // find cost from parent to successor by calling get_action _cost - // and add this to parent's cost to get g value - // - // else - // find cost here too - // - // update cost to optimal cost in priority queue - // - // - //at end of while loop, call extract path - -// we can stop if we have expanded the goal or we have nothing in our priority queue - */ - - // MY CODE ENDS } void Dijkstras::extract_path( @@ -178,64 +112,24 @@ void Dijkstras::extract_path( const int& start_id, const int& goal_id, std::vector *path_state_ids) { -// YOUR CODE HERE int x, y; -// I think I have to get the path backwards (i.e. from goal to start) -// Can stop if we have expanded the goal or we have nothing in our priority list. - -// do find and check if value is end before loop! - if (goal_id == start_id) { return; } - std::cout << "start_id = " << start_id << std::endl; - std::cout << "goal_id = " << goal_id << std::endl; - (*path_state_ids).push_back(goal_id); - // ChildToParentMap is typedef for std::unordered_map! - // maps child state id to parent state id - // what is the child_to parent_map doing exactly? auto iter = child_to_parent_map.find(goal_id); // loop till we find start or we reach end of map while (iter != child_to_parent_map.end()) { - // getchar(); - // std::cout << iter->second << std::endl; (*path_state_ids).push_back(iter->second); iter = child_to_parent_map.find(iter->second); } - // (*path_state_ids).push_back(start_id); - - -/* OLD CODE - while (1) { // we can put end condition here!!!!! - if (iter != child_to_parent_map.end()) { - // Should I remove the mapping? - std::string value = iter->second; - - (*path_state_ids).push_back(value); - child_to_parent_map.erase(iter); // removes mapping from map if key is found - - // will this work for the case when the start and goal are the same? - - if (value == start_id) { - break; - } - } else { - break; - } - - iter = child_to_parent_map.find(value); - } -*/ std::reverse(path_state_ids->begin(), path_state_ids->end()); - - // END OF MY CODE } } // namespace planners diff --git a/dijkstra.h b/dijkstra.h index aaa9aa6..013c619 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -33,7 +33,6 @@ #include #include #include -#include #include "graph.h" namespace grid_planner { @@ -52,29 +51,10 @@ 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 + const auto state_1Iter = cost_map_.find(state_1); + const auto state_2Iter = cost_map_.find(state_2); - // YOUR CODE HERE (replace line below) - - // return does state 1 have lower cost than state 2? - // FUNCTOR: a class that overloads the () operator so that it can be called like a function! - const auto state_1Iter = cost_map_.find(state_1); - const auto state_2Iter = cost_map_.find(state_2); - - if (state_1Iter == cost_map_.end()) { - std::cout << state_1 << " -> FUCK 1" << std::endl; - } - - if (state_2Iter == cost_map_.end()) { - std::cout << state_2 << " -> FUCK 2" << std::endl; - } - - return state_1Iter->second <= state_2Iter->second; - - // END OF MY CODE - - // return true; + return state_1Iter->second <= state_2Iter->second; } private: diff --git a/graph.cpp b/graph.cpp index ff513cb..f99866e 100644 --- a/graph.cpp +++ b/graph.cpp @@ -30,41 +30,28 @@ #include "graph.h" #include #include -#include namespace grid_planner { namespace graphs { int Graph::set_start_state(const int& x, const int& y) { - // YOUR CODE HERE - m_start_id = get_state_id(x, y); if (is_valid_state(x, y)) { - return m_start_id; // valid state + return m_start_id; // valid state } else { - return -1; // invalid state + return -1; // invalid state } - - // END OF MY CODE - -// return -1; } int Graph::set_goal_state(const int& x, const int& y) { - // YOUR CODE HERE - m_goal_id = get_state_id(x, y); if (is_valid_state(x, y)) { - return m_goal_id; // valid state + return m_goal_id; // valid state } else { - return -1; // invalid state + return -1; // invalid state } - - // END OF MY CODE - -// return -1; } void Graph::get_succs( @@ -74,15 +61,6 @@ void Graph::get_succs( assert(source_state_id < m_occupancy_grid.size()); - // YOUR CODE HERE -/* - std::vector::iterator iter; - for (iter = (*succ_ids).begin(); iter != (*succ_ids).end(); iter++) { - if (is_valid_state(*iter)) { // successor is valid - - } - } -*/ int x_source, y_source; get_coord_from_state_id(source_state_id, &x_source, &y_source); int x_succ, y_succ; @@ -90,92 +68,69 @@ void Graph::get_succs( double succ_cost; 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; - } - - x_succ = x_source + i; - y_succ = y_source + j; - // succ_state_id = get_state_id(x_succ, y_succ); - - if (!is_valid_state(x_succ, y_succ)) { - continue; // successor is not valid - } else { - 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 - succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); - (*costs).push_back(succ_cost); - - // std::cout << succ_state_id << " = " << succ_cost << std::endl; - } - } + for (int j = -1; j <= 1; j++) { + if (i == 0 && j == 0) { // current state, not a successor + continue; + } + + x_succ = x_source + i; + y_succ = y_source + j; + + if (!is_valid_state(x_succ, y_succ)) { + continue; // successor is not valid + } else { + 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 + succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); + (*costs).push_back(succ_cost); + } + } } - - // END OF MY CODE } void Graph::get_path_coordinates( const std::vector& path_state_ids, std::vector > *path_coordinates) const { - // YOUR CODE HERE - for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { - int x, y; - if (get_coord_from_state_id(*iter, &x, &y)) { - (*path_coordinates).push_back(std::make_pair(x, y)); // coordinates are valid - } + for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); + iter++) { + int x, y; + if (get_coord_from_state_id(*iter, &x, &y)) { + // coordinates are valid + (*path_coordinates).push_back(std::make_pair(x, y)); + } } - - // END OF MY CODE } int Graph::get_state_id(const int& x, const int& y) const { assert(x < m_width); assert(y < m_height); - // YOUR CODE HERE - return x + y * m_width; - - // END OF MY CODE - - // return 0; } 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 - *y = state_id / m_width; *x = state_id - *y * m_width; return is_valid_state(*x, *y); - - // END OF MY CODE - - return true; } bool Graph::is_valid_state(const int& x, const int& y) const { - // YOUR CODE HERE - - // check bounds (i.e. value is valid) + // check bounds (i.e. check value is valid) if (!(x >= 0 && x < m_width && y >= 0 && y < m_height)) { - return false; + return false; } else { // check occupancy grid to see if cell is free - if (m_occupancy_grid[get_state_id(x, y)] == 0) { - return true; - } else { - return false; - } + if (m_occupancy_grid[get_state_id(x, y)] == 0) { + return true; + } else { + return false; + } } - - // END OF MY CODE - - return true; } double Graph::get_action_cost( @@ -183,13 +138,8 @@ double Graph::get_action_cost( const int& source_y, const int& succ_x, const int& succ_y) const { - // YOUR CODE HERE - // Calculate Euclidean distance between the 2 points return sqrt(pow(succ_x - source_x, 2) + pow(succ_y - source_y, 2)); - - // END OF MY CODE - // return 0; } } // namespace graphs From 71033ee29863fc8cafc2068075c0d36ec2c6a4e7 Mon Sep 17 00:00:00 2001 From: Ramya Date: Wed, 3 Apr 2019 20:08:59 -0700 Subject: [PATCH 41/54] Fixed indentation and variable naming --- dijkstra.cpp | 54 +++++++++++++++++++++++++--------------------------- dijkstra.h | 6 +++--- 2 files changed, 29 insertions(+), 31 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index 6b9c421..f0e2b9f 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -72,39 +72,39 @@ void Dijkstras::run_planner( std::vector succ_ids; std::vector costs; m_graph.get_succs(parent, &succ_ids, &costs); - auto iterSuccStateID = succ_ids.begin(); - auto iterSuccCost = costs.begin(); + auto iter_succ_state_id = succ_ids.begin(); + auto iter_succ_cost = costs.begin(); - while (iterSuccStateID != succ_ids.end() && - iterSuccCost != costs.end()) { - // gValue is cost of parent + transition cost + while (iter_succ_state_id != succ_ids.end() && + iter_succ_cost != costs.end()) { + // g_value is cost of parent + transition cost // from parent to successor - double gValue = cost_map[parent] + *iterSuccCost; + double g_value = cost_map[parent] + *iter_succ_cost; // if node is not in the priority queue, we need to add it - if (cost_map.find(*iterSuccStateID) == cost_map.end()) { - cost_map[*iterSuccStateID] = gValue; - Q.insert(*iterSuccStateID); - child_to_parent_map[*iterSuccStateID] = parent; - } else if (gValue < cost_map[*iterSuccStateID]) { - cost_map[*iterSuccStateID] = gValue; - Q.erase(*iterSuccStateID); - Q.insert(*iterSuccStateID); - child_to_parent_map[*iterSuccStateID] = parent; + if (cost_map.find(*iter_succ_state_id) == cost_map.end()) { + cost_map[*iter_succ_state_id] = g_value; + Q.insert(*iter_succ_state_id); + child_to_parent_map[*iter_succ_state_id] = parent; + } else if (g_value < cost_map[*iter_succ_state_id]) { + cost_map[*iter_succ_state_id] = g_value; + Q.erase(*iter_succ_state_id); + Q.insert(*iter_succ_state_id); + child_to_parent_map[*iter_succ_state_id] = parent; } - ++iterSuccStateID; - ++iterSuccCost; + ++iter_succ_state_id; + ++iter_succ_cost; } } - // if Q is empty, we need to call extract_path - if (Q.empty()) { + // if Q is empty, we need to call extract_path + if (Q.empty()) { 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); + // set path parameter + m_graph.get_path_coordinates(path_state_ids, path); } void Dijkstras::extract_path( @@ -113,10 +113,8 @@ void Dijkstras::extract_path( const int& goal_id, std::vector *path_state_ids) { - int x, y; - if (goal_id == start_id) { - return; + return; } (*path_state_ids).push_back(goal_id); @@ -125,12 +123,12 @@ void Dijkstras::extract_path( // loop till we find start or we reach end of map while (iter != child_to_parent_map.end()) { - (*path_state_ids).push_back(iter->second); - iter = child_to_parent_map.find(iter->second); + (*path_state_ids).push_back(iter->second); + iter = child_to_parent_map.find(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 013c619..77b2705 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -51,10 +51,10 @@ class CostMapComparator { bool operator()(const int& state_1, const int& state_2) const { - const auto state_1Iter = cost_map_.find(state_1); - const auto state_2Iter = cost_map_.find(state_2); + const auto state_1_iter = cost_map_.find(state_1); + const auto state_2_iter = cost_map_.find(state_2); - return state_1Iter->second <= state_2Iter->second; + return state_1_iter->second <= state_2_iter->second; } private: From 39dcb2c9e6c6b50466187f6b168fd2d4ce248a10 Mon Sep 17 00:00:00 2001 From: Ramya Date: Thu, 4 Apr 2019 17:04:57 -0700 Subject: [PATCH 42/54] Fixed variable naming and removed errrors.txt --- dijkstra.cpp | 47 +++++++++++++++++++++-------------------------- errors.txt | 8 -------- 2 files changed, 21 insertions(+), 34 deletions(-) delete mode 100644 errors.txt diff --git a/dijkstra.cpp b/dijkstra.cpp index f0e2b9f..f1bd89b 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -41,7 +41,6 @@ void Dijkstras::run_planner( int* num_expansions, std::vector> *path) { - // set of state IDs ordered by cost CostMap cost_map; CostMapComparator cost_map_comparator(cost_map); std::set Q(cost_map_comparator); @@ -49,52 +48,48 @@ void Dijkstras::run_planner( cost_map[start_id] = 0; Q.insert(start_id); - std::vector path_state_ids; // close list + ChildToParentMap child_to_parent_map; - ChildToParentMap child_to_parent_map; // empty 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)++; - const int parent = *(Q.begin()); + const int parent_id = *(Q.begin()); Q.erase(Q.begin()); - if (parent == goal_id) { + if (parent_id == goal_id) { 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); return; } std::vector succ_ids; std::vector costs; - m_graph.get_succs(parent, &succ_ids, &costs); - auto iter_succ_state_id = succ_ids.begin(); - auto iter_succ_cost = costs.begin(); + m_graph.get_succs(parent_id, &succ_ids, &costs); + auto succ_state_id_iter = succ_ids.begin(); + auto transition_cost_iter = costs.begin(); - while (iter_succ_state_id != succ_ids.end() && - iter_succ_cost != costs.end()) { - // g_value is cost of parent + transition cost - // from parent to successor - double g_value = cost_map[parent] + *iter_succ_cost; + while (succ_state_id_iter != succ_ids.end() && + transition_cost_iter != costs.end()) { + double g_value = cost_map[parent_id] + *transition_cost_iter; // if node is not in the priority queue, we need to add it - if (cost_map.find(*iter_succ_state_id) == cost_map.end()) { - cost_map[*iter_succ_state_id] = g_value; - Q.insert(*iter_succ_state_id); - child_to_parent_map[*iter_succ_state_id] = parent; - } else if (g_value < cost_map[*iter_succ_state_id]) { - cost_map[*iter_succ_state_id] = g_value; - Q.erase(*iter_succ_state_id); - Q.insert(*iter_succ_state_id); - child_to_parent_map[*iter_succ_state_id] = parent; + if (cost_map.find(*succ_state_id_iter) == cost_map.end()) { + cost_map[*succ_state_id_iter] = g_value; + Q.insert(*succ_state_id_iter); + child_to_parent_map[*succ_state_id_iter] = parent_id; + } else if (g_value < cost_map[*succ_state_id_iter]) { + cost_map[*succ_state_id_iter] = g_value; + Q.erase(*succ_state_id_iter); + Q.insert(*succ_state_id_iter); + child_to_parent_map[*succ_state_id_iter] = parent_id; } - ++iter_succ_state_id; - ++iter_succ_cost; + ++succ_state_id_iter; + ++transition_cost_iter; } } diff --git a/errors.txt b/errors.txt deleted file mode 100644 index a979d14..0000000 --- a/errors.txt +++ /dev/null @@ -1,8 +0,0 @@ -B -A -B -B -A -A -g++ -std=c++11 -c test_planner.cpp graph.h graph.cpp dijkstra.h dijkstra.cpp -Makefile:2: recipe for target 'all' failed From e69783c2b7f5bf74e7191583646d72d52a7d0db9 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:29:32 -0700 Subject: [PATCH 43/54] Removed unnecessary if statement --- dijkstra.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index f1bd89b..c06c463 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -94,9 +94,7 @@ void Dijkstras::run_planner( } // if Q is empty, we need to call extract_path - if (Q.empty()) { - extract_path(child_to_parent_map, start_id, goal_id, &path_state_ids); - } + 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); From 875e49635f352d4975338ff2b5da5f2512c89af2 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:32:16 -0700 Subject: [PATCH 44/54] Renamed iter --- dijkstra.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dijkstra.cpp b/dijkstra.cpp index c06c463..2ecb033 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -112,12 +112,12 @@ void Dijkstras::extract_path( (*path_state_ids).push_back(goal_id); - auto iter = child_to_parent_map.find(goal_id); + auto path_iter = child_to_parent_map.find(goal_id); // loop till we find start or we reach end of map - while (iter != child_to_parent_map.end()) { - (*path_state_ids).push_back(iter->second); - iter = child_to_parent_map.find(iter->second); + 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()); From 3e298dcb05176378575b5cd637c4a391b5477686 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:34:26 -0700 Subject: [PATCH 45/54] Removed unnecessary else statement --- graph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/graph.cpp b/graph.cpp index f99866e..fdd53ab 100644 --- a/graph.cpp +++ b/graph.cpp @@ -39,9 +39,9 @@ int Graph::set_start_state(const int& x, const int& y) { if (is_valid_state(x, y)) { return m_start_id; // valid state - } else { - return -1; // invalid state - } + } + + return -1; // invalid state } int Graph::set_goal_state(const int& x, const int& y) { From 691f9897ba890c238b14c431bd866669fc8eff01 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:40:47 -0700 Subject: [PATCH 46/54] Changes placement of variable declarartions --- graph.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/graph.cpp b/graph.cpp index fdd53ab..647fd77 100644 --- a/graph.cpp +++ b/graph.cpp @@ -63,27 +63,25 @@ void Graph::get_succs( int x_source, y_source; get_coord_from_state_id(source_state_id, &x_source, &y_source); - int x_succ, y_succ; - int succ_state_id; - double succ_cost; 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; } - + + int x_succ, y_succ; x_succ = x_source + i; y_succ = y_source + j; if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid } else { - succ_state_id = get_state_id(x_succ, y_succ); + 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 - succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); + double succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); (*costs).push_back(succ_cost); } } From d8cd3254d53296e04fb215ac03aa39338a9e0ac4 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:43:00 -0700 Subject: [PATCH 47/54] add const keyword --- graph.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/graph.cpp b/graph.cpp index 647fd77..461a54e 100644 --- a/graph.cpp +++ b/graph.cpp @@ -70,9 +70,8 @@ void Graph::get_succs( continue; } - int x_succ, y_succ; - x_succ = x_source + i; - y_succ = y_source + j; + 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 From ab540ca74717345b13e6833b8e000d29a19d29ba Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:46:00 -0700 Subject: [PATCH 48/54] Removed unnecessary else statement --- graph.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/graph.cpp b/graph.cpp index 461a54e..5cab6be 100644 --- a/graph.cpp +++ b/graph.cpp @@ -75,14 +75,13 @@ void Graph::get_succs( if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid - } else { - 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 - double succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); - (*costs).push_back(succ_cost); } + 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 + double succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); + (*costs).push_back(succ_cost); } } } From 799d342ff849b1a6f38ff0065a48065c23b8ebe1 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:49:41 -0700 Subject: [PATCH 49/54] Added const keyword --- graph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/graph.cpp b/graph.cpp index 5cab6be..53e7aaa 100644 --- a/graph.cpp +++ b/graph.cpp @@ -76,11 +76,11 @@ void Graph::get_succs( if (!is_valid_state(x_succ, y_succ)) { continue; // successor is not valid } - int succ_state_id = get_state_id(x_succ, y_succ); + 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 - double succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); + const double succ_cost = get_action_cost(x_source, y_source, x_succ, y_succ); (*costs).push_back(succ_cost); } } From 9e6f010db8b907a29c87d5264ec0447c01081111 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 19:52:24 -0700 Subject: [PATCH 50/54] Removed two else statements --- graph.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/graph.cpp b/graph.cpp index 53e7aaa..b2e9ed7 100644 --- a/graph.cpp +++ b/graph.cpp @@ -120,13 +120,12 @@ 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; - } else { // check occupancy grid to see if cell is free - if (m_occupancy_grid[get_state_id(x, y)] == 0) { - return true; - } else { - 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( From 4cfe7d90237c4953f7fe89c3fd24f82d85ea033c Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 20:06:47 -0700 Subject: [PATCH 51/54] Iterate using index rather than using pointers --- .graph.cpp.un~ | Bin 0 -> 5588 bytes graph.cpp | 11 +++++++++++ 2 files changed, 11 insertions(+) create mode 100644 .graph.cpp.un~ diff --git a/.graph.cpp.un~ b/.graph.cpp.un~ new file mode 100644 index 0000000000000000000000000000000000000000..e08efec08e2604321d26f85d964dd334aee07deb GIT binary patch literal 5588 zcmeI0!D|yi6vn4%s;w5hX%R~1Vo7N!1t}9t`v-JLb9oQXjZa|R7;VbN4gB=Nq@< z1Ch&Zy#8`yZ+PMPi*SG9XrOgaD(`&yx%+N1`E~utmDP12#9R1|gz9lq7OQb1NoCZa zYvHn-m6Ox@a7I3^q>q9mt)yWP)sl%MdKMN(r{&m~`tGP)!KEI+m*5_F5T+Q7MXCMq zeV%9YBKXX+P%aDVlc!2%{2Nl2G7~ zT0|DYeH?pC%z{e+(^VT&4L4haU%^Dv#|mE6tjH!082a2mN32mKhg3E!h4iC6)`Y z5s?v7ffF7Rv*1#A^X~7)RK*PqrUFZn`m+hp(P(wCk=jM;IZ3o)#-5_dgsTXOcNN84 zwrnlqMx^)#wu_Be%mxe&6|yqoO%6OJX2GR^siiRWW|-IyStZwV>XNmrl0WiXG)QE=QK(r`TuHm1tKXE~#*{_j3QuBW4nu3W(bZalZ2f(jY#o KUDc_z@9ks81|$Oj literal 0 HcmV?d00001 diff --git a/graph.cpp b/graph.cpp index b2e9ed7..0e030c3 100644 --- a/graph.cpp +++ b/graph.cpp @@ -90,6 +90,16 @@ void Graph::get_path_coordinates( const std::vector& path_state_ids, 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)); + } + } + + /* for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); iter++) { int x, y; @@ -98,6 +108,7 @@ void Graph::get_path_coordinates( (*path_coordinates).push_back(std::make_pair(x, y)); } } + */ } int Graph::get_state_id(const int& x, const int& y) const { From 864fbcc7e13c7945e3454ec5ad9ee4ed54368d26 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 20:26:39 -0700 Subject: [PATCH 52/54] Iterate over index rather than pointers for safety --- .dijkstra.cpp.un~ | Bin 0 -> 17045 bytes .graph.cpp.un~ | Bin 5588 -> 6692 bytes dijkstra.cpp | 23 +++++++++++++++++++++++ graph.cpp | 11 ----------- 4 files changed, 23 insertions(+), 11 deletions(-) create mode 100644 .dijkstra.cpp.un~ diff --git a/.dijkstra.cpp.un~ b/.dijkstra.cpp.un~ new file mode 100644 index 0000000000000000000000000000000000000000..7c770dc71aa67cbd5a2b92f0b2ee355b65e6dd80 GIT binary patch literal 17045 zcmeI3%Wqpn6o;KQfffq1l=6ODpok<5RwR@Pkwz5yNLyMUK*J*mmh0TMMu{C_Cn~BU z+DcTVDnhEDNGy1)ko+I;Cgzux$<_q$D>>^}SPHCA{Esg}Ecx zg;vl6#BX-A@_D=EMgO*6{FFHI(pC^GS8|*f$dIA4AS;c8uT(JYu$bt8!1TJf#RQf$ z;%Xp6O|C3p(y;hSM=KYsZi8nrXx zy!O>_aWn4qA1&!8tnL7J6`x8Bs0YORI1aia3O-Fh{%S$eG(IH`NHF#3TL)>bSSXdF ze4$)FK2OH+rICDLJU%w2<1|=aEv^RV=fu}h5ZDq0pEg?gqT8|^mWw?vepVc|do9~V zPRw-I&CIx=y7GpvV=E~Jdw4T#6L=E@uO_^ETJhdzdC#r{Z~!~FNxgv2JROaC08S8= zV@OAv&z$5n(zBWu;E!0~Psj-xZx#4D3p^P$g6LXtHNF&N?n0qW*03xwT@13v>XD7f z#T40ki;TQ`c><7v)o~@@t~0j@)KPIaF&aRc$Sfs0F8`$3o05Dp?F~ARZPoYN5&00| z$LL(!ftP3wH0wE$7}%Z`zl-C5iYWLrX(lJF=7!{A&xo@+1*$<%spStxow_EKJ}mFE zo!yOV6J623{K{22mJfAxMaL7`(A^-e26<@rQV?hp1)nCgUui{qw~{7pr2QXZ1NVAU z9l-N!^-vaI69u0puxBl7boQ*cO8X%To4iVUq=~A>WEh()OzVgSoEuFAKn>noDGPXs zf=?5?ueQQlTSHdNm%-bFEF^C*_YP&AoIutD#a~v)9=Bu>)j@F;Zd!?II+V^AO7V2L zX?Lu01>z=i(*OqQ7RrKLiGoiPsB^8LKAnc+i-e~R`zC0=SQ5KjI1)nBN zPgthNX;@r^x=UGVnmd}G7|)c8nPfv3d%G^v+19pvDQno?WNHI_C_O|?eEYbvNuThyqG$*T+EjENsrIg`#s@kA+h zgC*U#xvlk^^!#WTuD6J*!5q%pC|9{i_LzzpNmA*CIttFN&*7mo{AmM7IW*+&F-N8hX_vAbt~p z`d%xjU5z?zItN)3Df}XH&#>hw2~$-jDE_j-^L@(`DX}cCBHdNbv)S~;VyCKLd$*}e zBzahGr7ZNZM8T&C>kl34Rn_em^{iKNsw$F-++!*Oc6Q{r{pJC+iHw)5p#Id#T6ejr ztQTx+j8BY^KUbpbxW`zQ;*iGoiP;1?b6)m3fR17FyVFA=ydIly+asSVr@h_e(C zSQ7-VCO_KW=vcQ`S?`yt!CS?>tluA44O0s^qhq(3FX)(%_%vbruw&cNT*nLh=tb(d M?)L{G<6kQO0@&%jC;$Ke literal 0 HcmV?d00001 diff --git a/.graph.cpp.un~ b/.graph.cpp.un~ index e08efec08e2604321d26f85d964dd334aee07deb..130faa418619fb5281d8bf52235b6389240b722b 100644 GIT binary patch delta 616 zcmcbjy~HFjEHih0Xr4>{QwFAeX1QIpPBArMzbk~6D#>z$cCCDxA$LeKCQ)*+-uFr$ z1_p*cAcg^cAcGNz`Oq*gNI}f1U7uJ$vTPeyxC%4!Zhj~Hgi$K~0#Jkph(UT_0Hi*3 z^Ild@#`;v47?92fVm2th3`m2tAsNf?RRYLQ0pc_uo(aTY1wfij0SNT9fGjT%D=oiB zK_juWBwr!3B(+GvR-quVBqP4KB(WqlJ~O3QFDW%WGfzX)8Yt}rlvl)1keZhQlm;3n z1Tqc`KqhHxYbsO&MFru4c_j)JItrB_rKXdY39Hm=fz;_Jr~yUQDm5X>Rba|Nw7$MV za(;eMN@gC=HN^^vMX3s9i8+}dUpXMF)6fDt8cD6DUO{PbMto9Ya<)csNs5(KZen(7 zd_iJnkp{>Onji<;GjPHTsRf$I3gUnm-~iIn2eKGzO&h^+0**7F01SZQCj}fQP9Vp? b0Emq(PQHMnngJ3g$v{P*H~|CG#^{QwAoc&S}>yj>|^W_oSZF`^GutX-vYJ({B#WH7NdLTqa)I z&cMJhbF!VNw;qrY3&i|D%m~ChKnwyP8ipBSR_$8N0+MCfnCL3Z$gp|2@DoO+=c@n% CvLLqr diff --git a/dijkstra.cpp b/dijkstra.cpp index 2ecb033..efdaf5d 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -69,6 +69,28 @@ void Dijkstras::run_planner( 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 node is not in the priority queue, we need to add it + if (cost_map.find(succ_state_id) == cost_map.end()) { + cost_map[succ_state_id] = g_value; + Q.insert(succ_state_id); + child_to_parent_map[succ_state_id] = parent_id; + } else if (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; + } + } +/* auto succ_state_id_iter = succ_ids.begin(); auto transition_cost_iter = costs.begin(); @@ -91,6 +113,7 @@ void Dijkstras::run_planner( ++succ_state_id_iter; ++transition_cost_iter; } +*/ } // if Q is empty, we need to call extract_path diff --git a/graph.cpp b/graph.cpp index 0e030c3..5bed121 100644 --- a/graph.cpp +++ b/graph.cpp @@ -98,17 +98,6 @@ void Graph::get_path_coordinates( (*path_coordinates).push_back(std::make_pair(x, y)); } } - - /* - for (auto iter = path_state_ids.begin(); iter != path_state_ids.end(); - iter++) { - int x, y; - if (get_coord_from_state_id(*iter, &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 { From c8767f568567cb72439d6a7bce82f441e028e269 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sun, 7 Apr 2019 20:40:32 -0700 Subject: [PATCH 53/54] fixed cpplint errors --- .dijkstra.cpp.un~ | Bin 17045 -> 31629 bytes .graph.cpp.un~ | Bin 6692 -> 8637 bytes dijkstra.cpp | 38 ++++++-------------------------------- graph.cpp | 7 ++++--- 4 files changed, 10 insertions(+), 35 deletions(-) diff --git a/.dijkstra.cpp.un~ b/.dijkstra.cpp.un~ index 7c770dc71aa67cbd5a2b92f0b2ee355b65e6dd80..59c662e217d400b4c61b33ebcd6112b0c5b0e5c1 100644 GIT binary patch literal 31629 zcmeI5U5r&%6~~9^&=#Zx1+?{}9;}v`fzDJ~Xf04a+DcnnElNRT;AWV)&>LZf&fFPn z01Gs2Nn>JSOf@y}Gro-R$rn?tT4Va`gVC56AB@KM5fhsjA2jO!x9?e(b?zK)@7yy7 zU-nAY?0xn3zBPyT4fepQD3EzOwWiudkeX;{&qzUWXOhb3Onow$o*5gfOxH*2 z)k|lI@Me+W7R)(dzNqME}6n%@+b$4!=ff0_8~7tWkO>vVYAZdtO%tO;3sm zEPZUJ0|!bC3{q}%WbTK3o)`7oqtc5~+{;Qquw2J>Dj>rRgG9G7 zOOzD~)BPS36%d%7lX^^Gxn61lGQ26IMNDRvvci$MS6}m(@TQMQpO=EEwXy$mCp^r70sc=a=ft)B+V zw@6Lkd_cN_gvb^vlqn-~KN|FG_v>Pxl)fm1?Zck!_~gvd6V>E+Wp?z$Of>~sIXQaj z(5ca>>O@`L^25o7WUz&N=&BoU_y)EL5!l1~1~wvZtWc(i_r`X-AMw0r7Xvte1Kc!Q z!2g((LOlY<2t@?fvEp+~akKQS#RYta2mVq0K;dl;zQO}fdyOEvQEI|TK^86)ebmg- zW!MR_-L1$D>tcp%l}AQAG@bxtV69&XxRvfUfjS}$1EUe7vrIS9aUEp7cS`X*-`f*F zE@`xH4{0aNk5IYx0WbAA(5&KzRKWIe>3i9ZsIWqrqGIx-_qn~g*e9efNkO#-R3^1! zNk45_nm$9v)Sm4hod0fOAlcq%@rJ#s+G$`QIg`?c?pviMkcajiBt+U+p-d6&r`pl} zu#qO5PkVu|f%`U>9l-N!Z6qzi#tLPMV9$Ek_}M{73-=DyCZ?-X_1x;%+!csx-Aw}+s2fO&?ur%46hS@L4(ja&)50dyoLbkI!tm|x zuHnm*@&VE!Q>;*?i0OIH6nEM$HAj8WSXwytWbMRwr9N3n#|;@myh3H$99cBhuziQi z4cuY9g|x^TE0if>eX$+u2aK_Wd93HVGPUnK)SzWrmgPRbyo|xC zm!$NGcNt>Bb4m5YbTxFA?ES#D_ATklyT^cKtDdOCi6q~+=ZRNbP zbIv)AeHk-YL(@6U49Iz&Vm~-FNV$P9f6luz9>%LabLXP{U~s<6WdYRSjlCsjl=7m& z`}uZw3-p5__-=RSQ0B=AWp*r|on=aiitHCWS)A&i)Eqab%T%2jovya+2E*_@?snk| z(?QZAQ>;*?i0K#GG2Q6vT;h9&8cgTcu+1e8AiTxhG1PzUDwY3>+g(@Ei%xO;{zPe~lAaz-gHDxP2U zJaHvn%bSxv*vd0+_@dLHD%h@bc}Wz9^%l}%9g7vp6tVt#VBOo8e$mQ$F^8%WW#oM> zGhk;%PMB{VQ9H|&5*6xKyj$BWH*@REw8msHrZociR+ksx1AH@S5ja*TQw03g0Q{PU zZ?^)^mg8Llw^t6ZeZR{M+#i#!B`vbX3T4X3+>ciW)_o1ukLqIZHs>z1{s5Mt?Um*L z&iHYd%oo|FRw*qI+j|4srTKoGt)n~iY44anJ@pWl$ zdY)N}3-$K{>hv|kh<;!JGj4pdboub0HNLgg9Ijlzryq3KCObtqtmQ$lZ$^Z5NX-vM z=qO~19eG+Wr>jZ-{DGEao%`HGn9!Nr#`BaZe{`1ItLoUsWo;&?4qUv&HDo}NnYTDR z%-gt$$#K1vn^wrGtfc;AH90j^o1CiEpH7~dsm@e4Ca0^(M727e=o1^G4a*^^7T>^{E{Zx3KC-3jui@sFWZUdmF>UrWxF-e zKC7LVNHb+XV~J>5m$v{<7ZtXy5(Q_!)CAPvT}eX3ixmn;CwPA!@Lrp3fg9ef-icN3 zbU_%R11=BHe%!VrVY5V8;mF)i4tfWs3&ea|BWx?mz!F{&rZ)mI9-r7%hz#lvL>wU+CRA7?y>@NIEVFsv8HpDDJ3e--)_fw zcPr;vTQJmE>Y3@rv;_^=A(scB@0G?zt20X1(Pq)8GZ4QoJrmS549;p{W=0MbY1UR&Ra>44G=Q)Zk4Ck%~a02xdh=K!2N1&Ri!`-vdY-x_x4 z`Y%3aAde`X2Xcc4NhxaP_x^^jeMo8oGJKQbGGf9AMFbx+OuX65!?eX?vgXVS+8IoD zSw&}I2ThqX=@=9!@0FT>8Bg6rLi9kaP(V6Cd!I*(SK)<*wmG}d(ApT(p==j({zu{f z$vh?#AXEIVB_V>v3I(L1V{b~2CvKJ4ti@GF?(<;rDZpNm; z96CS?ATwovee+g{WGfl9=`VkvP^ZYud2avpyX=YdLA0*(xSo6UlKWYhkj1NPj_k}+ zWia!Ug#{az*Knjox-ZUkL?|~NQrOEzEGw`=nWBG&{X!@!mN&|ZJ-Qg*D>y5qNblvJ IQ~Yu6f6*i?4FCWD delta 119 zcmeDE&N#J|Q9mp*cYbJ|Oa4;^rV0na-i6;fuN#~Z-Rah~{N(#f?|W>Wr=FhVaX-r- zgHfG{QwFA)OO?NVw&~ivu;|fYLA{#eyka4JpHCfjTa&y(>h90F z!wd`z{Xp!gfCQ5B^NLFpQu0faa#9tFOOuo1lkZX4ImS2IM|C|1M3e3AR8D2R1gZpFaslz z4WuY!fuRkI3vgHiX-#XWcO{Yi3UVIEsmRU?gM>6B=s?a30b)?3gMyBF5f60^FVIjJ M0L8@SDH0PH0gUvK?*IS* delta 128 zcmdn%yu>6iEHih0Xr4>{QwFAeX1QIpPBArMzbk~6D#>z$cCCDxA$LeKCQ)*+-uFr$ z1_p*cAcg^cAcGNz`Oq*gNI}f1U7uJ$vTPey8H+OVZN4Y^nTZo50|uM56ecqQ035m< Aa{vGU diff --git a/dijkstra.cpp b/dijkstra.cpp index efdaf5d..6336b40 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -69,7 +69,7 @@ void Dijkstras::run_planner( 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) { @@ -78,42 +78,16 @@ void Dijkstras::run_planner( const double g_value = cost_map[parent_id] + transition_cost; - // if node is not in the priority queue, we need to add it - if (cost_map.find(succ_state_id) == cost_map.end()) { - cost_map[succ_state_id] = g_value; - Q.insert(succ_state_id); - child_to_parent_map[succ_state_id] = parent_id; - } else if (g_value < cost_map[succ_state_id]) { + 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); + if (g_value < cost_map[succ_state_id]) { + Q.erase(succ_state_id); + } Q.insert(succ_state_id); child_to_parent_map[succ_state_id] = parent_id; } } -/* - auto succ_state_id_iter = succ_ids.begin(); - auto transition_cost_iter = costs.begin(); - - while (succ_state_id_iter != succ_ids.end() && - transition_cost_iter != costs.end()) { - double g_value = cost_map[parent_id] + *transition_cost_iter; - - // if node is not in the priority queue, we need to add it - if (cost_map.find(*succ_state_id_iter) == cost_map.end()) { - cost_map[*succ_state_id_iter] = g_value; - Q.insert(*succ_state_id_iter); - child_to_parent_map[*succ_state_id_iter] = parent_id; - } else if (g_value < cost_map[*succ_state_id_iter]) { - cost_map[*succ_state_id_iter] = g_value; - Q.erase(*succ_state_id_iter); - Q.insert(*succ_state_id_iter); - child_to_parent_map[*succ_state_id_iter] = parent_id; - } - - ++succ_state_id_iter; - ++transition_cost_iter; - } -*/ } // if Q is empty, we need to call extract_path diff --git a/graph.cpp b/graph.cpp index 5bed121..90e130f 100644 --- a/graph.cpp +++ b/graph.cpp @@ -39,7 +39,7 @@ int Graph::set_start_state(const int& x, const int& y) { if (is_valid_state(x, y)) { return m_start_id; // valid state - } + } return -1; // invalid state } @@ -69,7 +69,7 @@ void Graph::get_succs( 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; @@ -80,7 +80,8 @@ void Graph::get_succs( (*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); + const double succ_cost = get_action_cost(x_source, y_source, + x_succ, y_succ); (*costs).push_back(succ_cost); } } From e612fe1dcf3b401c3a8222a8fbe6d9130022e722 Mon Sep 17 00:00:00 2001 From: Ramya Date: Sat, 20 Apr 2019 15:54:23 -0700 Subject: [PATCH 54/54] Remove redundancy and unnecessary blank lines --- .dijkstra.cpp.un~ | Bin 31629 -> 34281 bytes .dijkstra.h.un~ | Bin 0 -> 2522 bytes .graph.cpp.un~ | Bin 8637 -> 10256 bytes dijkstra.cpp | 7 +------ dijkstra.h | 5 +---- graph.cpp | 4 ---- 6 files changed, 2 insertions(+), 14 deletions(-) create mode 100644 .dijkstra.h.un~ diff --git a/.dijkstra.cpp.un~ b/.dijkstra.cpp.un~ index 59c662e217d400b4c61b33ebcd6112b0c5b0e5c1..27fc7619297340d5d95b437984efb02e52d4ec42 100644 GIT binary patch delta 545 zcmeDE&iJyMDKRWFcYbJ|Oa4;^rX}ZM%3Wu!(aJgJzjWbbcJ1BXds>z{1TKE`c9qhN znEqW13=HK!3HsxcN=xd`7AG3qTPEAa(*` z7;pqK{5D@G&1SUOz2F2`69dCn381JiNCv75WEO*IBiICx4ri!1hyt1B4>rMJ_kt(L zCV+JNgOmUPvI!s^E?`jz0WvKJhy^w$in}sST%afk^nft3Nr6xm43qP8B_=;FR-61G zi&GM$&lQMa0Ayw`5W~&!0-7U&Y!1j4kO~k0xkyj}4b(zC2{H;6v>?T|Uq~b`%dJpmRVn MASFNm4W{R-08esckpKVy delta 120 zcmaFa&D8syF*z(VcYbJ|Oa4;^rddpDuW3rLo0V`4`oBg5ubmGc>yp05G` DywW21 diff --git a/.dijkstra.h.un~ b/.dijkstra.h.un~ new file mode 100644 index 0000000000000000000000000000000000000000..89f84b21cdc80f1dad3e1fde779f43a6cde30a89 GIT binary patch literal 2522 zcmWH`%$*;a=aT=Ffys2H&Lq*^TRBnlZ*(aev@ZSX7|i7msy4k#WaW%~(#oL>3=Gjg zY@+}LMX4pFMR^LvC5a`e@rLo4C8r3)$|DXMs(QH-1mL7o9c tHzr&(>c8Ara(6$whij!b7%e3+NDgYYsublt@ literal 0 HcmV?d00001 diff --git a/.graph.cpp.un~ b/.graph.cpp.un~ index 8d2006a55fff1b46466f389d3c3deab974c6ff1b..6c95332954b8d58c4586c91c9f3f700195ddf8f3 100644 GIT binary patch delta 376 zcmdn%JRu-4EHih0Xr4>{QwAo+m0RO9`h&g2uUa?QUE5xE%p@#*xlJ;kt$En)=l4`Y z7#JA3ffxonfec0<7J~|aXc%UQ*}ce*1tiP4aaFr4qwr=Uxz~)MLO=l#sB#b`48-c2 z_p@d*+U#EV8?2sz;j08tR1G8p#2^4NhQYKk{sNE!=@13+p#Wr>0n~&=g2*Q5gJhtp zkWB#TfH@f?4KmFdW`Yf}305E(kP;w3HUZgwkQ~UgD6k0%yBGOFd=2qMBuECN1PH(; KY!*{k&IkZH+&U)! delta 119 zcmbObu-92XEHih0Xr4>{QwFA)OO?NVw&~ivu;|fYLA{#eyka4JpHCfjTa&y(>h90F z!wd`z{gWHyef5BhU?BDcVn!eq0%8yV(J;&qv+7Vd3rLn@V`953Bg5vGvacC87pN^@ F1OV}?BZ2?` diff --git a/dijkstra.cpp b/dijkstra.cpp index 6336b40..783c463 100644 --- a/dijkstra.cpp +++ b/dijkstra.cpp @@ -75,15 +75,11 @@ void Dijkstras::run_planner( 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; - if (g_value < cost_map[succ_state_id]) { - Q.erase(succ_state_id); - } + Q.erase(succ_state_id); Q.insert(succ_state_id); child_to_parent_map[succ_state_id] = parent_id; } @@ -110,7 +106,6 @@ void Dijkstras::extract_path( (*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); diff --git a/dijkstra.h b/dijkstra.h index 77b2705..c0b5256 100644 --- a/dijkstra.h +++ b/dijkstra.h @@ -51,10 +51,7 @@ class CostMapComparator { bool operator()(const int& state_1, const int& state_2) const { - const auto state_1_iter = cost_map_.find(state_1); - const auto state_2_iter = cost_map_.find(state_2); - - return state_1_iter->second <= state_2_iter->second; + return cost_map_.find(state_1)->second <= cost_map_.find(state_2)->second; } private: diff --git a/graph.cpp b/graph.cpp index 90e130f..9cf455d 100644 --- a/graph.cpp +++ b/graph.cpp @@ -36,7 +36,6 @@ namespace graphs { 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 } @@ -46,7 +45,6 @@ int Graph::set_start_state(const int& x, const int& y) { 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 { @@ -58,7 +56,6 @@ void Graph::get_succs( const int& source_state_id, std::vector *succ_ids, std::vector *costs) const { - assert(source_state_id < m_occupancy_grid.size()); int x_source, y_source; @@ -90,7 +87,6 @@ void Graph::get_succs( void Graph::get_path_coordinates( const std::vector& path_state_ids, 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;