From 90ca9ba1457ffafbdb1815005747b22d0e2052fd Mon Sep 17 00:00:00 2001
From: "David V. Lu" <davidvlu@gmail.com>
Date: Thu, 2 Jul 2020 15:14:51 -0400
Subject: [PATCH] Fix for #43

---
 locomotor/include/locomotor/locomotor_action_server.h | 5 +++++
 locomotor/src/locomotor_action_server.cpp             | 1 +
 2 files changed, 6 insertions(+)

diff --git a/locomotor/include/locomotor/locomotor_action_server.h b/locomotor/include/locomotor/locomotor_action_server.h
index 09e4dd9..518bcee 100644
--- a/locomotor/include/locomotor/locomotor_action_server.h
+++ b/locomotor/include/locomotor/locomotor_action_server.h
@@ -43,6 +43,7 @@
 namespace locomotor
 {
 using NewGoalCallback = std::function<void (const nav_2d_msgs::Pose2DStamped&)>;
+using PreemptionCallback = std::function<void ()>;
 
 class LocomotorActionServer
 {
@@ -51,12 +52,16 @@ class LocomotorActionServer
   void publishFeedback(const locomotor_msgs::NavigationState& nav_state);
   void completeNavigation();
   void failNavigation(const locomotor_msgs::ResultCode& result_code);
+
+  // Note: This is a separate method so as not to change the previous constructor parameter list
+  void setPremptionCallback(PreemptionCallback cb) { preemption_cb_ = cb; }
 protected:
   void preGoalCallback();
   void preemptCallback();
   actionlib::SimpleActionServer<locomotor_msgs::NavigateToPoseAction> navigate_action_server_;
   locomotor_msgs::NavigateToPoseFeedback feedback_;
   NewGoalCallback goal_cb_;
+  PreemptionCallback preemption_cb_;
 };
 }  // namespace locomotor
 
diff --git a/locomotor/src/locomotor_action_server.cpp b/locomotor/src/locomotor_action_server.cpp
index ded6579..055da81 100644
--- a/locomotor/src/locomotor_action_server.cpp
+++ b/locomotor/src/locomotor_action_server.cpp
@@ -99,6 +99,7 @@ void LocomotorActionServer::preGoalCallback()
 void LocomotorActionServer::preemptCallback()
 {
   if (!navigate_action_server_.isActive()) return;
+  if (preemption_cb_) preemption_cb_();
   locomotor_msgs::NavigateToPoseResult result;
   result.result_code.result_code = -1;
   result.result_code.message = "Preempted.";