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

Merge branch 'master' of https://github.com/introlab/rtabmap_ros into devel

Browse Source
This commit is contained in:
Mathieu Labbe
2016-07-05 16:46:13 -04:00
parent 77faedf9d2 3d70d2fbad
commit fb09c96ba1
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
@@ -1553,8 +1553,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();
@@ -1677,7 +1676,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();