From fe32085fea803f1732a27bbb87e60873a1fa9cfb Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 15 Sep 2022 14:51:28 -0700 Subject: [PATCH 1/3] check if a node exists before creating --- nav2_smac_planner/src/a_star.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index deaa597b923..b560d646423 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -117,9 +117,12 @@ template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { - // Emplace will only create a new object if it doesn't already exist. - // If an element exists, it will return the existing object, not create a new one. - return &(_graph.emplace(index, NodeT(index)).first->second); + auto iter = graph_.find(index); + if (iter == graph_.end()) { + return &(_graph.emplace(index, NodeT(index)).first->second); + } + + return &(iter->second); } template<> From 4d4aa1793f56b797a444327ca7d4ed9108c615b8 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 15 Sep 2022 14:55:57 -0700 Subject: [PATCH 2/3] invert logic to group like with like --- nav2_smac_planner/src/a_star.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index b560d646423..1145898e612 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -118,11 +118,11 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { auto iter = graph_.find(index); - if (iter == graph_.end()) { - return &(_graph.emplace(index, NodeT(index)).first->second); + if (iter != graph_.end()) { + return &(iter->second); } - return &(iter->second); + return &(_graph.emplace(index, NodeT(index)).first->second); } template<> From 4c68db0baea1bb5ffcc40cf743025c40e1da982e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 15 Sep 2022 15:12:39 -0700 Subject: [PATCH 3/3] Update a_star.cpp --- nav2_smac_planner/src/a_star.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 1145898e612..b6a56271a93 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -117,8 +117,8 @@ template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { - auto iter = graph_.find(index); - if (iter != graph_.end()) { + auto iter = _graph.find(index); + if (iter != _graph.end()) { return &(iter->second); }