From 0172b28a07cf9e02e38c016401af52b8a00c9286 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Fri, 6 Feb 2015 18:07:49 -0500 Subject: [PATCH] sending current pose even if use_action_for_goal is true --- src/CoreWrapper.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 59ee61e2..3fed1270 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -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); } } }