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

Added warning when move_base tells it reached current goal which is not the last

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-23 16:24:52 -05:00
parent ed90b6c1f8
commit 0ffb5c4b31
1 changed files with 20 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+20 -4
View File
@@ -2095,18 +2095,31 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state, void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
const move_base_msgs::MoveBaseResultConstPtr& result) const move_base_msgs::MoveBaseResultConstPtr& result)
{ {
bool ignore = false;
if(!currentMetricGoal_.isNull()) if(!currentMetricGoal_.isNull())
{ {
if(state == actionlib::SimpleClientGoalState::SUCCEEDED) if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
{ {
ROS_INFO("Planning: move_base success!"); if(rtabmap_.getPath().size() &&
rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first))
{
ROS_WARN("Planning: move_base reached current goal but it is not "
"the last one planned by rtabmap. A new goal should be sent when "
"rtabmap will be able to retrieve next locations on the path.");
ignore = true;
}
else
{
ROS_INFO("Planning: move_base success!");
}
} }
else else
{ {
ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan..."); ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
} }
if(!goalReachedPub_.getNumSubscribers()) if(!ignore && !goalReachedPub_.getNumSubscribers())
{ {
std_msgs::Bool result; std_msgs::Bool result;
result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED; result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
@@ -2114,8 +2127,11 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
} }
} }
rtabmap_.clearPath(); if(!ignore)
currentMetricGoal_.setNull(); {
rtabmap_.clearPath();
currentMetricGoal_.setNull();
}
} }
// Called once when the goal becomes active // Called once when the goal becomes active