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 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 else
{ {
ROS_WARN("Planning: Cannot compute a path (or goal is already reached)!"); ROS_WARN("Planning: Cannot compute a path!");
rtabmap_.clearPath(); rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
@@ -1441,23 +1441,27 @@ 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)
{ {
if(state == actionlib::SimpleClientGoalState::SUCCEEDED) if(!currentMetricGoal_.isNull())
{ {
ROS_INFO("Planning: move_base success!"); if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
} {
else ROS_INFO("Planning: move_base success!");
{ }
ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan..."); 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(); rtabmap_.clearPath();
currentMetricGoal_.setNull(); 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 // Called once when the goal becomes active