diff --git a/rmf_task/include/rmf_task/events/SimpleEventState.hpp b/rmf_task/include/rmf_task/events/SimpleEventState.hpp index dcc74855..c90d528e 100644 --- a/rmf_task/include/rmf_task/events/SimpleEventState.hpp +++ b/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; diff --git a/rmf_task/src/rmf_task/events/SimpleEventState.cpp b/rmf_task/src/rmf_task/events/SimpleEventState.cpp index f5ce6036..1af9d685 100644 --- a/rmf_task/src/rmf_task/events/SimpleEventState.cpp +++ b/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 { diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 4cfe7498..dd2d3874 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -15,6 +15,8 @@ * */ +#include "phases/internal_CancellationPhase.hpp" + #include #include @@ -838,28 +840,63 @@ void Task::Active::_begin_next_stage(std::optional 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()); diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/CancellationPhase.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/CancellationPhase.cpp new file mode 100644 index 00000000..43adcbcb --- /dev/null +++ b/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::make( + ConstTagPtr tag, + std::shared_ptr phase) +{ + auto result = std::shared_ptr(new CancellationPhase); + result->_tag = std::make_shared( + 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 queue; + std::unordered_set 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 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 diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_CancellationPhase.hpp b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_CancellationPhase.hpp new file mode 100644 index 00000000..eebb720b --- /dev/null +++ b/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 + +#include + +namespace rmf_task_sequence { +namespace phases { + +class CancellationPhase : public Phase::Active +{ +public: + using ConstTagPtr = rmf_task::Phase::ConstTagPtr; + + static std::shared_ptr make( + ConstTagPtr tag, + std::shared_ptr 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 task_is_interrupted) final; + + void cancel() final; + + void kill() final; + +private: + CancellationPhase() = default; + + ConstTagPtr _tag; + rmf_task::events::SimpleEventStatePtr _state; + std::shared_ptr _phase; +}; + +} // namespace phases +} // namespace rmf_task_sequences + +#endif // SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_CANCELLATION_PHASE_HPP