diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index c47944fe..35f9e32e 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/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; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 528539ad..5d8ffded 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/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 diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index 10a1c265..b3d63272 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -156,6 +156,7 @@ class RobotUpdateHandle::ActionExecution::Implementation std::optional remaining_time; bool request_replan; bool okay; + bool automatic_cancel; std::optional 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 } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp index 89d1aecc..1b129970 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp +++ b/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(); + } } //============================================================================== diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index a72e48aa..f68de76c 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/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 ====================================================