From 3d70d2fbad9471cb8386c7ab202051659e3e0786 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Tue, 5 Jul 2016 15:30:41 -0400 Subject: [PATCH] Using local radius instead of goal radius to trigger the latest pose to reach --- src/CoreWrapper.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 859a2609..2d7b240e 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1513,8 +1513,7 @@ void CoreWrapper::process( if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) { if(latestNodeWasReached_ || - rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || - rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) + rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius()) { latestNodeWasReached_ = true; currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); @@ -1634,7 +1633,7 @@ void CoreWrapper::goalCommonCallback( // Adjust the target pose relative to last node if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) { - if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) + if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius()) { latestNodeWasReached_ = true; currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();