Cancellation phase

rmf_task/include/rmf_task/events/SimpleEventState.hpp

@@ -47,6 +47,11 @@ class SimpleEventState : public Event::State
   // Documentation inherited
   uint64_t id() const final;
 
+  /// Change the ID of this event state. Use this with great caution; the ID is
+  /// generally meant to remain constant. This method is only provided for very
+  /// niche cases.
+  SimpleEventState& modify_id(uint64_t new_id);
+
   // Documentation inherited
   Status status() const final;
 

rmf_task/src/rmf_task/events/SimpleEventState.cpp

@@ -63,6 +63,13 @@ uint64_t SimpleEventState::id() const
   return _pimpl->id;
 }
 
+//==============================================================================
+SimpleEventState& SimpleEventState::modify_id(uint64_t new_id)
+{
+  _pimpl->id = new_id;
+  return *this;
+}
+
 //==============================================================================
 Event::Status SimpleEventState::status() const
 {

rmf_task_sequence/src/rmf_task_sequence/Task.cpp

@@ -15,6 +15,8 @@
  *
 */
 
+#include "phases/internal_CancellationPhase.hpp"
+
 #include <list>
 
 #include <rmf_task/phases/RestoreBackup.hpp>
@@ -838,28 +840,63 @@ void Task::Active::_begin_next_stage(std::optional<nlohmann::json> restore)
 
     const auto phase_id = tag->id();
     _current_phase_start_time = _clock();
-    _active_phase = _phase_activator->activate(
-      _get_state,
-      _parameters,
-      std::move(tag),
-      *_active_stage->description,
-      std::move(restore),
-      [me = weak_from_this()](Phase::ConstSnapshotPtr snapshot)
-      {
-        if (const auto self = me.lock())
-          self->_update(snapshot);
-      },
-      [me = weak_from_this(), id = phase_id](
-        Phase::Active::Backup backup)
-      {
-        if (const auto self = me.lock())
-          self->_issue_backup(id, std::move(backup));
-      },
-      [me = weak_from_this()]()
-      {
-        if (const auto self = me.lock())
-          self->_finish_phase();
-      });
+
+    if (_cancelled_on_phase.has_value())
+    {
+      auto inner_phase = _phase_activator->activate(
+        _get_state,
+        _parameters,
+        tag,
+        *_active_stage->description,
+        std::move(restore),
+        [me = weak_from_this()](Phase::ConstSnapshotPtr snapshot)
+        {
+          if (const auto self = me.lock())
+          {
+            const auto active_phase = self->_active_phase;
+            if (active_phase)
+              self->_update(Phase::Snapshot::make(*active_phase));
+          }
+        },
+        [me = weak_from_this(), id = phase_id](
+          Phase::Active::Backup backup)
+        {
+          if (const auto self = me.lock())
+            self->_issue_backup(id, std::move(backup));
+        },
+        [me = weak_from_this()]()
+        {
+          if (const auto self = me.lock())
+            self->_finish_phase();
+        });
+
+      _active_phase = phases::CancellationPhase::make(tag, inner_phase);
+    }
+    else
+    {
+      _active_phase = _phase_activator->activate(
+        _get_state,
+        _parameters,
+        std::move(tag),
+        *_active_stage->description,
+        std::move(restore),
+        [me = weak_from_this()](Phase::ConstSnapshotPtr snapshot)
+        {
+          if (const auto self = me.lock())
+            self->_update(snapshot);
+        },
+        [me = weak_from_this(), id = phase_id](
+          Phase::Active::Backup backup)
+        {
+          if (const auto self = me.lock())
+            self->_issue_backup(id, std::move(backup));
+        },
+        [me = weak_from_this()]()
+        {
+          if (const auto self = me.lock())
+            self->_finish_phase();
+        });
+    }
 
     _update(Phase::Snapshot::make(*_active_phase));
     _issue_backup(phase_id, _active_phase->backup());

rmf_task_sequence/src/rmf_task_sequence/phases/CancellationPhase.cpp

@@ -0,0 +1,143 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "internal_CancellationPhase.hpp"
+
+namespace rmf_task_sequence {
+namespace phases {
+
+//==============================================================================
+std::shared_ptr<CancellationPhase> CancellationPhase::make(
+  ConstTagPtr tag,
+  std::shared_ptr<Phase::Active> phase)
+{
+  auto result = std::shared_ptr<CancellationPhase>(new CancellationPhase);
+  result->_tag = std::make_shared<Phase::Tag>(
+    tag->id(),
+    Header(
+      "Cancellation in progress: " + tag->header().category(),
+      "Cancellation in progress: " + tag->header().detail(),
+      tag->header().original_duration_estimate()));
+
+  result->_state = rmf_task::events::SimpleEventState::make(
+    0,
+    result->_tag->header().category(),
+    result->_tag->header().detail(),
+    rmf_task::Event::Status::Canceled,
+    {},
+    nullptr);
+
+  result->_phase = std::move(phase);
+  return result;
+}
+
+//==============================================================================
+auto CancellationPhase::tag() const -> ConstTagPtr
+{
+  return _tag;
+}
+
+//==============================================================================
+Event::ConstStatePtr CancellationPhase::final_event() const
+{
+  const auto child_event = _phase->final_event();
+  _state->update_dependencies({child_event});
+  const auto child_status = child_event->status();
+  if (Event::Status::Blocked == child_status
+    || Event::Status::Error == child_status
+    || Event::Status::Failed == child_status)
+  {
+    // Promote the child status into the overall cancellation phase status
+    // in case something needs the attention of the operator.
+    _state->update_status(child_status);
+  }
+  else
+  {
+    _state->update_status(rmf_task::Event::Status::Canceled);
+  }
+
+  std::vector<Event::ConstStatePtr> queue;
+  std::unordered_set<Event::ConstStatePtr> visited;
+  uint64_t highest_index = 0;
+
+  queue.push_back(child_event);
+  while (!queue.empty())
+  {
+    const Event::ConstStatePtr e = queue.back();
+    queue.pop_back();
+
+    if (!e)
+    {
+      // A nullptr for some reason..?
+      continue;
+    }
+
+    if (!visited.insert(e).second)
+    {
+      // This event state was already visited in the past
+      continue;
+    }
+
+    for (const auto& d : e->dependencies())
+    {
+      queue.push_back(d);
+    }
+
+    highest_index = std::max(highest_index, e->id());
+  }
+
+  // Set the cancellation event ID to one higher than any other event currently
+  // active in the tree. That way we can give it a valid event ID that doesn't
+  // conflict with any other events in the phase.
+  _state->modify_id(highest_index+1);
+
+  return _state;
+}
+
+//==============================================================================
+rmf_traffic::Duration CancellationPhase::estimate_remaining_time() const
+{
+  return _phase->estimate_remaining_time();
+}
+
+//==============================================================================
+auto CancellationPhase::backup() const -> Backup
+{
+  return _phase->backup();
+}
+
+//==============================================================================
+auto CancellationPhase::interrupt(std::function<void()> task_is_interrupted)
+-> Resume
+{
+  return _phase->interrupt(std::move(task_is_interrupted));
+}
+
+//==============================================================================
+void CancellationPhase::cancel()
+{
+  _phase->cancel();
+}
+
+//==============================================================================
+void CancellationPhase::kill()
+{
+  _phase->kill();
+}
+
+} // namespace phases
+} // namespace rmf_task_sequence

rmf_task_sequence/src/rmf_task_sequence/phases/internal_CancellationPhase.hpp

@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_CANCELLATION_PHASE_HPP
+#define SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_CANCELLATION_PHASE_HPP
+
+#include <rmf_task/events/SimpleEventState.hpp>
+
+#include <rmf_task_sequence/Phase.hpp>
+
+namespace rmf_task_sequence {
+namespace phases {
+
+class CancellationPhase : public Phase::Active
+{
+public:
+  using ConstTagPtr = rmf_task::Phase::ConstTagPtr;
+
+  static std::shared_ptr<CancellationPhase> make(
+    ConstTagPtr tag,
+    std::shared_ptr<Phase::Active> phase);
+
+  ConstTagPtr tag() const final;
+
+  Event::ConstStatePtr final_event() const final;
+
+  rmf_traffic::Duration estimate_remaining_time() const final;
+
+  Backup backup() const final;
+
+  Resume interrupt(std::function<void()> task_is_interrupted) final;
+
+  void cancel() final;
+
+  void kill() final;
+
+private:
+  CancellationPhase() = default;
+
+  ConstTagPtr _tag;
+  rmf_task::events::SimpleEventStatePtr _state;
+  std::shared_ptr<Phase::Active> _phase;
+};
+
+} // namespace phases
+} // namespace rmf_task_sequences
+
+#endif // SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_CANCELLATION_PHASE_HPP