Add preempt goal functionality to locomove_base (#40)

* Add preempt goal functionality to locomove_base

* Fix whitespace issue

Author: mbuijs

locomove_base/include/locomove_base/locomove_base.h

@@ -103,6 +103,7 @@ class LocoMoveBase
 
   // MoveBaseAction
   void executeCB();
+  void preemptCB();
   actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> server_;
 
   // Recoveries

locomove_base/src/locomove_base.cpp

@@ -203,6 +203,7 @@ LocoMoveBase::LocoMoveBase(const ros::NodeHandle& nh) :
   goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&LocoMoveBase::goalCB, this, _1));
 
   server_.registerGoalCallback(std::bind(&LocoMoveBase::executeCB, this));
+  server_.registerPreemptCallback(std::bind(&LocoMoveBase::preemptCB, this));
   server_.start();
 
   resetState();
@@ -628,6 +629,15 @@ void LocoMoveBase::executeCB()
   setGoal(nav_2d_utils::poseStampedToPose2D(move_base_goal->target_pose));
 }
 
+void LocoMoveBase::preemptCB()
+{
+  ROS_INFO("Preempting goal");
+  plan_loop_timer_.stop();
+  control_loop_timer_.stop();
+  resetState();
+  server_.setPreempted();
+}
+
 }  // namespace locomove_base
 
 int main(int argc, char** argv)