i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Using local radius instead of goal radius to trigger the latest pose to reach

Browse Source
This commit is contained in:
Mathieu Labbe
2016-07-05 15:30:41 -04:00
parent 247b6a70e6
commit 3d70d2fbad
1 changed files with 2 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+2 -3
View File
@@ -1513,8 +1513,7 @@ void CoreWrapper::process(
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
{ {
if(latestNodeWasReached_ || if(latestNodeWasReached_ ||
rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius())
{ {
latestNodeWasReached_ = true; latestNodeWasReached_ = true;
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
@@ -1634,7 +1633,7 @@ void CoreWrapper::goalCommonCallback(
// Adjust the target pose relative to last node // Adjust the target pose relative to last node
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) 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; latestNodeWasReached_ = true;
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();