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

Updated for 0.10.9: can abort the current plan if a node is not reachable (see RGBD/PlanStuckIterations parameter)

Browse Source
This commit is contained in:
matlabbe
2015-09-23 18:17:46 -04:00
parent 624aded6de
commit 7deff58782
2 changed files with 19 additions and 13 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.8 REQUIRED) find_package(RTABMap 0.10.9 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
src/CoreWrapper.cpp
+18 -12
View File
@@ -1250,12 +1250,19 @@ void CoreWrapper::process(
{ {
if(rtabmap_.getPath().size() == 0) if(rtabmap_.getPath().size() == 0)
{ {
// Goal reached if(rtabmap_.getPathStatus() > 0)
ROS_INFO("Planning: Publishing goal reached!"); {
// Goal reached
ROS_INFO("Planning: Publishing goal reached!");
}
else
{
ROS_WARN("Planning: Plan failed!");
}
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
std_msgs::Bool result; std_msgs::Bool result;
result.data = true; result.data = rtabmap_.getPathStatus() > 0;
goalReachedPub_.publish(result); goalReachedPub_.publish(result);
} }
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
@@ -1291,7 +1298,7 @@ void CoreWrapper::process(
{ {
ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)", ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)",
rtabmap_.getPathCurrentGoalId()); rtabmap_.getPathCurrentGoalId());
rtabmap_.clearPath(); rtabmap_.clearPath(-1);
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
std_msgs::Bool result; std_msgs::Bool result;
@@ -1359,10 +1366,9 @@ void CoreWrapper::goalCommonCallback(
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
if(poses.size() == 0) if(poses.size() == 0)
{ {
ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm) or too far from the graph (RGBD/GoalMaxDistance=%fm).", ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
rtabmap_.getGoalReachedRadius(), rtabmap_.getGoalReachedRadius());
rtabmap_.getLocalRadius()); rtabmap_.clearPath(1);
rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
std_msgs::Bool result; std_msgs::Bool result;
@@ -1421,12 +1427,12 @@ void CoreWrapper::goalCommonCallback(
} }
else else
{ {
ROS_ERROR("Planning: A node near the goal's pose not found!"); ROS_ERROR("Planning: A node near the goal's pose not found! The pose may be to far from the graph (RGBD/LocalRadius=%f m)", rtabmap_.getLocalRadius());
} }
if(!success) if(!success)
{ {
rtabmap_.clearPath(); rtabmap_.clearPath(-1);
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
std_msgs::Bool result; std_msgs::Bool result;
@@ -1910,7 +1916,7 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
if(rtabmap_.getPath().size()) if(rtabmap_.getPath().size())
{ {
ROS_WARN("Goal cancelled!"); ROS_WARN("Goal cancelled!");
rtabmap_.clearPath(); rtabmap_.clearPath(0);
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
if(mbClient_.isServerConnected()) if(mbClient_.isServerConnected())
@@ -2172,7 +2178,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
if(!ignore) if(!ignore)
{ {
rtabmap_.clearPath(); rtabmap_.clearPath(1);
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
} }