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

fixed last goal not reached in localization mode

Browse Source
This commit is contained in:
matlabbe
2015-11-25 15:46:56 -05:00
parent aa05bc4672
commit b44b8ca732
2 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -17,7 +17,7 @@ find_package(octomap_ros)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.10.11 REQUIRED) find_package(RTABMap 0.10.12 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
src/CoreWrapper.cpp
+2 -2
View File
@@ -1291,7 +1291,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_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() ||
rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius())
{ {
latestNodeWasReached_ = true; latestNodeWasReached_ = true;
@@ -1411,7 +1411,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_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
{ {
latestNodeWasReached_ = true; latestNodeWasReached_ = true;
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();