i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 13:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Version 0.10.9: added RGBD/PlanStuckIterations parameter (default 0) to detect when a node in the planned path is unreachable

Browse Source
This commit is contained in:
matlabbe
2015-09-23 18:15:34 -04:00
parent bb2ea8a3a3
commit 88e67f047a
7 changed files with 110 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 10)
SET(RTABMAP_PATCH_VERSION 8)
SET(RTABMAP_PATCH_VERSION 9)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -294,6 +294,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
corelib/include/rtabmap/core/Rtabmap.h
+8 -2
View File
@@ -129,8 +129,10 @@ public:
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures = 0);
void clearPath();
std::map<int, Signature> * signatures = 0);
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose); // only in current optimized map
const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
@@ -204,6 +206,7 @@ private:
float _goalReachedRadius; // meters
bool _planVirtualLinks;
bool _goalsSavedInUserData;
int _pathStuckIterations;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
@@ -235,10 +238,13 @@ private:
Transform _lastLocalizationPose; // for localization mode
// Planning stuff
int _pathStatus;
std::vector<std::pair<int,Transform> > _path;
std::set<unsigned int> _pathUnreachableNodes;
unsigned int _pathCurrentIndex;
unsigned int _pathGoalIndex;
Transform _pathTransformToGoal;
int _pathStuckCount;
};
corelib/src/Rtabmap.cpp
+71 -26
View File
@@ -111,6 +111,7 @@ Rtabmap::Rtabmap() :
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -124,9 +125,11 @@ Rtabmap::Rtabmap() :
_wDir("."),
_mapCorrection(Transform::getIdentity()),
_mapTransform(Transform::getIdentity()),
_pathStatus(0),
_pathCurrentIndex(0),
_pathGoalIndex(0),
_pathTransformToGoal(Transform::getIdentity())
_pathTransformToGoal(Transform::getIdentity()),
_pathStuckCount(0)
{
}
@@ -329,7 +332,7 @@ void Rtabmap::close()
_mapCorrection.setIdentity();
_mapTransform.setIdentity();
_lastLocalizationPose.setNull();
this->clearPath();
this->clearPath(0);
flushStatisticLogs();
if(_foutFloat)
@@ -414,6 +417,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -841,7 +845,7 @@ void Rtabmap::resetMemory()
_mapCorrection.setIdentity();
_mapTransform.setIdentity();
_lastLocalizationPose.setNull();
this->clearPath();
this->clearPath(0);
if(_memory)
{
@@ -3124,12 +3128,15 @@ void Rtabmap::getGraph(
}
}
void Rtabmap::clearPath()
void Rtabmap::clearPath(int status)
{
_pathStatus = status;
_path.clear();
_pathCurrentIndex=0;
_pathGoalIndex = 0;
_pathTransformToGoal.setIdentity();
_pathUnreachableNodes.clear();
_pathStuckCount = 0;
if(_memory)
{
_memory->removeAllVirtualLinks();
@@ -3140,7 +3147,7 @@ void Rtabmap::clearPath()
bool Rtabmap::computePath(int targetNode, bool global)
{
UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
this->clearPath();
this->clearPath(0);
if(!_rgbdSlamMode)
{
@@ -3235,7 +3242,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
{
UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
this->clearPath();
this->clearPath(0);
std::list<std::pair<int, Transform> > pathPoses;
if(!_rgbdSlamMode)
@@ -3316,7 +3323,6 @@ bool Rtabmap::computePath(const Transform & targetPose)
if(_path.size() == 0)
{
_path.clear();
UWARN("Cannot compute a path!");
}
else
@@ -3492,7 +3498,7 @@ void Rtabmap::updateGoalIndex()
!uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
{
UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
this->clearPath();
this->clearPath(-1);
return;
}
currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
@@ -3502,7 +3508,7 @@ void Rtabmap::updateGoalIndex()
if(_lastLocalizationPose.isNull())
{
UERROR("Last localization pose is null. Aborting the plan...");
this->clearPath();
this->clearPath(-1);
return;
}
currentPose = _lastLocalizationPose;
@@ -3516,31 +3522,36 @@ void Rtabmap::updateGoalIndex()
if(d < _goalReachedRadius)
{
UINFO("Goal %d reached!", goalId);
this->clearPath();
this->clearPath(1);
}
}
if(_path.size())
{
//Always check if the farthest node is accessible in local map (max to local space radius if set)
int goalIndex = _pathCurrentIndex;
unsigned int goalIndex = _pathCurrentIndex;
float distanceFromCurrentNode = 0.0f;
for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
bool sameGoalIndex = false;
for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
{
if(uContains(_optimizedPoses, _path[i].first))
{
if(_localRadius > 0.0f)
if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||
_pathUnreachableNodes.find(i) == _pathUnreachableNodes.end())
{
distanceFromCurrentNode = currentPose.getDistance(_optimizedPoses.at(_path[i].first));
}
if(_localRadius > 0.0f)
{
distanceFromCurrentNode = currentPose.getDistance(_optimizedPoses.at(_path[i].first));
}
if(distanceFromCurrentNode <= _localRadius)
{
goalIndex = i;
}
else
{
break;
if(distanceFromCurrentNode <= _localRadius)
{
goalIndex = i;
}
else
{
break;
}
}
}
else
@@ -3548,17 +3559,22 @@ void Rtabmap::updateGoalIndex()
break;
}
}
UASSERT(_pathGoalIndex < _path.size() && goalIndex >= 0 && goalIndex < (int)_path.size());
if((int)_pathGoalIndex != goalIndex)
UASSERT(_pathGoalIndex < _path.size() && goalIndex < _path.size());
if(_pathGoalIndex != goalIndex)
{
UINFO("Updated current goal from %d to %d (%d/%d)",
(int)_path[_pathGoalIndex].first, _path[goalIndex].first, goalIndex+1, (int)_path.size());
(int)_path[_pathGoalIndex].first, _path[goalIndex].first, (int)goalIndex+1, (int)_path.size());
_pathGoalIndex = goalIndex;
}
else
{
sameGoalIndex = true;
}
// update nearest pose in the path
unsigned int nearestNodeIndex = 0;
float distance = -1.0f;
bool sameCurrentIndex = false;
UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
{
@@ -3576,7 +3592,7 @@ void Rtabmap::updateGoalIndex()
if(distance < 0)
{
UERROR("The nearest pose on the path not found! Aborting the plan...");
this->clearPath();
this->clearPath(-1);
}
else
{
@@ -3585,6 +3601,35 @@ void Rtabmap::updateGoalIndex()
if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex)
{
_pathCurrentIndex = nearestNodeIndex;
_pathUnreachableNodes.erase(nearestNodeIndex); // if we are on it, it is reachable
}
else
{
sameCurrentIndex = true;
}
if(sameGoalIndex && sameCurrentIndex &&
_pathStuckIterations > 0 &&
++_pathStuckCount > _pathStuckIterations)
{
UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.",
_path[_pathGoalIndex].first,
_pathStuckCount,
_pathStuckIterations);
_pathStuckCount = 0;
_pathUnreachableNodes.insert(_pathGoalIndex);
// select previous one
if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
{
// plan failed!
UERROR("No upcoming nodes on the path are reachable! Aborting the plan...");
this->clearPath(-1);
return;
}
}
else if(!sameGoalIndex || !sameCurrentIndex)
{
_pathStuckCount = 0;
}
}
}
corelib/src/RtabmapThread.cpp
+1 -1
View File
@@ -265,7 +265,7 @@ void RtabmapThread::mainLoop()
this->post(new RtabmapGlobalPathEvent(id, parameters.at("label"), _rtabmap->getPath()));
break;
case kStateCancellingGoal:
_rtabmap->clearPath();
_rtabmap->clearPath(0);
break;
case kStateLabelling:
if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -587,6 +587,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
_ui->graphPlan_stuckIterations->setObjectName(Parameters::kRGBDPlanStuckIterations().c_str());
_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
guilib/src/ui/preferencesDialog.ui
+27 -7
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-349</y>
<y>-1094</y>
<width>760</width>
<height>1598</height>
<height>1655</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>9</number>
<number>19</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6986,14 +6986,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QCheckBox" name="graphPlan_planWithNearNodesLinked">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_space3_4">
<property name="text">
<string>Add virtual links. Before planning in the graph, near nodes are linked together. The maximum distance is defined by &quot;Goal reached radius&quot; above. If &quot;Maximum ID difference&quot; below is set, only close nodes in time can be linked together.</string>
@@ -7006,7 +7006,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_space3_5">
<property name="text">
<string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
@@ -7019,13 +7019,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_space2_4">
<property name="text">
<string>Mark the current goal node on the path as unreachable if it a new goal is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="graphPlan_stuckIterations">
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>