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

sending current pose even if use_action_for_goal is true

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 18:07:49 -05:00
parent d81a9cef23
commit 0172b28a07
1 changed files with 3 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+3 -7
View File
@@ -1403,6 +1403,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
poseMsg.header.stamp = stamp;
rtabmap_ros::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathCurrentGoalId());
if(useActionForGoal_)
{
if(!mbClient_.isServerConnected())
@@ -1415,7 +1416,6 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
move_base_msgs::MoveBaseGoal goal;
goal.target_pose = poseMsg;
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathCurrentGoalId());
mbClient_.sendGoal(goal,
boost::bind(&CoreWrapper::goalDoneCb, this, _1, _2),
boost::bind(&CoreWrapper::goalActiveCb, this),
@@ -1426,13 +1426,9 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
ROS_ERROR("Cannot connect to move_base action server!");
}
}
else
if(nextMetricGoalPub_.getNumSubscribers())
{
if(nextMetricGoalPub_.getNumSubscribers())
{
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathCurrentGoalId());
nextMetricGoalPub_.publish(poseMsg);
}
nextMetricGoalPub_.publish(poseMsg);
}
}
}