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

updated when goal_reached is sent

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 16:59:10 -05:00
parent ddddd70d66
commit 8f903eaff1
1 changed files with 17 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+17 -13
View File
@@ -1052,7 +1052,7 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
}
else
{
ROS_WARN("Planning: Cannot compute a path (or goal is already reached)!");
ROS_WARN("Planning: Cannot compute a path!");
rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers())
{
@@ -1441,23 +1441,27 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
const move_base_msgs::MoveBaseResultConstPtr& result)
{
if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
if(!currentMetricGoal_.isNull())
{
ROS_INFO("Planning: move_base success!");
}
else
{
ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Planning: move_base success!");
}
else
{
ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
}
if(!goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
goalReachedPub_.publish(result);
}
}
rtabmap_.clearPath();
currentMetricGoal_.setNull();
if(goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
goalReachedPub_.publish(result);
}
}
// Called once when the goal becomes active