i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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

1
corelib/include/rtabmap/core/Parameters.h
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.");

10
corelib/include/rtabmap/core/Rtabmap.h
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;
};