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

CMakeLists.txt

@@ -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

@@ -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

@@ -130,7 +130,9 @@ public:
 			bool optimized,
 			bool global,
 			std::map<int, Signature> * signatures = 0);
-	void clearPath();
+
+	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

@@ -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)
+						if(distanceFromCurrentNode <= _localRadius)
-					{
+						{
-						goalIndex = i;
+							goalIndex = i;
-					}
+						}
-					else
+						else
-					{
+						{
-						break;
+							break;
+						}
 					}
 				}
 				else
@@ -3548,17 +3559,22 @@ void Rtabmap::updateGoalIndex()
 					break;
 				}
 			}
-			UASSERT(_pathGoalIndex < _path.size() && goalIndex >= 0 && goalIndex < (int)_path.size());
+			UASSERT(_pathGoalIndex < _path.size() && goalIndex < _path.size());
-			if((int)_pathGoalIndex != goalIndex)
+			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

@@ -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

@@ -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

@@ -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>