Allow automatic action cancellation to be toggled (#392)

Signed-off-by: Michael X. Grey <[email protected]>

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -247,6 +247,19 @@ class RobotUpdateHandle
     /// Returns false if the Action has been killed or cancelled
     bool okay() const;
 
+    /// Set whether automatic cancellation is turned on for this action.
+    ///
+    /// When automatic cancellation is on, the task system will believe that the
+    /// action is successfully cancelled immediately upon receiving a cancel
+    /// signal. By default, automatic cancellation is on (true).
+    ///
+    /// If your action needs to perform some kind of wind-down or cleanup after
+    /// being cancelled, then you should set this to false. At that point you
+    /// must ensure that your action implementation is watching okay() to know
+    /// if it has been cancelled, and you must trigger finished() when your
+    /// wind-down or cleanup is finished.
+    void set_automatic_cancel(bool on);
+
     /// Activity identifier for this action. Used by the EasyFullControl API.
     ConstActivityIdentifierPtr identifier() const;
 

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp

@@ -1245,6 +1245,22 @@ bool RobotUpdateHandle::ActionExecution::okay() const
   return _pimpl->data->okay;
 }
 
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::set_automatic_cancel(bool on)
+{
+  if (_pimpl->data)
+  {
+    if (const auto context = _pimpl->data->w_context.lock())
+    {
+      context->worker().schedule(
+        [on, self = _pimpl->data](const auto&)
+        {
+          self->automatic_cancel = on;
+        });
+    }
+  }
+}
+
 //==============================================================================
 auto RobotUpdateHandle::ActionExecution::identifier() const
 -> ConstActivityIdentifierPtr

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp

@@ -156,6 +156,7 @@ class RobotUpdateHandle::ActionExecution::Implementation
     std::optional<rmf_traffic::Duration> remaining_time;
     bool request_replan;
     bool okay;
+    bool automatic_cancel;
     std::optional<ScheduleOverride> schedule_override;
 
     void update_location(
@@ -213,7 +214,8 @@ class RobotUpdateHandle::ActionExecution::Implementation
       state(std::move(state_)),
       remaining_time(remaining_time_),
       request_replan(false),
-      okay(true)
+      okay(true),
+      automatic_cancel(true)
     {
       // Do nothing
     }

rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp

@@ -221,22 +221,41 @@ auto PerformAction::Active::interrupt(
 //==============================================================================
 void PerformAction::Active::cancel()
 {
-  _state->update_status(Status::Canceled);
-  _state->update_log().info("Received signal to cancel");
-  auto self = shared_from_this();
-  _finished();
-  if (auto data = _execution_data.lock())
+  const auto self = shared_from_this();
+  self->_state->update_status(Status::Canceled);
+  self->_state->update_log().info("Received signal to cancel");
+  if (auto data = self->_execution_data.lock())
+  {
     data->okay = false;
+    if (data->automatic_cancel)
+    {
+      data->finished.trigger();
+    }
+  }
+  else
+  {
+    // If the action could not be obtained, then we will forcibly move on
+    self->_finished();
+  }
 }
 
 //==============================================================================
 void PerformAction::Active::kill()
 {
-  _state->update_status(Status::Killed);
-  _state->update_log().info("Received signal to kill");
-  _finished();
-  if (auto data = _execution_data.lock())
+  const auto self = shared_from_this();
+  self->_state->update_status(Status::Killed);
+  self->_state->update_log().info("Received signal to kill");
+  if (auto data = self->_execution_data.lock())
+  {
     data->okay = false;
+    // During a kill, we always immediately move on
+    data->finished.trigger();
+  }
+  else
+  {
+    // If the action could not be obtained, then we will forcibly move on
+    self->_finished();
+  }
 }
 
 //==============================================================================

rmf_fleet_adapter_python/src/adapter.cpp

@@ -361,6 +361,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("hold") = 0.0)
   .def("finished", &ActionExecution::finished)
   .def("okay", &ActionExecution::okay)
+  .def("set_automatic_cancel", &ActionExecution::set_automatic_cancel, py::arg("on"))
   .def_property_readonly("identifier", &ActionExecution::identifier);
 
   // ROBOT INTERRUPTION   ====================================================