Merge pull request #118 from yujinrobot/enableCB_fix

[diff_drive_pose_controller] Change target pose even if controller al…
yujinrobot · Oct 26, 2016 · 0fa79d6 · 0fa79d6
2 parents 38d4517 + ce834cb
commit 0fa79d6
Showing 1 changed file with 7 additions and 3 deletions.
diff --git a/yocs_diff_drive_pose_controller/src/diff_drive_pose_controller_ros.cpp b/yocs_diff_drive_pose_controller/src/diff_drive_pose_controller_ros.cpp
@@ -221,14 +221,18 @@ void DiffDrivePoseControllerROS::controlMaxVelCB(const std_msgs::Float32ConstPtr
 
 void DiffDrivePoseControllerROS::enableCB(const std_msgs::StringConstPtr msg)
 {
-  if (this->enable())
+  if (!msg->data.empty())
   {
     goal_frame_name_ = msg->data;
-    ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "] with goal frame [" << goal_frame_name_ << "]");
+  }
+
+  if (this->enable())
+  {
+    ROS_INFO_STREAM("Controller has been enabled [" << name_ << "] with goal frame [" << goal_frame_name_ << "].");
   }
   else
   {
-    ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"] with Goal frame [" << goal_frame_name_ << "]");
+    ROS_INFO_STREAM("Controller was already enabled [" << name_ <<"], now tracking goal frame [" << goal_frame_name_ << "].");
   }
 }