From 29f3b8c496ebaa4275fe9d866551b437d3d9f683 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 1 Sep 2021 22:53:19 +0800 Subject: [PATCH 01/85] Remove agv namespace and introduce CompositeData Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/CompositeData.hpp | 102 ++++++++++++++++ .../rmf_task/{agv => }/Constraints.hpp | 2 - rmf_task/include/rmf_task/Estimate.hpp | 8 +- .../include/rmf_task/{agv => }/Parameters.hpp | 2 - rmf_task/include/rmf_task/Request.hpp | 24 ++-- rmf_task/include/rmf_task/RequestFactory.hpp | 2 +- rmf_task/include/rmf_task/{agv => }/State.hpp | 6 +- .../rmf_task/{agv => }/TaskPlanner.hpp | 7 +- .../rmf_task/detail/impl_CompositeData.hpp | 72 ++++++++++++ .../rmf_task/requests/ChargeBattery.hpp | 4 +- .../requests/ChargeBatteryFactory.hpp | 4 +- rmf_task/include/rmf_task/requests/Clean.hpp | 4 +- .../include/rmf_task/requests/Delivery.hpp | 4 +- rmf_task/include/rmf_task/requests/Loop.hpp | 4 +- .../rmf_task/requests/ParkRobotFactory.hpp | 4 +- .../rmf_task/BinaryPriorityCostCalculator.cpp | 2 +- .../rmf_task/BinaryPriorityCostCalculator.hpp | 3 +- rmf_task/src/rmf_task/CompositeData.cpp | 75 ++++++++++++ rmf_task/src/rmf_task/CostCalculator.hpp | 3 +- rmf_task/src/rmf_task/Estimate.cpp | 10 +- rmf_task/src/rmf_task/agv/Constraints.cpp | 6 +- rmf_task/src/rmf_task/agv/Parameters.cpp | 6 +- rmf_task/src/rmf_task/agv/State.cpp | 5 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 6 +- .../rmf_task/agv/internal_task_planning.cpp | 4 +- .../rmf_task/agv/internal_task_planning.hpp | 6 +- .../src/rmf_task/requests/ChargeBattery.cpp | 18 +-- .../requests/ChargeBatteryFactory.cpp | 2 +- rmf_task/src/rmf_task/requests/Clean.cpp | 18 +-- rmf_task/src/rmf_task/requests/Delivery.cpp | 18 +-- rmf_task/src/rmf_task/requests/Loop.cpp | 18 +-- .../rmf_task/requests/ParkRobotFactory.cpp | 2 +- rmf_task/test/unit/agv/test_Constraints.cpp | 14 +-- rmf_task/test/unit/agv/test_State.cpp | 14 +-- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 110 +++++++++--------- 35 files changed, 408 insertions(+), 181 deletions(-) create mode 100644 rmf_task/include/rmf_task/CompositeData.hpp rename rmf_task/include/rmf_task/{agv => }/Constraints.hpp (98%) rename rmf_task/include/rmf_task/{agv => }/Parameters.hpp (98%) rename rmf_task/include/rmf_task/{agv => }/State.hpp (96%) rename rmf_task/include/rmf_task/{agv => }/TaskPlanner.hpp (98%) create mode 100644 rmf_task/include/rmf_task/detail/impl_CompositeData.hpp create mode 100644 rmf_task/src/rmf_task/CompositeData.cpp diff --git a/rmf_task/include/rmf_task/CompositeData.hpp b/rmf_task/include/rmf_task/CompositeData.hpp new file mode 100644 index 00000000..bb50bba8 --- /dev/null +++ b/rmf_task/include/rmf_task/CompositeData.hpp @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__COMPOSITEDATA_HPP +#define RMF_TASK__COMPOSITEDATA_HPP + +#include + +#include +#include + +namespace rmf_task { + +//============================================================================== +class CompositeData +{ +public: + + /// Create an empty CompositeData + CompositeData(); + + /// The result of performing an insertion operation + template + struct InsertResult + { + /// True if the value was inserted. This means that an entry of value T did + /// not already exist before you performed the insertion. + bool inserted; + + /// A reference to the value of type T that currently exists within the + /// CompositeData. + T* value; + }; + + /// Attempt to insert some data structure into the CompositeData. If a data + /// structure of type T already exists in the CompositeData, then this + /// function will have no effect, and InsertResult::value will point to + /// the value that already existed in the CompositeData. + /// + /// \param[in] value + /// The value to attempt to insert. + template + InsertResult insert(T&& value); + + /// Insert or assign some data structure into the CompositeData. If a data + /// structure of type T already exists in the CompositeData, then this + /// function will overwrite it with the new value. + /// + /// \param[in] value + /// The value to insert or assign. + template + InsertResult insert_or_assign(T&& value); + + /// Get a reference to a data structure of type T if one is available in the + /// CompositeData. If one is not available, this will return a nullptr. + template + T* get(); + + /// Get a reference to an immutable data structure of type T if one is + /// available in the CompositeData. If one is not available, this will return + /// a nullptr. + template + const T* get() const; + + /// Erase the data structure of type T if one is available in the + /// CompositeData. This will return true if it was erased, or false if type T + /// was not available. + template + bool erase(); + + /// Remove all data structures from this CompositeData + void clear(); + + class Implementation; +private: + std::any* _get(std::type_index type); + const std::any* _get(std::type_index type) const; + InsertResult _insert(std::any value, bool or_assign); + bool _erase(std::type_index type); + rmf_utils::impl_ptr _pimpl; +}; + + +} // namespace rmf_task + +#include + +#endif // RMF_TASK__COMPOSITEDATA_HPP diff --git a/rmf_task/include/rmf_task/agv/Constraints.hpp b/rmf_task/include/rmf_task/Constraints.hpp similarity index 98% rename from rmf_task/include/rmf_task/agv/Constraints.hpp rename to rmf_task/include/rmf_task/Constraints.hpp index 718685b9..6207838c 100644 --- a/rmf_task/include/rmf_task/agv/Constraints.hpp +++ b/rmf_task/include/rmf_task/Constraints.hpp @@ -21,7 +21,6 @@ #include namespace rmf_task { -namespace agv { //============================================================================== /// A class that describes constraints that are common among the agents/AGVs @@ -73,7 +72,6 @@ class Constraints rmf_utils::impl_ptr _pimpl; }; -} // namespace agv } // namespace rmf_task #endif // RMF_TASK__AGV__CONSTRAINTS_HPP diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index cf216122..42174de2 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -44,13 +44,13 @@ class Estimate /// /// \param[in] wait_until /// The ideal time the robot starts executing this request. - Estimate(agv::State finish_state, rmf_traffic::Time wait_until); + Estimate(State finish_state, rmf_traffic::Time wait_until); /// Finish state of the robot once it completes the request. - agv::State finish_state() const; + State finish_state() const; /// Sets a new finish state for the robot. - Estimate& finish_state(agv::State new_finish_state); + Estimate& finish_state(State new_finish_state); /// The ideal time the robot starts executing this request. rmf_traffic::Time wait_until() const; diff --git a/rmf_task/include/rmf_task/agv/Parameters.hpp b/rmf_task/include/rmf_task/Parameters.hpp similarity index 98% rename from rmf_task/include/rmf_task/agv/Parameters.hpp rename to rmf_task/include/rmf_task/Parameters.hpp index 08bf0f06..8a105a76 100644 --- a/rmf_task/include/rmf_task/agv/Parameters.hpp +++ b/rmf_task/include/rmf_task/Parameters.hpp @@ -27,7 +27,6 @@ #include namespace rmf_task { -namespace agv { //============================================================================== /// A class that containts parameters that are common among the agents/AGVs @@ -103,7 +102,6 @@ class Parameters }; -} // namespace agv } // namespace rmf_task #endif // RMF_TASK__AGV__PARAMETERS_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index e59cd55f..5299b665 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -20,9 +20,9 @@ #include -#include -#include -#include +#include +#include +#include #include #include @@ -42,11 +42,11 @@ class Request { public: - /// Estimate the state of the robot when the request is finished along with the - /// time the robot has to wait before commencing the request + /// Estimate the state of the robot when the request is finished along with + /// the time the robot has to wait before commencing the request virtual std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const = 0; /// Estimate the invariant component of the request's duration @@ -65,14 +65,14 @@ class Request /// description /// /// \param[in] earliest_start_time - /// The earliest time this request should begin execution. This is usually the - /// requested start time for the request. + /// The earliest time this request should begin execution. This is usually + /// the requested start time for the request. /// /// \param[in] parameters /// The parameters that describe this AGV virtual std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const = 0; + const Parameters& parameters) const = 0; virtual ~Description() = default; }; @@ -85,8 +85,8 @@ class Request /// The desired start time for this request /// /// \param[in] priority - /// The priority for this request. This is provided by the Priority Scheme. For - /// requests that do not have any priority this is a nullptr. + /// The priority for this request. This is provided by the Priority Scheme. + /// For requests that do not have any priority this is a nullptr. /// /// \param[in] description /// The description for this request diff --git a/rmf_task/include/rmf_task/RequestFactory.hpp b/rmf_task/include/rmf_task/RequestFactory.hpp index 2698d811..281052f4 100644 --- a/rmf_task/include/rmf_task/RequestFactory.hpp +++ b/rmf_task/include/rmf_task/RequestFactory.hpp @@ -31,7 +31,7 @@ class RequestFactory /// Generate a request for the AGV given the state that the robot will have /// when it is ready to perform the request virtual ConstRequestPtr make_request( - const agv::State& state) const = 0; + const State& state) const = 0; virtual ~RequestFactory() = default; }; diff --git a/rmf_task/include/rmf_task/agv/State.hpp b/rmf_task/include/rmf_task/State.hpp similarity index 96% rename from rmf_task/include/rmf_task/agv/State.hpp rename to rmf_task/include/rmf_task/State.hpp index 957ac793..4bdc1a26 100644 --- a/rmf_task/include/rmf_task/agv/State.hpp +++ b/rmf_task/include/rmf_task/State.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__AGV__STATE_HPP -#define RMF_TASK__AGV__STATE_HPP +#ifndef RMF_TASK__STATE_HPP +#define RMF_TASK__STATE_HPP #include @@ -26,7 +26,6 @@ #include namespace rmf_task { -namespace agv { //============================================================================== /// A class that is used to describe the state of an AGV @@ -90,7 +89,6 @@ class State rmf_utils::impl_ptr _pimpl; }; -} // namespace agv } // namespace rmf_task #endif // RMF_TASK__AGV__STATE_HPP diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp similarity index 98% rename from rmf_task/include/rmf_task/agv/TaskPlanner.hpp rename to rmf_task/include/rmf_task/TaskPlanner.hpp index 5d3120e7..fdc78682 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -21,8 +21,9 @@ #include #include #include -#include -#include +#include +#include +#include #include @@ -32,7 +33,6 @@ #include namespace rmf_task { -namespace agv { //============================================================================== class TaskPlanner @@ -257,7 +257,6 @@ class TaskPlanner }; -} // namespace agv } // namespace rmf_task #endif // RMF_TASK__AGV__TASKPLANNER_HPP diff --git a/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp b/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp new file mode 100644 index 00000000..d4d8fb65 --- /dev/null +++ b/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__DETAIL__IMPL_COMPOSITEDATA_HPP +#define RMF_TASK__DETAIL__IMPL_COMPOSITEDATA_HPP + +#include + +namespace rmf_task { + +namespace detail { +//============================================================================== +template +CompositeData::InsertResult insertion_cast( + CompositeData::InsertResult result) +{ + return {result.inserted, std::any_cast(result.value)}; +} +} // namespace detail + +//============================================================================== +template +auto CompositeData::insert(T&& value) -> InsertResult +{ + return detail::insertion_cast(_insert(std::any(std::move(value)), false)); +} + +//============================================================================== +template +auto CompositeData::insert_or_assign(T&& value) -> InsertResult +{ + return detail::insertion_cast(_insert(std::any(std::move(value)), true)); +} + +//============================================================================== +template +T* CompositeData::get() +{ + return std::any_cast(_get(typeid(T))); +} + +//============================================================================== +template +const T* CompositeData::get() const +{ + return std::any_cast(_get(typeid(T))); +} + +//============================================================================== +template +bool CompositeData::erase() +{ + return _erase(typeid(T)); +} + +} // namespace rmf_task + +#endif // RMF_TASK__DETAIL__IMPL_COMPOSITEDATA_HPP diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 1171dbaf..45a0822e 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -56,7 +56,7 @@ class ChargeBattery /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + const Parameters& parameters) const final; class Implementation; private: diff --git a/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp b/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp index 3a4b0f62..85797672 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp @@ -19,7 +19,7 @@ #define RMF_TASK__REQUESTS__FACTORY__CHARGEBATTERYFACTORY_HPP #include -#include +#include #include @@ -39,7 +39,7 @@ class ChargeBatteryFactory : public RequestFactory /// Documentation inherited ConstRequestPtr make_request( - const agv::State& state) const final; + const State& state) const final; class Implementation; diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index eb92c482..1ed42b83 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -59,7 +59,7 @@ class Clean /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + const Parameters& parameters) const final; /// Get the start waypoint in this request std::size_t start_waypoint() const; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 10a74da3..758eece4 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -61,7 +61,7 @@ class Delivery /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + const Parameters& parameters) const final; /// Get the pickup waypoint in this request std::size_t pickup_waypoint() const; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 33c74e53..b9e31735 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -30,7 +30,7 @@ #include -#include +#include #include #include @@ -60,7 +60,7 @@ class Loop /// Documentation inherited std::shared_ptr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + const Parameters& parameters) const final; /// Get the start waypoint of the loop in this request std::size_t start_waypoint() const; diff --git a/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp b/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp index 254167b1..c33b2ebc 100644 --- a/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp +++ b/rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp @@ -19,7 +19,7 @@ #define RMF_TASK__REQUESTS__FACTORY__PARKROBOTFACTORY_HPP #include -#include +#include #include @@ -49,7 +49,7 @@ class ParkRobotFactory : public RequestFactory /// Documentation inherited ConstRequestPtr make_request( - const agv::State& state) const final; + const State& state) const final; class Implementation; diff --git a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp index 2a8c542a..1c446fdf 100644 --- a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp +++ b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp @@ -220,7 +220,7 @@ double BinaryPriorityCostCalculator::compute_cost( //============================================================================== double BinaryPriorityCostCalculator::compute_cost( - rmf_task::agv::TaskPlanner::Assignments assignments) const + rmf_task::TaskPlanner::Assignments assignments) const { return compute_g(assignments); } diff --git a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.hpp b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.hpp index 8424095c..a8bfdb6d 100644 --- a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.hpp +++ b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.hpp @@ -39,10 +39,9 @@ class BinaryPriorityCostCalculator : public CostCalculator /// Compute the cost of assignments double compute_cost( - rmf_task::agv::TaskPlanner::Assignments assignments) const final; + rmf_task::TaskPlanner::Assignments assignments) const final; private: - using TaskPlanner = rmf_task::agv::TaskPlanner; using Assignments = TaskPlanner::Assignments; double _priority_penalty; diff --git a/rmf_task/src/rmf_task/CompositeData.cpp b/rmf_task/src/rmf_task/CompositeData.cpp new file mode 100644 index 00000000..9a0a9775 --- /dev/null +++ b/rmf_task/src/rmf_task/CompositeData.cpp @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2021 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 + +#include + +namespace rmf_task { + +//============================================================================== +class CompositeData::Implementation +{ +public: + + std::unordered_map data; + +}; + +//============================================================================== +CompositeData::CompositeData() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +std::any* CompositeData::_get(std::type_index type) +{ + const auto it = _pimpl->data.find(type); + if (it == _pimpl->data.end()) + return nullptr; + + return &it->second; +} + +//============================================================================== +const std::any* CompositeData::_get(std::type_index type) const +{ + return const_cast(this)->_get(type); +} + +//============================================================================== +auto CompositeData::_insert(std::any value, bool or_assign) +-> InsertResult +{ + const auto insertion = _pimpl->data.insert({value.type(), std::move(value)}); + if (!insertion.second && or_assign) + { + insertion.first->second = std::move(value); + } + + return {insertion.second, &insertion.first->second}; +} + +//============================================================================== +bool CompositeData::_erase(std::type_index type) +{ + return _pimpl->data.erase(type) > 0; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/CostCalculator.hpp b/rmf_task/src/rmf_task/CostCalculator.hpp index 61e41240..fe7f16ff 100644 --- a/rmf_task/src/rmf_task/CostCalculator.hpp +++ b/rmf_task/src/rmf_task/CostCalculator.hpp @@ -27,7 +27,6 @@ namespace rmf_task { class CostCalculator { public: - using Node = agv::Node; /// Compute the total cost of a node while factoring in the prioritization scheme virtual double compute_cost( @@ -37,7 +36,7 @@ class CostCalculator /// Compute the cost of assignments virtual double compute_cost( - rmf_task::agv::TaskPlanner::Assignments assignments) const = 0; + rmf_task::TaskPlanner::Assignments assignments) const = 0; virtual ~CostCalculator() = default; }; diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index d7831d49..0f643623 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -27,30 +27,30 @@ class Estimate::Implementation { public: - Implementation(agv::State finish_state, rmf_traffic::Time wait_until) + Implementation(State finish_state, rmf_traffic::Time wait_until) : _finish_state(std::move(finish_state)), _wait_until(std::move(wait_until)) {} - agv::State _finish_state; + State _finish_state; rmf_traffic::Time _wait_until; }; //============================================================================== -Estimate::Estimate(agv::State finish_state, rmf_traffic::Time wait_until) +Estimate::Estimate(State finish_state, rmf_traffic::Time wait_until) : _pimpl(rmf_utils::make_impl( std::move(finish_state), std::move(wait_until))) { } //============================================================================== -agv::State Estimate::finish_state() const +State Estimate::finish_state() const { return _pimpl->_finish_state; } //============================================================================== -Estimate& Estimate::finish_state(agv::State new_finish_state) +Estimate& Estimate::finish_state(State new_finish_state) { _pimpl->_finish_state = std::move(new_finish_state); return *this; diff --git a/rmf_task/src/rmf_task/agv/Constraints.cpp b/rmf_task/src/rmf_task/agv/Constraints.cpp index 7b0f2452..2d269845 100644 --- a/rmf_task/src/rmf_task/agv/Constraints.cpp +++ b/rmf_task/src/rmf_task/agv/Constraints.cpp @@ -17,10 +17,9 @@ #include -#include +#include namespace rmf_task { -namespace agv { //============================================================================== class Constraints::Implementation @@ -117,5 +116,4 @@ auto Constraints::drain_battery( return *this; } -} // namespace agv -} // namespace rmf_task \ No newline at end of file +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/Parameters.cpp b/rmf_task/src/rmf_task/agv/Parameters.cpp index f4678c6d..bfe137cc 100644 --- a/rmf_task/src/rmf_task/agv/Parameters.cpp +++ b/rmf_task/src/rmf_task/agv/Parameters.cpp @@ -15,10 +15,9 @@ * */ -#include +#include namespace rmf_task { -namespace agv { //============================================================================== class Parameters::Implementation @@ -125,5 +124,4 @@ auto Parameters::tool_sink( return *this; } -} // namespace agv -} // namespace task \ No newline at end of file +} // namespace task diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp index 6a4ae0ae..b0e7f3a6 100644 --- a/rmf_task/src/rmf_task/agv/State.cpp +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -17,10 +17,9 @@ #include -#include +#include namespace rmf_task { -namespace agv { //============================================================================== class State::Implementation @@ -132,6 +131,4 @@ State& State::finish_time(rmf_traffic::Time new_finish_time) return *this; } -//============================================================================== -} // namespace agv } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 575986f0..2c64f894 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include @@ -28,7 +28,6 @@ #include namespace rmf_task { -namespace agv { //============================================================================== class TaskPlanner::Configuration::Implementation @@ -1146,7 +1145,7 @@ const std::shared_ptr& TaskPlanner::estimate_cache() const } // ============================================================================ -const rmf_task::agv::TaskPlanner::Configuration& TaskPlanner::configuration() +const rmf_task::TaskPlanner::Configuration& TaskPlanner::configuration() const { return _pimpl->config; @@ -1164,5 +1163,4 @@ auto TaskPlanner::default_options() -> Options& return _pimpl->default_options; } -} // namespace agv } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp index fda81c3d..619da6df 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp @@ -19,7 +19,6 @@ #include namespace rmf_task { -namespace agv { // ============================================================================ void Candidates::update_map() @@ -193,7 +192,7 @@ PendingTask::PendingTask( // ============================================================================ std::shared_ptr PendingTask::make( const rmf_traffic::Time start_time, - const std::vector& initial_states, + const std::vector& initial_states, const Constraints& constraints, const Parameters& parameters, const ConstRequestPtr request_, @@ -218,5 +217,4 @@ std::shared_ptr PendingTask::make( return pending_task; } -} // namespace agv } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp index ad89ccd8..25e1832f 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP #define SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP -#include +#include #include #include @@ -27,7 +27,6 @@ #include namespace rmf_task { -namespace agv { // ============================================================================ struct Invariant @@ -113,7 +112,7 @@ class PendingTask static std::shared_ptr make( const rmf_traffic::Time start_time, - const std::vector& initial_states, + const std::vector& initial_states, const Constraints& constraints, const Parameters& parameters, const ConstRequestPtr request_, @@ -212,7 +211,6 @@ struct LowestCostEstimate } }; -} // namespace agv } // namespace rmf_task #endif // SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ac5b1ed3..ec6c8b84 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -52,26 +52,26 @@ class ChargeBattery::Model : public Request::Model public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; Model( const rmf_traffic::Time earliest_start_time, - agv::Parameters parameters); + Parameters parameters); private: rmf_traffic::Time _earliest_start_time; - agv::Parameters _parameters; + Parameters _parameters; rmf_traffic::Duration _invariant_duration; }; //============================================================================== ChargeBattery::Model::Model( const rmf_traffic::Time earliest_start_time, - agv::Parameters parameters) + Parameters parameters) : _earliest_start_time(earliest_start_time), _parameters(parameters) { @@ -81,8 +81,8 @@ ChargeBattery::Model::Model( //============================================================================== std::optional ChargeBattery::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const { // Important to return nullopt if a charging task is not needed. In the task @@ -101,7 +101,7 @@ ChargeBattery::Model::estimate_finish( initial_state.finish_time(), initial_state.charging_waypoint(), initial_state.location().orientation()}; - agv::State state{ + State state{ std::move(final_plan_start), initial_state.charging_waypoint(), initial_state.battery_soc()}; @@ -216,7 +216,7 @@ ChargeBattery::Description::Description() //============================================================================== std::shared_ptr ChargeBattery::Description::make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const + const Parameters& parameters) const { return std::make_shared( earliest_start_time, diff --git a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp index b3219fc6..05fe56c1 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp @@ -36,7 +36,7 @@ ChargeBatteryFactory::ChargeBatteryFactory() //============================================================================== ConstRequestPtr ChargeBatteryFactory::make_request( - const agv::State& state) const + const State& state) const { return ChargeBattery::make(state.finish_time()); } diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 386b668b..92d2d55d 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -28,22 +28,22 @@ class Clean::Model : public Request::Model public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; Model( const rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters, + const Parameters& parameters, const rmf_traffic::Trajectory& cleaning_path, std::size_t start_waypoint, std::size_t end_waypoint); private: rmf_traffic::Time _earliest_start_time; - agv::Parameters _parameters; + Parameters _parameters; std::size_t _start_waypoint; std::size_t _end_waypoint; @@ -54,7 +54,7 @@ class Clean::Model : public Request::Model //============================================================================== Clean::Model::Model( const rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters, + const Parameters& parameters, const rmf_traffic::Trajectory& cleaning_path, std::size_t start_waypoint, std::size_t end_waypoint) @@ -85,15 +85,15 @@ Clean::Model::Model( //============================================================================== std::optional Clean::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), _end_waypoint, initial_state.location().orientation()}; - agv::State state{ + State state{ std::move(final_plan_start), initial_state.charging_waypoint(), initial_state.battery_soc()}; @@ -296,7 +296,7 @@ Clean::Description::Description() //============================================================================== std::shared_ptr Clean::Description::make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const + const Parameters& parameters) const { if (parameters.tool_sink() == nullptr) { diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 5a7734ab..30faa08e 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -28,15 +28,15 @@ class Delivery::Model : public Request::Model public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; Model( const rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters, + const Parameters& parameters, std::size_t pickup_waypoint, rmf_traffic::Duration pickup_wait, std::size_t dropoff_waypoint, @@ -44,7 +44,7 @@ class Delivery::Model : public Request::Model private: rmf_traffic::Time _earliest_start_time; - agv::Parameters _parameters; + Parameters _parameters; std::size_t _pickup_waypoint; rmf_traffic::Duration _pickup_wait; std::size_t _dropoff_waypoint; @@ -57,7 +57,7 @@ class Delivery::Model : public Request::Model //============================================================================== Delivery::Model::Model( const rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters, + const Parameters& parameters, std::size_t pickup_waypoint, rmf_traffic::Duration pickup_wait, std::size_t dropoff_waypoint, @@ -107,15 +107,15 @@ Delivery::Model::Model( //============================================================================== std::optional Delivery::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), _dropoff_waypoint, initial_state.location().orientation()}; - agv::State state{ + State state{ std::move(final_plan_start), initial_state.charging_waypoint(), initial_state.battery_soc()}; @@ -315,7 +315,7 @@ Delivery::Description::Description() //============================================================================== std::shared_ptr Delivery::Description::make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const + const Parameters& parameters) const { return std::make_shared( earliest_start_time, diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index b13978c5..2794da80 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -26,22 +26,22 @@ class Loop::Model : public Request::Model public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; Model( const rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters, + const Parameters& parameters, std::size_t start_waypoint, std::size_t finish_waypoint, std::size_t num_loops); private: rmf_traffic::Time _earliest_start_time; - agv::Parameters _parameters; + Parameters _parameters; std::size_t _start_waypoint; std::size_t _finish_waypoint; @@ -52,7 +52,7 @@ class Loop::Model : public Request::Model //============================================================================== Loop::Model::Model( const rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters, + const Parameters& parameters, std::size_t start_waypoint, std::size_t finish_waypoint, std::size_t num_loops) @@ -105,8 +105,8 @@ Loop::Model::Model( //============================================================================== std::optional Loop::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, + const State& initial_state, + const Constraints& task_planning_constraints, EstimateCache& estimate_cache) const { rmf_traffic::Duration variant_duration(0); @@ -261,7 +261,7 @@ std::optional Loop::Model::estimate_finish( state_finish_time, _finish_waypoint, initial_state.location().orientation()}; - agv::State state{ + State state{ std::move(location), initial_state.charging_waypoint(), battery_soc}; @@ -312,7 +312,7 @@ Loop::Description::Description() //============================================================================== std::shared_ptr Loop::Description::make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const + const Parameters& parameters) const { return std::make_shared( earliest_start_time, diff --git a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp index 609eb278..15b8d05d 100644 --- a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp @@ -65,7 +65,7 @@ ParkRobotFactory::ParkRobotFactory( //============================================================================== ConstRequestPtr ParkRobotFactory::make_request( - const agv::State& state) const + const State& state) const { std::string id = "ParkRobot" + generate_uuid(); const auto start_waypoint = state.location().waypoint(); diff --git a/rmf_task/test/unit/agv/test_Constraints.cpp b/rmf_task/test/unit/agv/test_Constraints.cpp index f0d737bd..c8f0f121 100644 --- a/rmf_task/test/unit/agv/test_Constraints.cpp +++ b/rmf_task/test/unit/agv/test_Constraints.cpp @@ -18,41 +18,41 @@ #include #include -#include +#include #include SCENARIO("Test Constraints") { - std::unique_ptr constraints; + std::unique_ptr constraints; WHEN("Minimum battery threshold") { CHECK_NOTHROW( - constraints.reset(new rmf_task::agv::Constraints{0.0})); + constraints.reset(new rmf_task::Constraints{0.0})); } WHEN("Maximum battery threshold") { CHECK_NOTHROW( - constraints.reset(new rmf_task::agv::Constraints{1.0})); + constraints.reset(new rmf_task::Constraints{1.0})); } WHEN("Half battery threshold") { CHECK_NOTHROW( - constraints.reset(new rmf_task::agv::Constraints{0.5})); + constraints.reset(new rmf_task::Constraints{0.5})); } WHEN("Below minimum battery threshold") { CHECK_THROWS( - constraints.reset(new rmf_task::agv::Constraints{0.0 - 1e-4})); + constraints.reset(new rmf_task::Constraints{0.0 - 1e-4})); } WHEN("Above maximum battery threshold") { CHECK_THROWS( - constraints.reset(new rmf_task::agv::Constraints{1.0 + 1e-4})); + constraints.reset(new rmf_task::Constraints{1.0 + 1e-4})); } } diff --git a/rmf_task/test/unit/agv/test_State.cpp b/rmf_task/test/unit/agv/test_State.cpp index 778a8cbf..a4fd9619 100644 --- a/rmf_task/test/unit/agv/test_State.cpp +++ b/rmf_task/test/unit/agv/test_State.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -32,11 +32,11 @@ SCENARIO("Robot States") 0, 0.0}; - std::unique_ptr basic_state; + std::unique_ptr basic_state; WHEN("Empty battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ + CHECK_NOTHROW(basic_state.reset(new rmf_task::State{ basic_start, 0, 0.0})); @@ -44,7 +44,7 @@ SCENARIO("Robot States") WHEN("Full battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ + CHECK_NOTHROW(basic_state.reset(new rmf_task::State{ basic_start, 0, 1.0})); @@ -52,7 +52,7 @@ SCENARIO("Robot States") WHEN("Half battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ + CHECK_NOTHROW(basic_state.reset(new rmf_task::State{ basic_start, 0, 0.5})); @@ -60,7 +60,7 @@ SCENARIO("Robot States") WHEN("Battery soc more than 1.0") { - CHECK_THROWS(basic_state.reset(new rmf_task::agv::State{ + CHECK_THROWS(basic_state.reset(new rmf_task::State{ basic_start, 0, 1.0 + 1e-4})); @@ -68,7 +68,7 @@ SCENARIO("Robot States") WHEN("Battery soc less than 0.0") { - CHECK_THROWS(basic_state.reset(new rmf_task::agv::State{ + CHECK_THROWS(basic_state.reset(new rmf_task::State{ basic_start, 0, 0.0 - 1e-4})); diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 14f9354e..8885ee1b 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -15,10 +15,10 @@ * */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -46,7 +46,7 @@ #include -using TaskPlanner = rmf_task::agv::TaskPlanner; +using TaskPlanner = rmf_task::TaskPlanner; //============================================================================== inline void CHECK_TIMES(const TaskPlanner::Assignments& assignments, @@ -201,8 +201,8 @@ SCENARIO("Grid World") const auto cost_calculator = rmf_task::BinaryPriorityScheme::make_cost_calculator(); - const rmf_task::agv::Constraints constraints{0.2, 1.0, drain_battery}; - const rmf_task::agv::Parameters parameters{ + const rmf_task::Constraints constraints{0.2, 1.0, drain_battery}; + const rmf_task::Parameters parameters{ planner, battery_system, motion_sink, @@ -235,10 +235,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, - rmf_task::agv::State{second_location, 2, 1.0} + rmf_task::State{first_location, 13, 1.0}, + rmf_task::State{second_location, 2, 1.0} }; std::vector requests = @@ -318,10 +318,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, - rmf_task::agv::State{second_location, 2, 1.0} + rmf_task::State{first_location, 13, 1.0}, + rmf_task::State{second_location, 2, 1.0} }; std::vector requests = @@ -465,10 +465,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, initial_soc}, - rmf_task::agv::State{second_location, 2, initial_soc} + rmf_task::State{first_location, 13, initial_soc}, + rmf_task::State{second_location, 2, initial_soc} }; std::vector requests = @@ -565,10 +565,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 9, 1.0}, - rmf_task::agv::State{second_location, 2, 1.0} + rmf_task::State{first_location, 9, 1.0}, + rmf_task::State{second_location, 2, 1.0} }; std::vector requests = @@ -710,9 +710,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::State{first_location, 13, 1.0}, }; std::vector requests = @@ -756,9 +756,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 9, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 0.0}, + rmf_task::State{first_location, 13, 0.0}, }; std::vector requests = @@ -803,9 +803,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::State{first_location, 13, 1.0}, }; std::vector requests = @@ -902,9 +902,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::State{first_location, 13, 1.0}, }; std::vector requests = @@ -968,9 +968,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::State{first_location, 13, 1.0}, }; std::vector requests = @@ -1050,9 +1050,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::State{first_location, 13, 1.0}, }; std::vector requests = @@ -1133,10 +1133,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 1, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, - rmf_task::agv::State{second_location, 1, 1.0} + rmf_task::State{first_location, 13, 1.0}, + rmf_task::State{second_location, 1, 1.0} }; std::vector requests = @@ -1277,11 +1277,11 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start second_location{now, 1, default_orientation}; rmf_traffic::agv::Plan::Start third_location{now, 5, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, - rmf_task::agv::State{second_location, 1, 1.0}, - rmf_task::agv::State{third_location, 5, 1.0} + rmf_task::State{first_location, 13, 1.0}, + rmf_task::State{second_location, 1, 1.0}, + rmf_task::State{third_location, 5, 1.0} }; std::vector requests = @@ -1436,9 +1436,9 @@ SCENARIO("Grid World") const double default_orientation = 0.0; const double initial_soc = 0.3; const double recharge_soc = 0.9; - rmf_task::agv::Constraints new_constraints{0.2, recharge_soc, + rmf_task::Constraints new_constraints{0.2, recharge_soc, drain_battery}; - rmf_task::agv::TaskPlanner::Configuration new_task_config{ + rmf_task::TaskPlanner::Configuration new_task_config{ parameters, new_constraints, cost_calculator}; @@ -1446,10 +1446,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 2, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, initial_soc}, - rmf_task::agv::State{second_location, 2, initial_soc} + rmf_task::State{first_location, 13, initial_soc}, + rmf_task::State{second_location, 2, initial_soc} }; std::vector requests = @@ -1549,9 +1549,9 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::State{first_location, 13, 1.0}, }; const auto start_time = @@ -1606,18 +1606,18 @@ SCENARIO("Grid World") const double default_orientation = 0.0; const double initial_soc = 0.3; const double recharge_soc = 1.0; - rmf_task::agv::Constraints new_constraints{0.2, recharge_soc, + rmf_task::Constraints new_constraints{0.2, recharge_soc, drain_battery}; - rmf_task::agv::TaskPlanner::Configuration new_task_config{ + rmf_task::TaskPlanner::Configuration new_task_config{ parameters, new_constraints, cost_calculator}; rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, initial_soc}, + rmf_task::State{first_location, 13, initial_soc}, }; std::vector requests = @@ -1690,10 +1690,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 1, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, - rmf_task::agv::State{second_location, 1, 1.0}, + rmf_task::State{first_location, 13, 1.0}, + rmf_task::State{second_location, 1, 1.0}, }; std::vector requests = @@ -1800,10 +1800,10 @@ SCENARIO("Grid World") rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; rmf_traffic::agv::Plan::Start second_location{now, 1, default_orientation}; - std::vector initial_states = + std::vector initial_states = { - rmf_task::agv::State{first_location, 13, 1.0}, - rmf_task::agv::State{second_location, 1, 1.0}, + rmf_task::State{first_location, 13, 1.0}, + rmf_task::State{second_location, 1, 1.0}, }; std::vector requests = From 2ea66b573e31f9f0ba56b7548cc83ab17df76790 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 2 Sep 2021 17:28:00 +0800 Subject: [PATCH 02/85] Redesign State to be composable Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/CompositeData.hpp | 5 +- rmf_task/include/rmf_task/State.hpp | 140 ++++++++++++------ rmf_task/include/rmf_task/TaskPlanner.hpp | 4 +- .../rmf_task/detail/impl_CompositeData.hpp | 8 + .../rmf_task/BinaryPriorityCostCalculator.cpp | 8 +- rmf_task/src/rmf_task/CompositeData.cpp | 15 +- rmf_task/src/rmf_task/agv/State.cpp | 138 ++++++++++------- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 34 +++-- .../rmf_task/agv/internal_task_planning.cpp | 6 +- .../src/rmf_task/requests/ChargeBattery.cpp | 35 +++-- .../requests/ChargeBatteryFactory.cpp | 2 +- rmf_task/src/rmf_task/requests/Clean.cpp | 43 +++--- rmf_task/src/rmf_task/requests/Delivery.cpp | 43 +++--- rmf_task/src/rmf_task/requests/Loop.cpp | 39 ++--- .../rmf_task/requests/ParkRobotFactory.cpp | 8 +- rmf_task/test/unit/agv/test_State.cpp | 22 ++- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 80 +++++----- 17 files changed, 382 insertions(+), 248 deletions(-) diff --git a/rmf_task/include/rmf_task/CompositeData.hpp b/rmf_task/include/rmf_task/CompositeData.hpp index bb50bba8..929ce17a 100644 --- a/rmf_task/include/rmf_task/CompositeData.hpp +++ b/rmf_task/include/rmf_task/CompositeData.hpp @@ -65,6 +65,10 @@ class CompositeData template InsertResult insert_or_assign(T&& value); + /// Same as insert_or_assign, but *this is returned instead of the new value. + template + CompositeData& with(T&& value); + /// Get a reference to a data structure of type T if one is available in the /// CompositeData. If one is not available, this will return a nullptr. template @@ -94,7 +98,6 @@ class CompositeData rmf_utils::impl_ptr _pimpl; }; - } // namespace rmf_task #include diff --git a/rmf_task/include/rmf_task/State.hpp b/rmf_task/include/rmf_task/State.hpp index 4bdc1a26..d8852fc2 100644 --- a/rmf_task/include/rmf_task/State.hpp +++ b/rmf_task/include/rmf_task/State.hpp @@ -25,68 +25,120 @@ #include #include +#include + namespace rmf_task { //============================================================================== -/// A class that is used to describe the state of an AGV -class State +class State : public CompositeData { public: - /// Constructor - /// - /// \param[in] location - /// Current state's location, which includes the time that the robot can - /// feasibly take on a new task, according to the finishing time of any - /// tasks that the robot is currently performing. - /// - /// \param[in] charging_waypoint - /// The charging waypoint index of this robot. - /// - /// \param[in] battery_soc - /// Current battery state of charge of the robot. This value needs to be - /// between 0.0 to 1.0. - State( - rmf_traffic::agv::Plan::Start location, - std::size_t charging_waypoint, - double battery_soc); + /// The current waypoint of the robot state + struct CurrentWaypoint + { + std::size_t value; - /// The current state's location. - rmf_traffic::agv::Plan::Start location() const; + CurrentWaypoint(std::size_t input) + : value(input) {} + }; - /// Sets the state's location. - State& location(rmf_traffic::agv::Plan::Start new_location); + std::optional waypoint() const; + State& waypoint(std::size_t new_waypoint); - /// Robot's charging waypoint index. - std::size_t charging_waypoint() const; + /// The current orientation of the robot state + struct CurrentOrientation + { + double value; - /// Sets the charging waypoint index. - State& charging_waypoint(std::size_t new_charging_waypoint); + CurrentOrientation(double input) + : value(input) {} + }; - /// The current battery state of charge of the robot. This value is between - /// 0.0 and 1.0. - double battery_soc() const; + std::optional orientation() const; + State& orientation(double new_orientation); + + /// The current time for the robot + struct CurrentTime + { + rmf_traffic::Time value; + + CurrentTime(rmf_traffic::Time input) + : value(input) {} + }; + + std::optional time() const; + State& time(rmf_traffic::Time new_time); + + /// The dedicated charging point for this robot + // TODO(MXG): Consider removing this field and using some kind of + // ChargingStrategy abstract interface to determine where and how the robots + // should be charging. + struct DedicatedChargingPoint + { + std::size_t value; + + DedicatedChargingPoint(std::size_t input) + : value(input) {} + }; - /// Sets a new battery state of charge value. This value needs to be between + std::optional dedicated_charging_waypoint() const; + State& dedicated_charging_waypoint(std::size_t new_charging_waypoint); + + /// The current battery state of charge of the robot. This value is between /// 0.0 and 1.0. - State& battery_soc(double new_battery_soc); + struct CurrentBatterySoC + { + double value; - /// The current location waypoint index. - std::size_t waypoint() const; + CurrentBatterySoC(double input) + : value(input) {} + }; - /// Sets the current location waypoint index. - State& waypoint(std::size_t new_waypoint); + std::optional battery_soc() const; + State& battery_soc(double new_battery_soc); - /// The time which the robot finishes its current task or when it is ready for - /// a new task. - rmf_traffic::Time finish_time() const; + /// Load the basic state components expected for the planner. + /// + /// \param[in] location + /// The robot's initial location data. + /// + /// \param[in] charging_point + /// The robot's dedicated charging point. + /// + /// \param[in] battery_soc + /// The robot's initial battery state of charge. + State& load_basic( + const rmf_traffic::agv::Plan::Start& location, + std::size_t charging_point, + double battery_soc); - /// Sets the finish time for the robot. - State& finish_time(rmf_traffic::Time new_finish_time); + /// Load the plan start into the State. The location info will be split into + /// CurrentWaypoint, CurrentOrientation, and CurrentTime data. + State& load(const rmf_traffic::agv::Plan::Start& location); - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; + /// Project an rmf_traffic::agv::Plan::Start from this State. + /// + /// If CurrentWaypoint is unavailable, this will return a std::nullopt. For + /// any other components that are unavailable (CurrentOrientation or + /// CurrentTime), the given default values will be used. + /// + /// \param[in] default_orientation + /// The orientation value that will be used if CurrentOrientation is not + /// available in this State. + /// + /// \param[in] default_time + /// The time value that will be used if default_time is not available in + /// this State. + std::optional project_plan_start( + double default_orientation = 0.0, + rmf_traffic::Time default_time = rmf_traffic::Time()) const; + + /// Extract an rmf_traffic::agv::Plan::Start from this State. + /// + /// If any necessary component is missing (i.e. CurrentWaypoint, + /// CurrentOrientation, or CurrentTime) then this will return a std::nullopt. + std::optional extract_plan_start() const; }; } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index fdc78682..8ab5f63a 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -149,14 +149,14 @@ class TaskPlanner /// The earliest time the agent will begin exececuting this task Assignment( rmf_task::ConstRequestPtr request, - State state, + State finish_state, rmf_traffic::Time deployment_time); // Get the request of this task const rmf_task::ConstRequestPtr& request() const; // Get a const reference to the predicted state at the end of the assignment - const State& state() const; + const State& finish_state() const; // Get the time when the robot begins executing // this assignment diff --git a/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp b/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp index d4d8fb65..f8548f3c 100644 --- a/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp +++ b/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp @@ -46,6 +46,14 @@ auto CompositeData::insert_or_assign(T&& value) -> InsertResult return detail::insertion_cast(_insert(std::any(std::move(value)), true)); } +//============================================================================== +template +CompositeData& CompositeData::with(T&& value) +{ + insert_or_assign(std::forward(value)); + return *this; +} + //============================================================================== template T* CompositeData::get() diff --git a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp index 1c446fdf..bc5690dc 100644 --- a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp +++ b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp @@ -33,8 +33,9 @@ auto BinaryPriorityCostCalculator::compute_g_assignment( return 0.0; // Ignore charging tasks in cost } - return rmf_traffic::time::to_seconds(assignment.state().finish_time() - - assignment.request()->earliest_start_time()); + return rmf_traffic::time::to_seconds( + assignment.finish_state().time().value() + - assignment.request()->earliest_start_time()); } //============================================================================== @@ -108,7 +109,8 @@ auto BinaryPriorityCostCalculator::compute_h( value = rmf_traffic::time::to_seconds(time_now.time_since_epoch()); else value = rmf_traffic::time::to_seconds( - assignments.back().assignment.state().finish_time().time_since_epoch()); + assignments.back().assignment.finish_state() + .time().value().time_since_epoch()); } } diff --git a/rmf_task/src/rmf_task/CompositeData.cpp b/rmf_task/src/rmf_task/CompositeData.cpp index 9a0a9775..95ee595e 100644 --- a/rmf_task/src/rmf_task/CompositeData.cpp +++ b/rmf_task/src/rmf_task/CompositeData.cpp @@ -37,6 +37,12 @@ CompositeData::CompositeData() // Do nothing } +//============================================================================== +void CompositeData::clear() +{ + _pimpl->data.clear(); +} + //============================================================================== std::any* CompositeData::_get(std::type_index type) { @@ -57,12 +63,15 @@ const std::any* CompositeData::_get(std::type_index type) const auto CompositeData::_insert(std::any value, bool or_assign) -> InsertResult { - const auto insertion = _pimpl->data.insert({value.type(), std::move(value)}); - if (!insertion.second && or_assign) + if (or_assign) { - insertion.first->second = std::move(value); + const auto insertion = + _pimpl->data.insert_or_assign(value.type(), std::move(value)); + + return {insertion.second, &insertion.first->second}; } + const auto insertion = _pimpl->data.insert({value.type(), std::move(value)}); return {insertion.second, &insertion.first->second}; } diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp index b0e7f3a6..26bc6337 100644 --- a/rmf_task/src/rmf_task/agv/State.cpp +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -22,72 +22,76 @@ namespace rmf_task { //============================================================================== -class State::Implementation +std::optional State::waypoint() const { -public: - - Implementation( - rmf_traffic::agv::Plan::Start location, - std::size_t charging_waypoint, - double battery_soc) - : _location(std::move(location)), - _charging_waypoint(charging_waypoint), - _battery_soc(battery_soc) - { - if (_battery_soc < 0.0 || _battery_soc > 1.0) - { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::invalid_argument( - "Battery State of Charge needs to be between 0.0 and 1.0."); - // *INDENT-ON* - } - } + if (const auto* wp = get()) + return wp->value; + + return std::nullopt; +} + +//============================================================================== +State& State::waypoint(std::size_t new_waypoint) +{ + with(new_waypoint); + return *this; +} - rmf_traffic::agv::Plan::Start _location; - std::size_t _charging_waypoint; - double _battery_soc; -}; +//============================================================================== +std::optional State::orientation() const +{ + if (const auto* ori = get()) + return ori->value; + + return std::nullopt; +} //============================================================================== -State::State( - rmf_traffic::agv::Plan::Start location, - std::size_t charging_waypoint, - double battery_soc) -: _pimpl(rmf_utils::make_impl( - Implementation(std::move(location), charging_waypoint, battery_soc))) +State& State::orientation(double new_orientation) { + with(new_orientation); + return *this; } //============================================================================== -rmf_traffic::agv::Plan::Start State::location() const +std::optional State::time() const { - return _pimpl->_location; + if (const auto* time = get()) + return time->value; + + return std::nullopt; } //============================================================================== -State& State::location(rmf_traffic::agv::Plan::Start new_location) +State& State::time(rmf_traffic::Time new_time) { - _pimpl->_location = std::move(new_location); + with(new_time); return *this; } //============================================================================== -std::size_t State::charging_waypoint() const +std::optional State::dedicated_charging_waypoint() const { - return _pimpl->_charging_waypoint; + if (const auto* p = get()) + return p->value; + + return std::nullopt; } //============================================================================== -State& State::charging_waypoint(std::size_t new_charging_waypoint) +State& State::dedicated_charging_waypoint(std::size_t new_charging_waypoint) { - _pimpl->_charging_waypoint = new_charging_waypoint; + with(new_charging_waypoint); return *this; } //============================================================================== -double State::battery_soc() const +std::optional State::battery_soc() const { - return _pimpl->_battery_soc; + if (const auto* b = get()) + return b->value; + + return std::nullopt; } //============================================================================== @@ -97,38 +101,72 @@ State& State::battery_soc(double new_battery_soc) { // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::invalid_argument( - "Battery State of Charge needs be between 0.0 and 1.0."); + "Battery State of Charge needs to be between 0.0 and 1.0."); // *INDENT-ON* } - _pimpl->_battery_soc = new_battery_soc; + with(new_battery_soc); return *this; } //============================================================================== -std::size_t State::waypoint() const +State& State::load_basic( + const rmf_traffic::agv::Plan::Start& input_location, + std::size_t input_charging_point, + double input_battery_soc) { - return _pimpl->_location.waypoint(); + load(input_location); + dedicated_charging_waypoint(input_charging_point); + battery_soc(input_battery_soc); + return *this; } //============================================================================== -State& State::waypoint(std::size_t new_waypoint) +State& State::load(const rmf_traffic::agv::Plan::Start& location) { - _pimpl->_location.waypoint(new_waypoint); + with(location.waypoint()); + with(location.orientation()); + with(location.time()); return *this; } //============================================================================== -rmf_traffic::Time State::finish_time() const +std::optional State::project_plan_start( + double default_orientation, + rmf_traffic::Time default_time) const { - return _pimpl->_location.time(); + const auto* wp = get(); + if (!wp) + return std::nullopt; + + rmf_traffic::agv::Plan::Start start( + default_time, wp->value, default_orientation); + + if (const auto* ori = get()) + start.orientation(ori->value); + + if (const auto* t = get()) + start.time(t->value); + + return start; } //============================================================================== -State& State::finish_time(rmf_traffic::Time new_finish_time) +std::optional State::extract_plan_start() const { - _pimpl->_location.time(new_finish_time); - return *this; + const auto* wp = get(); + if (!wp) + return std::nullopt; + + const auto* ori = get(); + if (!ori) + return std::nullopt; + + const auto* t = get(); + if (!t) + return std::nullopt; + + return rmf_traffic::agv::Plan::Start(t->value, wp->value, ori->value); } } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 2c64f894..29f6b42a 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -195,7 +195,7 @@ const rmf_task::ConstRequestPtr& TaskPlanner::Assignment::request() const } //============================================================================== -const State& TaskPlanner::Assignment::state() const +const State& TaskPlanner::Assignment::finish_state() const { return _pimpl->state; } @@ -429,7 +429,7 @@ class TaskPlanner::Implementation continue; } - const auto& state = agent.back().state(); + const auto& state = agent.back().finish_state(); const auto request = factory.make_request(state); // TODO(YV) Currently we are unable to recursively call complete_solve() @@ -439,7 +439,7 @@ class TaskPlanner::Implementation // When we fix the logic with unnecessary ChargeBattery tasks, we should // revist making this a recursive call. auto model = request->description()->make_model( - state.finish_time(), + state.time().value(), config.parameters()); auto estimate = model->estimate_finish( state, config.constraints(), *estimate_cache); @@ -458,10 +458,10 @@ class TaskPlanner::Implementation // Insufficient battery to perform the finishing request. We check if // adding a ChargeBattery task before will allow for it to be performed const auto charging_request = - make_charging_request(state.finish_time()); + make_charging_request(state.time().value()); const auto charge_battery_model = charging_request->description()->make_model( - state.finish_time(), + state.time().value(), config.parameters()); const auto charge_battery_estimate = charge_battery_model->estimate_finish( @@ -469,7 +469,7 @@ class TaskPlanner::Implementation if (charge_battery_estimate.has_value()) { model = request->description()->make_model( - charge_battery_estimate.value().finish_state().finish_time(), + charge_battery_estimate.value().finish_state().time().value(), config.parameters()); estimate = model->estimate_finish( charge_battery_estimate.value().finish_state(), @@ -575,14 +575,14 @@ class TaskPlanner::Implementation time_now, 0, 0.0}; estimates.resize( node->assigned_tasks.size(), - State{empty_new_location, 0, 0.0}); + State().load_basic(empty_new_location, 0, 0.0)); for (std::size_t i = 0; i < node->assigned_tasks.size(); ++i) { const auto& assignments = node->assigned_tasks[i]; if (assignments.empty()) estimates[i] = initial_states[i]; else - estimates[i] = assignments.back().assignment.state(); + estimates[i] = assignments.back().assignment.finish_state(); } node = make_initial_node( @@ -646,8 +646,8 @@ class TaskPlanner::Implementation rmf_traffic::Time latest = rmf_traffic::Time::min(); for (const auto& s : initial_states) { - if (latest < s.finish_time()) - latest = s.finish_time(); + if (latest < s.time().value()) + latest = s.time().value(); } return latest; @@ -679,7 +679,9 @@ class TaskPlanner::Implementation if (a.empty()) continue; - const auto finish_time = a.back().assignment.state().finish_time(); + const auto finish_time = + a.back().assignment.finish_state().time().value(); + if (latest < finish_time) latest = finish_time; } @@ -717,7 +719,7 @@ class TaskPlanner::Implementation assignments.back().assignment.request()->description())) { auto charge_battery = make_charging_request( - entry.previous_state.finish_time()); + entry.previous_state.time().value()); const auto charge_battery_model = charge_battery->description()->make_model( charge_battery->earliest_start_time(), @@ -794,7 +796,9 @@ class TaskPlanner::Implementation if (add_charger) { - auto charge_battery = make_charging_request(entry.state.finish_time()); + auto charge_battery = make_charging_request( + entry.state.time().value()); + const auto charge_battery_model = charge_battery->description()->make_model( charge_battery->earliest_start_time(), @@ -877,10 +881,10 @@ class TaskPlanner::Implementation const rmf_task::requests::ChargeBattery::Description>( assignments.back().assignment.request()->description())) return nullptr; - state = assignments.back().assignment.state(); + state = assignments.back().assignment.finish_state(); } - auto charge_battery = make_charging_request(state.finish_time()); + auto charge_battery = make_charging_request(state.time().value()); const auto charge_battery_model = charge_battery->description()->make_model( charge_battery->earliest_start_time(), diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp index 619da6df..37d5f42e 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp @@ -52,7 +52,7 @@ void Candidates::update_candidate( _value_map.erase(it); _candidate_map[candidate] = _value_map.insert( { - state.finish_time(), + state.time().value(), Entry{candidate, state, wait_until, previous_state, require_charge_battery} }); } @@ -113,7 +113,7 @@ std::shared_ptr Candidates::make( if (finish.has_value()) { initial_map.insert({ - finish.value().finish_state().finish_time(), + finish.value().finish_state().time().value(), Entry{ i, finish.value().finish_state(), @@ -140,7 +140,7 @@ std::shared_ptr Candidates::make( if (new_finish.has_value()) { initial_map.insert( - {new_finish.value().finish_state().finish_time(), + {new_finish.value().finish_state().time().value(), Entry{ i, new_finish.value().finish_state(), diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ec6c8b84..3c34254c 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -93,22 +93,25 @@ ChargeBattery::Model::estimate_finish( // returning. const auto recharge_soc = task_planning_constraints.recharge_soc(); if (initial_state.battery_soc() >= recharge_soc - 1e-3 - && initial_state.waypoint() == initial_state.charging_waypoint()) + && initial_state.waypoint().value() + == initial_state.dedicated_charging_waypoint().value()) + { return std::nullopt; + } // Compute time taken to reach charging waypoint from current location rmf_traffic::agv::Plan::Start final_plan_start{ - initial_state.finish_time(), - initial_state.charging_waypoint(), - initial_state.location().orientation()}; - State state{ + initial_state.time().value(), + initial_state.dedicated_charging_waypoint().value(), + initial_state.orientation().value()}; + auto state = State().load_basic( std::move(final_plan_start), - initial_state.charging_waypoint(), - initial_state.battery_soc()}; + initial_state.dedicated_charging_waypoint().value(), + initial_state.battery_soc().value()); - const auto start_time = initial_state.finish_time(); + const auto start_time = initial_state.time().value(); - double battery_soc = initial_state.battery_soc(); + double battery_soc = initial_state.battery_soc().value(); double dSOC_motion = 0.0; double dSOC_device = 0.0; double variant_battery_drain = 0.0; @@ -118,10 +121,12 @@ ChargeBattery::Model::estimate_finish( const auto& motion_sink = *_parameters.motion_sink(); const auto& ambient_sink = *_parameters.ambient_sink(); - if (initial_state.waypoint() != initial_state.charging_waypoint()) + if (initial_state.waypoint() != initial_state.dedicated_charging_waypoint()) { - const auto endpoints = std::make_pair(initial_state.waypoint(), - initial_state.charging_waypoint()); + const auto endpoints = std::make_pair( + initial_state.waypoint().value(), + initial_state.dedicated_charging_waypoint().value()); + const auto& cache_result = estimate_cache.get(endpoints); // Use memoized values if possible if (cache_result) @@ -134,7 +139,7 @@ ChargeBattery::Model::estimate_finish( // Compute plan to charging waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; const auto result = planner.plan( - initial_state.location(), goal); + initial_state.extract_plan_start().value(), goal); auto itinerary_start_time = start_time; for (const auto& itinerary : result->get_itinerary()) { @@ -176,8 +181,8 @@ ChargeBattery::Model::estimate_finish( (3600 * delta_soc * _parameters.battery_system().capacity()) / _parameters.battery_system().charging_current(); - const rmf_traffic::Time wait_until = initial_state.finish_time(); - state.finish_time( + const rmf_traffic::Time wait_until = initial_state.time().value(); + state.time( wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge)); diff --git a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp index 05fe56c1..9bccbc44 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp @@ -38,7 +38,7 @@ ChargeBatteryFactory::ChargeBatteryFactory() ConstRequestPtr ChargeBatteryFactory::make_request( const State& state) const { - return ChargeBattery::make(state.finish_time()); + return ChargeBattery::make(state.time().value()); } } // namespace requests diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 92d2d55d..d8061ea7 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -90,19 +90,19 @@ std::optional Clean::Model::estimate_finish( EstimateCache& estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ - initial_state.finish_time(), + initial_state.time().value(), _end_waypoint, - initial_state.location().orientation()}; - State state{ + initial_state.orientation().value()}; + auto state = State().load_basic( std::move(final_plan_start), - initial_state.charging_waypoint(), - initial_state.battery_soc()}; + initial_state.dedicated_charging_waypoint().value(), + initial_state.battery_soc().value()); rmf_traffic::Duration variant_duration(0); rmf_traffic::Duration end_duration(0); - const rmf_traffic::Time start_time = initial_state.finish_time(); - double battery_soc = initial_state.battery_soc(); + const rmf_traffic::Time start_time = initial_state.time().value(); + double battery_soc = initial_state.battery_soc().value(); double dSOC_motion = 0.0; double dSOC_ambient = 0.0; const bool drain_battery = task_planning_constraints.drain_battery(); @@ -112,8 +112,10 @@ std::optional Clean::Model::estimate_finish( if (initial_state.waypoint() != _start_waypoint) { - const auto endpoints = std::make_pair(initial_state.waypoint(), - _start_waypoint); + const auto endpoints = std::make_pair( + initial_state.waypoint().value(), + _start_waypoint); + const auto& cache_result = estimate_cache.get(endpoints); if (cache_result) { @@ -126,7 +128,7 @@ std::optional Clean::Model::estimate_finish( rmf_traffic::agv::Planner::Goal goal{endpoints.second}; const auto result_to_start = planner.plan( - initial_state.location(), goal); + initial_state.extract_plan_start().value(), goal); // We assume we can always compute a plan auto itinerary_start_time = start_time; double variant_battery_drain = 0.0; @@ -169,16 +171,17 @@ std::optional Clean::Model::estimate_finish( const rmf_traffic::Time ideal_start = _earliest_start_time - variant_duration; const rmf_traffic::Time wait_until = - initial_state.finish_time() > ideal_start ? - initial_state.finish_time() : ideal_start; + initial_state.time().value() > ideal_start ? + initial_state.time().value() : ideal_start; // Factor in battery drain while waiting to move to start waypoint. If a robot // is initially at a charging waypoint, it is assumed to be continually charging - if (drain_battery && wait_until > initial_state.finish_time() && - initial_state.waypoint() != initial_state.charging_waypoint()) + if (drain_battery && wait_until > initial_state.time().value() && + initial_state.waypoint() + != initial_state.dedicated_charging_waypoint().value()) { rmf_traffic::Duration wait_duration( - wait_until - initial_state.finish_time()); + wait_until - initial_state.time().value()); dSOC_ambient = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_ambient; @@ -190,7 +193,7 @@ std::optional Clean::Model::estimate_finish( } // Factor in invariants - state.finish_time( + state.time( wait_until + variant_duration + _invariant_duration + end_duration); if (drain_battery) @@ -201,10 +204,10 @@ std::optional Clean::Model::estimate_finish( // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; - if (_end_waypoint != state.charging_waypoint()) + if (_end_waypoint != state.dedicated_charging_waypoint().value()) { const auto endpoints = std::make_pair(_end_waypoint, - state.charging_waypoint()); + state.dedicated_charging_waypoint().value()); const auto& cache_result = estimate_cache.get(endpoints); if (cache_result) { @@ -213,7 +216,7 @@ std::optional Clean::Model::estimate_finish( else { rmf_traffic::agv::Planner::Start start{ - state.finish_time(), + state.time().value(), endpoints.first, 0.0}; @@ -221,7 +224,7 @@ std::optional Clean::Model::estimate_finish( const auto result_to_charger = planner.plan(start, goal); // We assume we can always compute a plan - auto itinerary_start_time = state.finish_time(); + auto itinerary_start_time = state.time().value(); rmf_traffic::Duration retreat_duration(0); for (const auto& itinerary : result_to_charger->get_itinerary()) { diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 30faa08e..97777dbc 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -112,18 +112,18 @@ std::optional Delivery::Model::estimate_finish( EstimateCache& estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ - initial_state.finish_time(), + initial_state.time().value(), _dropoff_waypoint, - initial_state.location().orientation()}; - State state{ + initial_state.orientation().value()}; + auto state = State().load_basic( std::move(final_plan_start), - initial_state.charging_waypoint(), - initial_state.battery_soc()}; + initial_state.dedicated_charging_waypoint().value(), + initial_state.battery_soc().value()); rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.finish_time(); - double battery_soc = initial_state.battery_soc(); + const rmf_traffic::Time start_time = initial_state.time().value(); + double battery_soc = initial_state.battery_soc().value(); double dSOC_motion = 0.0; double dSOC_device = 0.0; const bool drain_battery = task_planning_constraints.drain_battery(); @@ -134,8 +134,10 @@ std::optional Delivery::Model::estimate_finish( // Factor in battery drain while moving to start waypoint of task if (initial_state.waypoint() != _pickup_waypoint) { - const auto endpoints = std::make_pair(initial_state.waypoint(), - _pickup_waypoint); + const auto endpoints = std::make_pair( + initial_state.waypoint().value(), + _pickup_waypoint); + const auto& cache_result = estimate_cache.get(endpoints); // Use previously memoized values if possible if (cache_result) @@ -149,7 +151,7 @@ std::optional Delivery::Model::estimate_finish( // Compute plan to pickup waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; const auto result_to_pickup = planner.plan( - initial_state.location(), goal); + initial_state.extract_plan_start().value(), goal); // We assume we can always compute a plan auto itinerary_start_time = start_time; double variant_battery_drain = 0.0; @@ -184,16 +186,16 @@ std::optional Delivery::Model::estimate_finish( const rmf_traffic::Time ideal_start = _earliest_start_time - variant_duration; const rmf_traffic::Time wait_until = - initial_state.finish_time() > ideal_start ? - initial_state.finish_time() : ideal_start; + initial_state.time().value() > ideal_start ? + initial_state.time().value() : ideal_start; // Factor in battery drain while waiting to move to start waypoint. If a robot // is initially at a charging waypoint, it is assumed to be continually charging - if (drain_battery && wait_until > initial_state.finish_time() && - initial_state.waypoint() != initial_state.charging_waypoint()) + if (drain_battery && wait_until > initial_state.time().value() && + initial_state.waypoint() != initial_state.dedicated_charging_waypoint()) { rmf_traffic::Duration wait_duration( - wait_until - initial_state.finish_time()); + wait_until - initial_state.time().value()); dSOC_device = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_device; @@ -205,8 +207,7 @@ std::optional Delivery::Model::estimate_finish( } // Factor in invariants - state.finish_time( - wait_until + variant_duration + _invariant_duration); + state.time(wait_until + variant_duration + _invariant_duration); if (drain_battery) { @@ -216,10 +217,10 @@ std::optional Delivery::Model::estimate_finish( // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; - if (_dropoff_waypoint != state.charging_waypoint()) + if (_dropoff_waypoint != state.dedicated_charging_waypoint().value()) { const auto endpoints = std::make_pair(_dropoff_waypoint, - state.charging_waypoint()); + state.dedicated_charging_waypoint().value()); const auto& cache_result = estimate_cache.get(endpoints); if (cache_result) { @@ -228,7 +229,7 @@ std::optional Delivery::Model::estimate_finish( else { rmf_traffic::agv::Planner::Start start{ - state.finish_time(), + state.time().value(), endpoints.first, 0.0}; @@ -236,7 +237,7 @@ std::optional Delivery::Model::estimate_finish( const auto result_to_charger = planner.plan(start, goal); // We assume we can always compute a plan - auto itinerary_start_time = state.finish_time(); + auto itinerary_start_time = state.time().value(); rmf_traffic::Duration retreat_duration(0); for (const auto& itinerary : result_to_charger->get_itinerary()) { diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 2794da80..30ca7c44 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -111,8 +111,8 @@ std::optional Loop::Model::estimate_finish( { rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.finish_time(); - double battery_soc = initial_state.battery_soc(); + const rmf_traffic::Time start_time = initial_state.time().value(); + double battery_soc = initial_state.battery_soc().value(); double dSOC_motion = 0.0; double dSOC_device = 0.0; const bool drain_battery = task_planning_constraints.drain_battery(); @@ -123,8 +123,10 @@ std::optional Loop::Model::estimate_finish( // Check if a plan has to be generated from finish location to start_waypoint if (initial_state.waypoint() != _start_waypoint) { - auto endpoints = std::make_pair(initial_state.waypoint(), - _start_waypoint); + auto endpoints = std::make_pair( + initial_state.waypoint().value(), + _start_waypoint); + const auto& cache_result = estimate_cache.get(endpoints); // Use previously memoized values if possible if (cache_result) @@ -138,7 +140,7 @@ std::optional Loop::Model::estimate_finish( // Compute plan to start_waypoint along with battery drain rmf_traffic::agv::Planner::Goal loop_start_goal{endpoints.second}; const auto plan_to_start = planner.plan( - initial_state.location(), loop_start_goal); + initial_state.extract_plan_start().value(), loop_start_goal); // We assume we can always compute a plan auto itinerary_start_time = start_time; double variant_battery_drain = 0.0; @@ -174,16 +176,17 @@ std::optional Loop::Model::estimate_finish( // Compute wait_until const rmf_traffic::Time ideal_start = _earliest_start_time - variant_duration; const rmf_traffic::Time wait_until = - initial_state.finish_time() > ideal_start ? - initial_state.finish_time() : ideal_start; + initial_state.time().value() > ideal_start ? + initial_state.time().value() : ideal_start; // Factor in battery drain while waiting to move to start waypoint. If a robot // is initially at a charging waypoint, it is assumed to be continually charging - if (drain_battery && wait_until > initial_state.finish_time() && - initial_state.waypoint() != initial_state.charging_waypoint()) + if (drain_battery && wait_until > initial_state.time().value() && + initial_state.waypoint().value() != + initial_state.dedicated_charging_waypoint().value()) { rmf_traffic::Duration wait_duration( - wait_until - initial_state.finish_time()); + wait_until - initial_state.time().value()); dSOC_device = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_device; @@ -206,10 +209,12 @@ std::optional Loop::Model::estimate_finish( if (battery_soc <= task_planning_constraints.threshold_soc()) return std::nullopt; - if (_finish_waypoint != initial_state.charging_waypoint()) + if (_finish_waypoint != initial_state.dedicated_charging_waypoint().value()) { - const auto endpoints = std::make_pair(_finish_waypoint, - initial_state.charging_waypoint()); + const auto endpoints = std::make_pair( + _finish_waypoint, + initial_state.dedicated_charging_waypoint().value()); + const auto& cache_result = estimate_cache.get(endpoints); if (cache_result) { @@ -260,11 +265,11 @@ std::optional Loop::Model::estimate_finish( rmf_traffic::agv::Planner::Start location{ state_finish_time, _finish_waypoint, - initial_state.location().orientation()}; - State state{ + initial_state.orientation().value()}; + auto state = State().load_basic( std::move(location), - initial_state.charging_waypoint(), - battery_soc}; + initial_state.dedicated_charging_waypoint().value(), + battery_soc); return Estimate(state, wait_until); } diff --git a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp index 15b8d05d..e10281e3 100644 --- a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp @@ -68,15 +68,17 @@ ConstRequestPtr ParkRobotFactory::make_request( const State& state) const { std::string id = "ParkRobot" + generate_uuid(); - const auto start_waypoint = state.location().waypoint(); + const auto start_waypoint = state.waypoint().value(); const auto finish_waypoint = _pimpl->parking_waypoint.has_value() ? - _pimpl->parking_waypoint.value() : state.charging_waypoint(); + _pimpl->parking_waypoint.value() : + state.dedicated_charging_waypoint().value(); + const auto request = Loop::make( start_waypoint, finish_waypoint, 1, id, - state.finish_time(), + state.time().value(), nullptr, true); diff --git a/rmf_task/test/unit/agv/test_State.cpp b/rmf_task/test/unit/agv/test_State.cpp index a4fd9619..f5684256 100644 --- a/rmf_task/test/unit/agv/test_State.cpp +++ b/rmf_task/test/unit/agv/test_State.cpp @@ -32,45 +32,43 @@ SCENARIO("Robot States") 0, 0.0}; - std::unique_ptr basic_state; - WHEN("Empty battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_task::State{ + CHECK_NOTHROW(rmf_task::State().load_basic( basic_start, 0, - 0.0})); + 0.0)); } WHEN("Full battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_task::State{ + CHECK_NOTHROW(rmf_task::State().load_basic( basic_start, 0, - 1.0})); + 1.0)); } WHEN("Half battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_task::State{ + CHECK_NOTHROW(rmf_task::State().load_basic( basic_start, 0, - 0.5})); + 0.5)); } WHEN("Battery soc more than 1.0") { - CHECK_THROWS(basic_state.reset(new rmf_task::State{ + CHECK_THROWS(rmf_task::State().load_basic( basic_start, 0, - 1.0 + 1e-4})); + 1.0 + 1e-4)); } WHEN("Battery soc less than 0.0") { - CHECK_THROWS(basic_state.reset(new rmf_task::State{ + CHECK_THROWS(rmf_task::State().load_basic( basic_start, 0, - 0.0 - 1e-4})); + 0.0 - 1e-4)); } } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 8885ee1b..17acb527 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -57,10 +57,14 @@ inline void CHECK_TIMES(const TaskPlanner::Assignments& assignments, for (std::size_t i = 0; i < agent.size(); ++i) { CHECK(agent[i].deployment_time() >= now); - CHECK(agent[i].state().finish_time() >= agent[i].deployment_time()); + CHECK(agent[i].finish_state().time().value() + >= agent[i].deployment_time()); + if (i == 0) continue; - CHECK(agent[i].deployment_time() >= agent[i-1].state().finish_time()); + + CHECK(agent[i].deployment_time() + >= agent[i-1].finish_state().time().value()); } } } @@ -78,7 +82,7 @@ inline bool check_implicit_charging_task_start( continue; } - const auto& s = agent[0].state(); + const auto& s = agent[0].finish_state(); auto is_charge_request = std::dynamic_pointer_cast< const rmf_task::requests::ChargeBattery::Description>( @@ -110,17 +114,17 @@ inline void display_solution( std::cout << "--Agent: " << i << std::endl; for (const auto& a : assignments[i]) { - const auto& s = a.state(); + const auto& s = a.finish_state(); const double request_seconds = a.request()->earliest_start_time().time_since_epoch().count(); const double start_seconds = a.deployment_time().time_since_epoch().count(); - const rmf_traffic::Time finish_time = s.finish_time(); + const rmf_traffic::Time finish_time = s.time().value(); const double finish_seconds = finish_time.time_since_epoch().count(); std::cout << std::fixed << " <" << a.request()->id() << ": " << request_seconds << ", " << start_seconds - << ", "<< finish_seconds << ", " << 100* s.battery_soc() + << ", "<< finish_seconds << ", " << 100*s.battery_soc().value() << "%>" << std::endl; } } @@ -237,8 +241,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, - rmf_task::State{second_location, 2, 1.0} + rmf_task::State().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 2, 1.0) }; std::vector requests = @@ -320,8 +324,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, - rmf_task::State{second_location, 2, 1.0} + rmf_task::State().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 2, 1.0) }; std::vector requests = @@ -467,8 +471,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, initial_soc}, - rmf_task::State{second_location, 2, initial_soc} + rmf_task::State().load_basic(first_location, 13, initial_soc), + rmf_task::State().load_basic(second_location, 2, initial_soc) }; std::vector requests = @@ -567,8 +571,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 9, 1.0}, - rmf_task::State{second_location, 2, 1.0} + rmf_task::State().load_basic(first_location, 9, 1.0), + rmf_task::State().load_basic(second_location, 2, 1.0) }; std::vector requests = @@ -712,7 +716,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -758,7 +762,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 0.0}, + rmf_task::State().load_basic(first_location, 13, 0.0) }; std::vector requests = @@ -805,7 +809,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -904,7 +908,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -970,7 +974,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -1052,7 +1056,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -1135,8 +1139,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, - rmf_task::State{second_location, 1, 1.0} + rmf_task::State().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0) }; std::vector requests = @@ -1279,9 +1283,9 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, - rmf_task::State{second_location, 1, 1.0}, - rmf_task::State{third_location, 5, 1.0} + rmf_task::State().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0), + rmf_task::State().load_basic(third_location, 5, 1.0) }; std::vector requests = @@ -1448,8 +1452,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, initial_soc}, - rmf_task::State{second_location, 2, initial_soc} + rmf_task::State().load_basic(first_location, 13, initial_soc), + rmf_task::State().load_basic(second_location, 2, initial_soc) }; std::vector requests = @@ -1535,7 +1539,7 @@ SCENARIO("Grid World") const rmf_task::requests::ChargeBattery::Description>( assignment.request()->description())) { - CHECK(assignment.state().battery_soc() == recharge_soc); + CHECK(assignment.finish_state().battery_soc() == recharge_soc); } } } @@ -1551,7 +1555,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0) }; const auto start_time = @@ -1617,7 +1621,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, initial_soc}, + rmf_task::State().load_basic(first_location, 13, initial_soc) }; std::vector requests = @@ -1692,8 +1696,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, - rmf_task::State{second_location, 1, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0) }; std::vector requests = @@ -1802,8 +1806,8 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::State{first_location, 13, 1.0}, - rmf_task::State{second_location, 1, 1.0}, + rmf_task::State().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0) }; std::vector requests = @@ -1856,8 +1860,8 @@ SCENARIO("Grid World") { const auto last_assignment = agent.back(); CHECK_FALSE(last_assignment.request()->automatic()); - const auto& state = last_assignment.state(); - CHECK_FALSE(state.location().waypoint() == state.charging_waypoint()); + const auto& state = last_assignment.finish_state(); + CHECK_FALSE(state.waypoint() == state.dedicated_charging_waypoint()); } } @@ -1892,8 +1896,8 @@ SCENARIO("Grid World") { const auto last_assignment = agent.back(); CHECK(last_assignment.request()->automatic()); - const auto& state = last_assignment.state(); - CHECK(state.location().waypoint() == state.charging_waypoint()); + const auto& state = last_assignment.finish_state(); + CHECK(state.waypoint() == state.dedicated_charging_waypoint()); } } } From b181017dea5a82c3211bc43c221ad82762c80c65 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 2 Sep 2021 22:01:39 +0800 Subject: [PATCH 03/85] Defining task execution interfaces Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/CompositeData.hpp | 7 + rmf_task/include/rmf_task/Request.hpp | 5 +- rmf_task/include/rmf_task/State.hpp | 45 +------ .../include/rmf_task/execute/Condition.hpp | 62 +++++++++ rmf_task/include/rmf_task/execute/Phase.hpp | 90 +++++++++++++ rmf_task/include/rmf_task/execute/Task.hpp | 125 ++++++++++++++++++ rmf_task/src/rmf_task/Request.cpp | 6 +- 7 files changed, 295 insertions(+), 45 deletions(-) create mode 100644 rmf_task/include/rmf_task/execute/Condition.hpp create mode 100644 rmf_task/include/rmf_task/execute/Phase.hpp create mode 100644 rmf_task/include/rmf_task/execute/Task.hpp diff --git a/rmf_task/include/rmf_task/CompositeData.hpp b/rmf_task/include/rmf_task/CompositeData.hpp index 929ce17a..2aa1e1b7 100644 --- a/rmf_task/include/rmf_task/CompositeData.hpp +++ b/rmf_task/include/rmf_task/CompositeData.hpp @@ -100,6 +100,13 @@ class CompositeData } // namespace rmf_task +//============================================================================== +/// Define a component class that is convenient to use in a CompositeData +/// instance. The defined class will contain only one field whose type is +/// specified by Type. The name of the class will be Name. +#define RMF_TASK_DEFINE_COMPONENT(Type, Name) \ + struct Name { Type value; Name(Type input) : value(input) { } } + #include #endif // RMF_TASK__COMPOSITEDATA_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 5299b665..6570095f 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -78,6 +78,7 @@ class Request }; using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; /// Constructor /// @@ -97,7 +98,7 @@ class Request const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - DescriptionPtr description, + ConstDescriptionPtr description, bool automatic = false); /// The unique id for this request @@ -110,7 +111,7 @@ class Request ConstPriorityPtr priority() const; /// Get the description of this request - const DescriptionPtr& description() const; + const ConstDescriptionPtr& description() const; // Returns true if this request was automatically generated bool automatic() const; diff --git a/rmf_task/include/rmf_task/State.hpp b/rmf_task/include/rmf_task/State.hpp index d8852fc2..0770fd20 100644 --- a/rmf_task/include/rmf_task/State.hpp +++ b/rmf_task/include/rmf_task/State.hpp @@ -35,38 +35,17 @@ class State : public CompositeData public: /// The current waypoint of the robot state - struct CurrentWaypoint - { - std::size_t value; - - CurrentWaypoint(std::size_t input) - : value(input) {} - }; - + RMF_TASK_DEFINE_COMPONENT(std::size_t, CurrentWaypoint); std::optional waypoint() const; State& waypoint(std::size_t new_waypoint); /// The current orientation of the robot state - struct CurrentOrientation - { - double value; - - CurrentOrientation(double input) - : value(input) {} - }; - + RMF_TASK_DEFINE_COMPONENT(double, CurrentOrientation); std::optional orientation() const; State& orientation(double new_orientation); /// The current time for the robot - struct CurrentTime - { - rmf_traffic::Time value; - - CurrentTime(rmf_traffic::Time input) - : value(input) {} - }; - + RMF_TASK_DEFINE_COMPONENT(rmf_traffic::Time, CurrentTime); std::optional time() const; State& time(rmf_traffic::Time new_time); @@ -74,27 +53,13 @@ class State : public CompositeData // TODO(MXG): Consider removing this field and using some kind of // ChargingStrategy abstract interface to determine where and how the robots // should be charging. - struct DedicatedChargingPoint - { - std::size_t value; - - DedicatedChargingPoint(std::size_t input) - : value(input) {} - }; - + RMF_TASK_DEFINE_COMPONENT(std::size_t, DedicatedChargingPoint); std::optional dedicated_charging_waypoint() const; State& dedicated_charging_waypoint(std::size_t new_charging_waypoint); /// The current battery state of charge of the robot. This value is between /// 0.0 and 1.0. - struct CurrentBatterySoC - { - double value; - - CurrentBatterySoC(double input) - : value(input) {} - }; - + RMF_TASK_DEFINE_COMPONENT(double, CurrentBatterySoC); std::optional battery_soc() const; State& battery_soc(double new_battery_soc); diff --git a/rmf_task/include/rmf_task/execute/Condition.hpp b/rmf_task/include/rmf_task/execute/Condition.hpp new file mode 100644 index 00000000..d83b6aa6 --- /dev/null +++ b/rmf_task/include/rmf_task/execute/Condition.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__EXECUTE__CONDITION_HPP +#define RMF_TASK__EXECUTE__CONDITION_HPP + +#include +#include +#include + +namespace rmf_task { +namespace execute { + +//============================================================================== +class Condition +{ +public: + + enum class Status : uint32_t + { + Uninitialized = 0, + Standby, + Underway, + Delayed, + Blocked, + Error, + Failed, + Finished + }; + + virtual Status status() const = 0; + + inline bool finished() const { return status() == Status::Finished; } + + virtual std::string name() const = 0; + + virtual std::string detail() const = 0; + + virtual std::vector> + subconditions() const = 0; +}; + +using ConstConditionPtr = std::shared_ptr; + +} // namespace execute +} // namespace rmf_task + +#endif // RMF_TASK__EXECUTE__CONDITION_HPP diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/execute/Phase.hpp new file mode 100644 index 00000000..38ead858 --- /dev/null +++ b/rmf_task/include/rmf_task/execute/Phase.hpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__EXECUTE__PHASE_HPP +#define RMF_TASK__EXECUTE__PHASE_HPP + +#include + +#include + +#include + +#include + +namespace rmf_task { +namespace execute { + +//============================================================================== +class CompletedPhase +{ +public: + + const std::string& name() const; + const std::string& detail() const; + const std::vector& issues() const; + + rmf_traffic::Time start_time() const; + rmf_traffic::Time original_finish_estimate() const; + rmf_traffic::Time finish_time() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class ActivePhase +{ +public: + + /// The name of this phase. + virtual std::string name() const = 0; + + /// Details about this phase + virtual std::string detail() const = 0; + + /// The condition that needs to be satisfied for this phase to be complete + virtual ConstConditionPtr finish_condition() const = 0; + + /// The estimated finish time for this phase + virtual rmf_traffic::Time estimate_finish_time() const = 0; + + // Virtual destructor + virtual ~ActivePhase() = default; +}; + +using ConstActivePhasePtr = std::shared_ptr; + +//============================================================================== +class PendingPhase +{ +public: + + const std::string& name() const; + const std::string& detail() const; + rmf_traffic::Duration duration_estimate() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace execute +} // namespace rmf_task + +#endif // RMF_TASK__EXECUTE__PHASE_HPP diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/execute/Task.hpp new file mode 100644 index 00000000..0ed898ab --- /dev/null +++ b/rmf_task/include/rmf_task/execute/Task.hpp @@ -0,0 +1,125 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__EXECUTE__TASK_HPP +#define RMF_TASK__EXECUTE__TASK_HPP + +#include + +#include +#include + +namespace rmf_task { +namespace execute { + +//============================================================================== +/// Pure abstract interface for an executable Task +class Task +{ +public: + + /// Descriptions of the phases that have been completed + virtual const std::vector& completed_phases() const = 0; + + /// Interface for the phase that is currently active + virtual ConstActivePhasePtr active_phase() const = 0; + + /// Descriptions of the phases that are expected in the future + virtual std::vector pending_phases() const = 0; + + /// The ID of this Task + virtual std::string id() const = 0; + + /// The category of this Task + virtual std::string category() const = 0; + + /// Human-readable details about this task + virtual std::string detail() const = 0; + + /// Estimate the overall finishing time of the task + virtual rmf_traffic::Time estimate_finish_time() const = 0; + + /// The Resume class keeps track of when the Task is allowed to Resume. + /// You can either call the object's operator() or let the object expire to + /// tell the Task that it may resume. + class Resume + { + public: + + /// Call this object to tell the Task to resume. + void operator()() const; + + class Implementation; + private: + rmf_utils::unique_impl_ptr _pimpl; + }; + + /// Tell this Task that it needs to be interrupted. An interruption means + /// the robot may be commanded to do other tasks before this task resumes. + /// + /// Interruptions may occur to allow operators to take manual control of the + /// robot, or to engage automatic behaviors in response to emergencies, e.g. + /// fire alarms or code blues. + /// + /// \param[in] task_is_interrupted + /// This callback will be triggered when the Task has reached a state where + /// it is okay to start issuing other commands to the robot. + /// + /// \return an object to inform the Task when it is allowed to resume. + virtual Resume interrupt(std::function task_is_interrupted) = 0; + + // TODO(MXG): Should we have a pause() interface? It would be the same as + // interrupt() except without the expectation that the robot will do any other + // task before resuming. + + /// Tell the Task that it has been canceled. The behavior that follows a + /// cancellation will vary between different Tasks, but generally it means + /// that the robot should no longer try to complete its Task and should + /// instead try to return itself to an unencumbered state as quickly as + /// possible. + /// + /// The Task may continue to perform some phases after being canceled. The + /// pending_phases are likely to change after the Task is canceled, being + /// replaced with phases that will help to relieve the robot so it can + /// return to an unencumbered state. + /// + /// The Task should continue to be tracked as normal. When its finished + /// callback is triggered, the cancellation is complete. + virtual void cancel() = 0; + + /// Kill this Task. The behavior that follows a kill will vary between + /// different Tasks, but generally it means that the robot should be returned + /// to safe idle state as soon as possible, even if it remains encumbered by + /// something related to this Task. + /// + /// The finished callback will be triggered when the Task is fully killed. + /// + /// The kill() command supersedes the cancel() command. Calling cancel() after + /// calling kill() will have no effect. + virtual void kill() = 0; + + // Virtual destructor + virtual ~Task() = default; + +protected: + static Resume make_resumer(std::function callback); +}; + +} // namespace execute +} // namespace rmf_task + +#endif // RMF_TASK__TASK_HPP diff --git a/rmf_task/src/rmf_task/Request.cpp b/rmf_task/src/rmf_task/Request.cpp index 9530c7c4..cf830ecd 100644 --- a/rmf_task/src/rmf_task/Request.cpp +++ b/rmf_task/src/rmf_task/Request.cpp @@ -26,7 +26,7 @@ class Request::Implementation std::string id; rmf_traffic::Time earliest_start_time; rmf_task::ConstPriorityPtr priority; - DescriptionPtr description; + ConstDescriptionPtr description; bool automatic; }; @@ -35,7 +35,7 @@ Request::Request( const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - DescriptionPtr description, + ConstDescriptionPtr description, bool automatic) : _pimpl(rmf_utils::make_impl( Implementation { @@ -68,7 +68,7 @@ ConstPriorityPtr Request::priority() const } //============================================================================== -const Request::DescriptionPtr& Request::description() const +const Request::ConstDescriptionPtr& Request::description() const { return _pimpl->description; } From 096ae8a0b76f736c97332449c9924925322defd0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 3 Sep 2021 16:03:00 +0800 Subject: [PATCH 04/85] Adding a task logging interface Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Request.hpp | 1 + .../include/rmf_task/execute/Condition.hpp | 82 ++++++- rmf_task/include/rmf_task/execute/Log.hpp | 213 ++++++++++++++++++ rmf_task/include/rmf_task/execute/Task.hpp | 10 + 4 files changed, 303 insertions(+), 3 deletions(-) create mode 100644 rmf_task/include/rmf_task/execute/Log.hpp diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 6570095f..184fc4f6 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -74,6 +74,7 @@ class Request rmf_traffic::Time earliest_start_time, const Parameters& parameters) const = 0; + // Virtual destructor virtual ~Description() = default; }; diff --git a/rmf_task/include/rmf_task/execute/Condition.hpp b/rmf_task/include/rmf_task/execute/Condition.hpp index d83b6aa6..60ea4824 100644 --- a/rmf_task/include/rmf_task/execute/Condition.hpp +++ b/rmf_task/include/rmf_task/execute/Condition.hpp @@ -18,6 +18,11 @@ #ifndef RMF_TASK__EXECUTE__CONDITION_HPP #define RMF_TASK__EXECUTE__CONDITION_HPP +#include + +#include + +#include #include #include #include @@ -25,36 +30,107 @@ namespace rmf_task { namespace execute { +class Condition; +using ConstConditionPtr = std::shared_ptr; + //============================================================================== class Condition { public: + /// A simple computer-friendly indicator of the current status of this + /// condition. This enum may be used to automatically identify when a + /// condition requires special attention, e.g. logging a warning or alerting + /// an operator. enum class Status : uint32_t { + /// The condition status has not been initialized. This is a sentinel value + /// that should not generally be used. Uninitialized = 0, + + /// The condition is on standby. It cannot be satisfied yet, and that is its + /// expected status. Standby, + + /// The condition is underway, and coming along as expected. Underway, + + /// The condition is underway but it has been temporarily delayed. Delayed, + + /// The condition is underway but it has been blocked. The blockage may + /// require manual intervention to fix. Blocked, + + /// An error has occurred that the Task implementation does not know how to + /// deal with. Manual intervention is needed to get the task back on track. Error, + + /// The condition cannot ever be satisfied, even with manual intervention. + /// This may mean that the Task cannot be completed if it does not have + /// an automated way to recover from this failure state. Failed, + + /// The condition is satisfied. Finished }; + /// The current Status of this condition virtual Status status() const = 0; + /// Simple wrapper for identifying when a condition is finished inline bool finished() const { return status() == Status::Finished; } + /// The "name" of this condition. Ideally a short, simple piece of text that + /// helps a human being intuit what this condition is expecting at a glance. virtual std::string name() const = 0; + /// A detailed explanation of this condition. virtual std::string detail() const = 0; - virtual std::vector> - subconditions() const = 0; + /// A view of the event log for this condition. + virtual Log::View log() const = 0; + + /// Get more granular subconditions of this condition, if any exist. + virtual std::vector subconditions() const = 0; + + class Snapshot; + + // Virtual destructor + virtual ~Condition() = default; }; -using ConstConditionPtr = std::shared_ptr; +//============================================================================== +/// A snapshot of the state of a condition. This snapshot can be read while the +/// original condition is arbitrarily changed, and there is no risk of a race +/// condition, as long as the snapshot is not being created while the condition +/// is changing. +class Condition::Snapshot : public Condition +{ +public: + + /// Make a snapshot of the current state of a Condition + static ConstConditionPtr make(const Condition& other); + + // Documentation inherited + Status status() const final; + + // Documentation inherited + std::string name() const final; + + // Documentation inherited + std::string detail() const final; + + // Documentation inherited + Log::View log() const final; + + // Documentation inherited + std::vector subconditions() const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; } // namespace execute } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/execute/Log.hpp b/rmf_task/include/rmf_task/execute/Log.hpp new file mode 100644 index 00000000..99833aa6 --- /dev/null +++ b/rmf_task/include/rmf_task/execute/Log.hpp @@ -0,0 +1,213 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__EXECUTE__LOG_HPP +#define RMF_TASK__EXECUTE__LOG_HPP + +#include + +#include +#include +#include + +namespace rmf_task { +namespace execute { + +class Log; +using ConstLogPtr = std::shared_ptr; + +//============================================================================== +class Log +{ +public: + // Inner class declarations. See below for their definitions. + class Entry; + class View; + class Reader; + + /// Construct a log. + /// + /// \param[in] clock + /// Specify a clock for this log to use. If nullptr is given, then + /// std::chrono::system_clock::now() will be used. + Log(std::function clock = nullptr); + + // TODO(MXG): Should we have a debug log option? + + /// Add an informational entry to the log. + void info(std::string text); + + /// Add a warning to the log. + void warn(std::string text); + + /// Add an error to the log. + void error(std::string text); + + /// Insert an arbitrary entry into the log. + void insert(Log::Entry entry); + + /// Get a View of the current state of this log. Any new entries that are + /// added after calling this function will not be visible to the View that + /// is returned. + View view() const; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +//============================================================================== +/// A single entry within the log. +class Log::Entry +{ +public: + + /// A computer-friendly ranking of how serious the log entry is. + enum class Tier : uint32_t + { + /// This is a sentinel value that should not generally be used. + Uninitialized = 0, + + /// An expected occurrence took place. + Info, + + /// An unexpected, problematic occurrence took place, but it can be + /// recovered from. Human attention is recommended but not necessary. + Warning, + + /// A problem happened, and humans should be alerted. + Error + }; + + /// What was the tier of this entry. + Tier tier() const; + + /// What was the timestamp of this entry. + std::chrono::system_clock::time_point time() const; + + /// What was the text of this entry. + const std::string& text() const; + + class Implementation; +private: + Entry(); + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// A snapshot view of the log's contents. This is thread-safe to read even +/// while new entries are being added to the log, but those new entries will +/// not be seen by this View. You must retrieve a new View to see new entries. +class Log::View +{ +public: + class Implementation; +private: + View(); + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// A Reader that can iterate through the Views of Logs. The Reader will keep +/// track of which Entries have already been viewed, so every Entry read by a +/// single Reader instance is unique. +class Log::Reader +{ +public: + + /// Construct a Reader + Reader(); + + class Iterable; + + /// Create an object that can iterate through the entries of a View. Any + /// entries that have been read by this Reader in the past will be skipped. + /// This can be used in a range-based for loop, e.g.: + /// + /// \code + /// for (const auto& entry : reader.read(view)) + /// { + /// std::cout << entry.text() << std::endl; + /// } + /// \endcode + Iterable read(const View& view); + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +//============================================================================== +class Log::Reader::Iterable +{ +public: + + class iterator; + using const_iterator = iterator; + + /// Get the beginning iterator of the read + iterator begin() const; + + /// Get the ending iterator of the read + iterator end() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Log::Reader::Iterable::iterator +{ +public: + + /// Dereference operator + const Entry& operator*() const; + + /// Drill-down operator + const Entry* operator->() const; + + /// Pre-increment operator: ++it + /// + /// \note This is more efficient than the post-increment operator. + /// + /// \warning It is undefined behavior to perform this operation on an iterator + /// that is equal to Log::Reader::Iterable::end(). + /// + /// \return a reference to the iterator itself + iterator& operator++(); + + /// Post-increment operator: it++ + /// + /// \warning It is undefined behavior to perform this operation on an iterator + /// that is equal to Log::Reader::Iterable::end(). + /// + /// \return a copy of the iterator before it was incremented. + iterator& operator++(int); + + /// Equality comparison operator + bool operator==(const iterator& other) const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace execute +} // namespace rmf_task + +#endif // RMF_TASK__EXECUTE__LOG_HPP diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/execute/Task.hpp index 0ed898ab..291510a2 100644 --- a/rmf_task/include/rmf_task/execute/Task.hpp +++ b/rmf_task/include/rmf_task/execute/Task.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__EXECUTE__TASK_HPP #include +#include #include #include @@ -50,6 +51,9 @@ class Task /// Human-readable details about this task virtual std::string detail() const = 0; + /// The original task Request that spawned this Task + virtual const Request& original_request() const = 0; + /// Estimate the overall finishing time of the task virtual rmf_traffic::Time estimate_finish_time() const = 0; @@ -116,6 +120,12 @@ class Task virtual ~Task() = default; protected: + + /// Used by classes that inherit the Task interface to create a Resumer object + /// + /// \param[in] callback + /// Provide the callback that should be triggered when the Task is allowed + /// to resume static Resume make_resumer(std::function callback); }; From 86c3c1335ad763f253917149db275d9a57d2b06b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 3 Sep 2021 17:44:22 +0800 Subject: [PATCH 05/85] Drafting the interfaces for phase sequence tasks Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/execute/Phase.hpp | 2 +- .../rmf_task/sequence/PhaseDescription.hpp | 80 ++++++++++++++ .../rmf_task/sequence/PhaseFactory.hpp | 95 ++++++++++++++++ rmf_task/include/rmf_task/sequence/Task.hpp | 72 ++++++++++++ .../rmf_task/sequence/TaskDescription.hpp | 103 ++++++++++++++++++ 5 files changed, 351 insertions(+), 1 deletion(-) create mode 100644 rmf_task/include/rmf_task/sequence/PhaseDescription.hpp create mode 100644 rmf_task/include/rmf_task/sequence/PhaseFactory.hpp create mode 100644 rmf_task/include/rmf_task/sequence/Task.hpp create mode 100644 rmf_task/include/rmf_task/sequence/TaskDescription.hpp diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/execute/Phase.hpp index 38ead858..768f6b11 100644 --- a/rmf_task/include/rmf_task/execute/Phase.hpp +++ b/rmf_task/include/rmf_task/execute/Phase.hpp @@ -36,7 +36,7 @@ class CompletedPhase const std::string& name() const; const std::string& detail() const; - const std::vector& issues() const; + const Log::View& log() const; rmf_traffic::Time start_time() const; rmf_traffic::Time original_finish_estimate() const; diff --git a/rmf_task/include/rmf_task/sequence/PhaseDescription.hpp b/rmf_task/include/rmf_task/sequence/PhaseDescription.hpp new file mode 100644 index 00000000..80032858 --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/PhaseDescription.hpp @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASEDESCRIPTION_HPP +#define RMF_TASK__SEQUENCE__PHASEDESCRIPTION_HPP + +#include +#include + +#include +#include +#include +#include + +namespace rmf_task { +namespace sequence { + +//============================================================================== +class PhaseDescription +{ +public: + + /// An abstract interface for computing estimates and invariants related to + /// this phase. + class Model + { + public: + + /// Estimate the state that the robot will have when the phase is finished. + /// + /// \param[in] initial_state + /// + virtual std::optional estimate_finish( + State initial_state, + const Constraints& constraints) const = 0; + + /// Estimate the invariant component of the request's duration. + virtual rmf_traffic::Duration invariant_duration() const = 0; + + /// Get the components of the finish state that this phase is guaranteed to + /// result in once the phase is finished. + virtual State invariant_finish_state() const = 0; + + // Virtual destructor + virtual ~Model() = default; + }; + + /// Generate a Model for this phase based on its description, parameters, and + /// the invariants of its initial state. + virtual std::shared_ptr make_model( + State invariant_initial_state, + const Parameters& parameters) const = 0; + + /// Get the human-friendly description of this phase as pending + virtual const execute::PendingPhase& pending() const = 0; + + // Virtual destructor + virtual ~PhaseDescription() = default; +}; + +using ConstPhaseDescriptionPtr = std::shared_ptr; + +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASEDESCRIPTION_HPP diff --git a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp new file mode 100644 index 00000000..033429df --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASEFACTORY_HPP +#define RMF_TASK__SEQUENCE__PHASEFACTORY_HPP + +#include +#include +#include + +namespace rmf_task { +namespace sequence { + +//============================================================================== +/// A factory for generating execute::Phase instances from descriptions. +class PhaseFactory +{ +public: + + /// Construct an empty PhaseFactory + PhaseFactory(); + + /// Signature for activating a phase + /// + /// \tparam Description + /// A class that implements the sequence::PhaseDescription interface + /// + /// \param[in] description + /// An immutable reference to the relevant Description instance + /// + /// \param[in] update + /// A callback that will be triggered when the phase has a significant + /// update in its status. + /// + /// \param[in] finished + /// A callback that will be triggered when the phase has finished. + /// + /// \return an active, running instance of the described phase. + template + using Activate = + std::function< + execute::ConstActivePhasePtr( + const Description& description, + std::function update, + std::function finished) + >; + + /// Add a callback to convert from a PhaseDescription into an active phase. + /// + /// \tparam Description + /// A class that implements the sequence::PhaseDescription interface + template + void add_activator(Activate activator); + + /// Build a Task object based on a phase sequence description. + /// + /// \param[in] description + /// The description of the phase sequence + /// + /// \param[in] update + /// A callback that will be triggered when the task has a significant update + /// + /// \param[in] finished + /// A callback that will be triggered when the task has finished + /// + /// \return an active, running instance of the described task. + std::shared_ptr build( + ConstTaskDescriptionPtr description, + std::function update, + std::function finished) const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + + +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASEFACTORY_HPP diff --git a/rmf_task/include/rmf_task/sequence/Task.hpp b/rmf_task/include/rmf_task/sequence/Task.hpp new file mode 100644 index 00000000..42e67953 --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/Task.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__TASK_HPP +#define RMF_TASK__SEQUENCE__TASK_HPP + +#include + +namespace rmf_task { +namespace sequence { + +//============================================================================== +class Task : public execute::Task +{ +public: + + // Documentation inherited + const std::vector& completed_phases() const final; + + // Documentation inherited + execute::ConstActivePhasePtr active_phase() const final; + + // Documentation inherited + std::vector pending_phases() const final; + + // Documentation inherited + std::string id() const final; + + // Documentation inherited + std::string category() const final; + + // Documentation inherited + std::string detail() const final; + + // Documentation inherited + const Request& original_request() const final; + + // Documentation inherited + rmf_traffic::Time estimate_finish_time() const final; + + // Documentation inherited + Resume interrupt(std::function task_is_interrupted) final; + + // Documentation inherited + void cancel() final; + + // Documentation inherited + void kill() final; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__TASK_HPP diff --git a/rmf_task/include/rmf_task/sequence/TaskDescription.hpp b/rmf_task/include/rmf_task/sequence/TaskDescription.hpp new file mode 100644 index 00000000..1e9280a9 --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/TaskDescription.hpp @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__TASKDESCRIPTION_HPP +#define RMF_TASK__SEQUENCE__TASKDESCRIPTION_HPP + +#include +#include + +#include + +namespace rmf_task { +namespace sequence { + +//============================================================================== +/// The Request::Description implementation for a phase sequence task. +class TaskDescription : public Request::Description +{ +public: + + class Builder; + class Model; + + // Documentation inherited + std::shared_ptr make_model( + rmf_traffic::Time earliest_start_time, + const Parameters& parameters) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +using ConstTaskDescriptionPtr = std::shared_ptr; + +//============================================================================== +class TaskDescription::Builder +{ +public: + + /// Get the builder ready. + Builder(); + + /// Add a phase to the sequence. + /// + /// \param[in] description + /// A description of the phase + /// + /// \param[in] cancellation_sequence + /// This phase sequence will be run if the task is cancelled during this + /// phase. + void add_phase( + ConstPhaseDescriptionPtr description, + std::vector cancellation_sequence); + + /// Generate a TaskDescription instance from the phases that have been given + /// to the builder. + ConstTaskDescriptionPtr build(); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// The implementation of a task description model for a phase sequence task. +class TaskDescription::Model : public Request::Model +{ +public: + + // Documentation inherited + std::optional estimate_finish( + const State& initial_state, + const Constraints& task_planning_constraints, + EstimateCache& estimate_cache) const final; + + // Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace sequence +} // namespace rmf_task + + +#endif // RMF_TASK__SEQUENCE__TASKDESCRIPTION_HPP From e00b02cc4b406e367be8d8b53532caaf3024e894 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 3 Sep 2021 17:46:46 +0800 Subject: [PATCH 06/85] Satisfy uncrustify Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/CompositeData.hpp | 2 +- rmf_task/include/rmf_task/sequence/PhaseFactory.hpp | 8 ++++---- rmf_task/include/rmf_task/sequence/Task.hpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmf_task/include/rmf_task/CompositeData.hpp b/rmf_task/include/rmf_task/CompositeData.hpp index 2aa1e1b7..c8bb0410 100644 --- a/rmf_task/include/rmf_task/CompositeData.hpp +++ b/rmf_task/include/rmf_task/CompositeData.hpp @@ -105,7 +105,7 @@ class CompositeData /// instance. The defined class will contain only one field whose type is /// specified by Type. The name of the class will be Name. #define RMF_TASK_DEFINE_COMPONENT(Type, Name) \ - struct Name { Type value; Name(Type input) : value(input) { } } + struct Name { Type value; Name(Type input) : value(input) {} } #include diff --git a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp index 033429df..98222578 100644 --- a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp +++ b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp @@ -53,10 +53,10 @@ class PhaseFactory template using Activate = std::function< - execute::ConstActivePhasePtr( - const Description& description, - std::function update, - std::function finished) + execute::ConstActivePhasePtr( + const Description& description, + std::function update, + std::function finished) >; /// Add a callback to convert from a PhaseDescription into an active phase. diff --git a/rmf_task/include/rmf_task/sequence/Task.hpp b/rmf_task/include/rmf_task/sequence/Task.hpp index 42e67953..fc364b7c 100644 --- a/rmf_task/include/rmf_task/sequence/Task.hpp +++ b/rmf_task/include/rmf_task/sequence/Task.hpp @@ -53,7 +53,7 @@ class Task : public execute::Task rmf_traffic::Time estimate_finish_time() const final; // Documentation inherited - Resume interrupt(std::function task_is_interrupted) final; + Resume interrupt(std::function task_is_interrupted) final; // Documentation inherited void cancel() final; From 7a7d7d3c18d698df40e00da00ffd8bc45977c768 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 3 Sep 2021 18:10:59 +0800 Subject: [PATCH 07/85] Draft interface for Task Factory Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/execute/Task.hpp | 2 + .../include/rmf_task/execute/TaskFactory.hpp | 98 +++++++++++++++++++ .../rmf_task/sequence/PhaseFactory.hpp | 5 +- 3 files changed, 103 insertions(+), 2 deletions(-) create mode 100644 rmf_task/include/rmf_task/execute/TaskFactory.hpp diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/execute/Task.hpp index 291510a2..3e2f0662 100644 --- a/rmf_task/include/rmf_task/execute/Task.hpp +++ b/rmf_task/include/rmf_task/execute/Task.hpp @@ -129,6 +129,8 @@ class Task static Resume make_resumer(std::function callback); }; +using ConstTaskPtr = std::shared_ptr; + } // namespace execute } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/execute/TaskFactory.hpp b/rmf_task/include/rmf_task/execute/TaskFactory.hpp new file mode 100644 index 00000000..be222e4c --- /dev/null +++ b/rmf_task/include/rmf_task/execute/TaskFactory.hpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__TASKFACTORY_HPP +#define RMF_TASK__TASKFACTORY_HPP + +#include +#include + + +namespace rmf_task { +namespace execute { + +//============================================================================== +/// A factory for generating Task instances from requests. +class TaskFactory +{ +public: + + /// Construct an empty TaskFactory + TaskFactory(); + + /// Signature for activating a task + /// + /// \tparam Description + /// A class that implements the Request::Description interface + /// + /// \param[in] request + /// An immutable reference to the relevant task request + /// + /// \param[in] description + /// The down-casted description of the task + /// + /// \param[in] update + /// A callback that will be triggered when the task has a significant + /// update in its status. + /// + /// \param[in] finished + /// A callback that will be triggered when the task has finished. + /// + /// \return an active, running instance of the requested task. + template + using Activate = + std::function< + execute::ConstTaskPtr( + const Request& request, + const Description& description, + std::function update, + std::function finished) + >; + + /// Add a callback to convert from a Request into an active Task. + /// + /// \tparam Description + /// A class that implements the Request::Description interface + template + void add_activator(Activate activator); + + /// Activate a Task object based on a Request::Description. + /// + /// \param[in] request + /// The task request + /// + /// \param[in] update + /// A callback that will be triggered when the task has a significant update + /// + /// \param[in] finished + /// A callback that will be triggered when the task has finished + /// + /// \return an active, running instance of the requested task. + std::shared_ptr activate( + const Request& request, + std::function update, + std::function finished); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace execute +} // namespace rmf_task + +#endif // RMF_TASK__TASKFACTORY_HPP diff --git a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp index 98222578..7dfb8781 100644 --- a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp +++ b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp @@ -66,7 +66,7 @@ class PhaseFactory template void add_activator(Activate activator); - /// Build a Task object based on a phase sequence description. + /// Activate a Task object based on a phase sequence description. /// /// \param[in] description /// The description of the phase sequence @@ -78,7 +78,8 @@ class PhaseFactory /// A callback that will be triggered when the task has finished /// /// \return an active, running instance of the described task. - std::shared_ptr build( + std::shared_ptr activate( + const Request& request, ConstTaskDescriptionPtr description, std::function update, std::function finished) const; From 15aac52166a3ac8be5b76ce5b2c47c3f006804ac Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 6 Sep 2021 17:24:10 +0800 Subject: [PATCH 08/85] Rework the design, improve consistency Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Request.hpp | 65 +++++- rmf_task/include/rmf_task/detail/Resume.hpp | 42 ++++ rmf_task/include/rmf_task/execute/Phase.hpp | 92 ++++++-- rmf_task/include/rmf_task/execute/Task.hpp | 64 +++--- .../include/rmf_task/execute/TaskFactory.hpp | 2 +- rmf_task/include/rmf_task/sequence/Phase.hpp | 213 ++++++++++++++++++ .../rmf_task/sequence/PhaseDescription.hpp | 80 ------- .../rmf_task/sequence/PhaseFactory.hpp | 96 -------- rmf_task/include/rmf_task/sequence/Task.hpp | 117 +++++++++- .../rmf_task/sequence/TaskDescription.hpp | 103 --------- .../rmf_task/BinaryPriorityCostCalculator.cpp | 8 +- rmf_task/src/rmf_task/Request.cpp | 60 +++-- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 8 +- .../rmf_task/agv/internal_task_planning.cpp | 2 +- .../rmf_task/agv/internal_task_planning.hpp | 2 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 32 +-- 16 files changed, 602 insertions(+), 384 deletions(-) create mode 100644 rmf_task/include/rmf_task/detail/Resume.hpp create mode 100644 rmf_task/include/rmf_task/sequence/Phase.hpp delete mode 100644 rmf_task/include/rmf_task/sequence/PhaseDescription.hpp delete mode 100644 rmf_task/include/rmf_task/sequence/PhaseFactory.hpp delete mode 100644 rmf_task/include/rmf_task/sequence/TaskDescription.hpp diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 184fc4f6..38972e03 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -36,6 +36,49 @@ namespace rmf_task { class Request { public: + + class Tag + { + public: + + /// Constructor + /// + /// \param[in] id_ + /// The identify of the request + /// + /// \param[in] earliest_start_time_ + /// The earliest time that the request may begin + /// + /// \param[in] priority_ + /// The priority of the request + /// + /// \param[in] automatic_ + /// Whether this request was automatically generated + Tag( + std::string id_, + rmf_traffic::Time earliest_start_time_, + ConstPriorityPtr priority_, + bool automatic_ = false); + + /// The unique id for this request + const std::string& id() const; + + /// Get the earliest time that this request may begin + rmf_traffic::Time earliest_start_time() const; + + /// Get the priority of this request + ConstPriorityPtr priority() const; + + // Returns true if this request was automatically generated + bool automatic() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + using ConstTagPtr = std::shared_ptr; + /// An abstract interface for computing the estimate and invariant durations /// of this request class Model @@ -95,6 +138,8 @@ class Request /// /// \param[in] automatic /// True if this request is auto-generated + // + // TODO(MXG): Deprecate this constructor? Request( const std::string& id, rmf_traffic::Time earliest_start_time, @@ -102,21 +147,21 @@ class Request ConstDescriptionPtr description, bool automatic = false); - /// The unique id for this request - const std::string& id() const; - - /// Get the earliest time that this request may begin - rmf_traffic::Time earliest_start_time() const; + /// Constructor + /// + /// \param[in] tag + /// Tag of the request + /// + /// \param[in] description + /// Description for this request + Request(ConstTagPtr tag, ConstDescriptionPtr description); - /// Get the priority of this request - ConstPriorityPtr priority() const; + /// Get the tag of this request + const ConstTagPtr& tag() const; /// Get the description of this request const ConstDescriptionPtr& description() const; - // Returns true if this request was automatically generated - bool automatic() const; - class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_task/include/rmf_task/detail/Resume.hpp b/rmf_task/include/rmf_task/detail/Resume.hpp new file mode 100644 index 00000000..a1413b3c --- /dev/null +++ b/rmf_task/include/rmf_task/detail/Resume.hpp @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__DETAIL__RESUME_HPP +#define RMF_TASK__DETAIL__RESUME_HPP + +#include + +namespace rmf_task { +namespace detail { + +//============================================================================== +class Resume +{ +public: + + /// Call this object to tell the Task to resume. + void operator()() const; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace detail +} // namespace rmf_task + +#endif // RMF_TASK__DETAIL__RESUME_HPP diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/execute/Phase.hpp index 768f6b11..443dd3d5 100644 --- a/rmf_task/include/rmf_task/execute/Phase.hpp +++ b/rmf_task/include/rmf_task/execute/Phase.hpp @@ -30,16 +30,64 @@ namespace rmf_task { namespace execute { //============================================================================== -class CompletedPhase +class Phase { public: + class Tag; + using ConstTagPtr = std::shared_ptr; + + class Completed; + using ConstCompletedPtr = std::shared_ptr; + + class Active; + using ConstActivePtr = std::shared_ptr; + + class Snapshot; + using ConstSnapshotPtr = std::shared_ptr; + + class Pending; + using ConstPendingPtr = std::shared_ptr; +}; + +//============================================================================== +/// Basic static information about a phase. This information should go +/// unchanged from the Pending state, through the Active state, and into the +/// Completed state. +class Phase::Tag +{ +public: + + /// Name of the phase const std::string& name() const; + + /// Details about the phase const std::string& detail() const; + + /// The original (ideal) estimate of how long the phase will last + rmf_traffic::Duration original_duration_estimate() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// Information about a phase that has been completed. +class Phase::Completed +{ +public: + + /// Tag of the phase + const ConstTagPtr& tag() const; + + /// The final log of the phase const Log::View& log() const; + /// The actual time that the phase started rmf_traffic::Time start_time() const; - rmf_traffic::Time original_finish_estimate() const; + + /// The actual time that the phase finished. rmf_traffic::Time finish_time() const; class Implementation; @@ -48,15 +96,12 @@ class CompletedPhase }; //============================================================================== -class ActivePhase +class Phase::Active { public: - /// The name of this phase. - virtual std::string name() const = 0; - - /// Details about this phase - virtual std::string detail() const = 0; + /// Tag of the phase + virtual ConstTagPtr tag() const = 0; /// The condition that needs to be satisfied for this phase to be complete virtual ConstConditionPtr finish_condition() const = 0; @@ -65,19 +110,38 @@ class ActivePhase virtual rmf_traffic::Time estimate_finish_time() const = 0; // Virtual destructor - virtual ~ActivePhase() = default; + virtual ~Active() = default; }; -using ConstActivePhasePtr = std::shared_ptr; +//============================================================================== +class Phase::Snapshot : public Phase::Active +{ +public: + + /// Make a snapshot of an Active phase + static ConstSnapshotPtr make(const Active& active); + + // Documentation inherited + ConstTagPtr tag() const final; + + // Documentation inherited + ConstConditionPtr finish_condition() const final; + + // Documentation inherited + rmf_traffic::Time estimate_finish_time() const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; //============================================================================== -class PendingPhase +class Phase::Pending { public: - const std::string& name() const; - const std::string& detail() const; - rmf_traffic::Duration duration_estimate() const; + /// Tag of the phase + const ConstTagPtr& tag() const; class Implementation; private: diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/execute/Task.hpp index 3e2f0662..6fe9466f 100644 --- a/rmf_task/include/rmf_task/execute/Task.hpp +++ b/rmf_task/include/rmf_task/execute/Task.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__EXECUTE__TASK_HPP #include +#include #include #include @@ -33,44 +34,48 @@ class Task { public: - /// Descriptions of the phases that have been completed - virtual const std::vector& completed_phases() const = 0; + /// Basic static information about the task. + class Tag + { + public: - /// Interface for the phase that is currently active - virtual ConstActivePhasePtr active_phase() const = 0; + /// The original request that this Task is carrying out + const Request::ConstTagPtr& request() const; - /// Descriptions of the phases that are expected in the future - virtual std::vector pending_phases() const = 0; + /// The category of this Task. + const std::string& category() const; + + /// Details about this Task. + const std::string& detail() const; + + /// The original finish estimate of this Task. + rmf_traffic::Time original_finish_estimate() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; - /// The ID of this Task - virtual std::string id() const = 0; + /// Descriptions of the phases that have been completed + virtual const std::vector& + completed_phases() const = 0; - /// The category of this Task - virtual std::string category() const = 0; + /// Interface for the phase that is currently active + virtual Phase::ConstActivePtr active_phase() const = 0; - /// Human-readable details about this task - virtual std::string detail() const = 0; + /// Descriptions of the phases that are expected in the future + virtual std::vector pending_phases() const = 0; - /// The original task Request that spawned this Task - virtual const Request& original_request() const = 0; + /// The request tag of this Task + virtual const Request::ConstTagPtr& tag() const = 0; /// Estimate the overall finishing time of the task virtual rmf_traffic::Time estimate_finish_time() const = 0; /// The Resume class keeps track of when the Task is allowed to Resume. - /// You can either call the object's operator() or let the object expire to - /// tell the Task that it may resume. - class Resume - { - public: - - /// Call this object to tell the Task to resume. - void operator()() const; - - class Implementation; - private: - rmf_utils::unique_impl_ptr _pimpl; - }; + /// You can either call the Resume object's operator() or let the object + /// expire to tell the Task that it may resume. + class Resume : public detail::Resume {}; /// Tell this Task that it needs to be interrupted. An interruption means /// the robot may be commanded to do other tasks before this task resumes. @@ -107,10 +112,11 @@ class Task /// Kill this Task. The behavior that follows a kill will vary between /// different Tasks, but generally it means that the robot should be returned - /// to safe idle state as soon as possible, even if it remains encumbered by + /// to a safe idle state as soon as possible, even if it remains encumbered by /// something related to this Task. /// - /// The finished callback will be triggered when the Task is fully killed. + /// The Task should continue to be tracked as normal. When its finished + /// callback is triggered, the killing is complete. /// /// The kill() command supersedes the cancel() command. Calling cancel() after /// calling kill() will have no effect. diff --git a/rmf_task/include/rmf_task/execute/TaskFactory.hpp b/rmf_task/include/rmf_task/execute/TaskFactory.hpp index be222e4c..b0a25c27 100644 --- a/rmf_task/include/rmf_task/execute/TaskFactory.hpp +++ b/rmf_task/include/rmf_task/execute/TaskFactory.hpp @@ -57,7 +57,7 @@ class TaskFactory using Activate = std::function< execute::ConstTaskPtr( - const Request& request, + const Request::ConstTagPtr& request, const Description& description, std::function update, std::function finished) diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task/include/rmf_task/sequence/Phase.hpp new file mode 100644 index 00000000..05a550f5 --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/Phase.hpp @@ -0,0 +1,213 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASE_HPP +#define RMF_TASK__SEQUENCE__PHASE_HPP + +#include +#include +#include +#include +#include + +namespace rmf_task { +namespace sequence { + +//============================================================================== +/// A factory for generating execute::ActivePhase instances from descriptions. +class Phase +{ +public: + + // Declarations + class Active; + using ActivePtr = std::shared_ptr; + + class Activator; + using ActivatorPtr = std::shared_ptr; + using ConstActivatorPtr = std::shared_ptr; + + class Description; + using ConstDescriptionPtr = std::shared_ptr; +}; + +//============================================================================== +/// The interface for an Active phase within a phase sequence task. +class Phase::Active : public execute::Phase::Active +{ +public: + + /// The Resume class keeps track of when the phase is allowed to Resume. + /// You can either call the Resume object's operator() or let the object + /// expire to tell the phase that it may resume. + class Resume : public detail::Resume {}; + + /// Tell this phase that it needs to be interrupted. An interruption means + /// the robot may be commanded to do other tasks before this phase resumes. + /// + /// Interruptions may occur to allow operators to take manual control of the + /// robot, or to engage automatic behaviors in response to emergencies, e.g. + /// fire alarms or code blues. + virtual Resume interrupt(std::function task_is_interrupted) = 0; + + /// Tell the phase that it has been canceled. The behavior that follows a + /// cancellation will vary between different phases, but generally it means + /// that the robot should no longer try to complete its Task and should + /// instead try to return itself to an unencumbered state as quickly as + /// possible. + /// + /// The phase may continue to perform some actions after being canceled. + /// + /// The phase should continue to be tracked as normal. When its finished + /// callback is triggered, the cancellation is complete. + virtual void cancel() = 0; + + /// Kill this phase. The behavior that follows a kill will vary between + /// different phases, but generally it means that the robot should be returned + /// to a safe idle state as soon as possible, even if it remains encumbered by + /// something related to this Task. + /// + /// The phase should continue to be tracked as normal. When its finished + /// callback is triggered, the killing is complete. + /// + /// The kill() command supersedes the cancel() command. Calling cancel() after + /// calling kill() will have no effect. + virtual void kill() = 0; + + // Virtual destructor + virtual ~Active() = default; +}; + +//============================================================================== +class Phase::Activator +{ + /// Construct an empty Activator + Activator(); + + /// Signature for activating a phase + /// + /// \tparam Description + /// A class that implements the sequence::PhaseDescription interface + /// + /// \param[in] description + /// An immutable reference to the relevant Description instance + /// + /// \param[in] update + /// A callback that will be triggered when the phase has a significant + /// update in its status. The callback will be given a snapshot of the + /// active phase. This snapshot can be safely read in parallel to the phase + /// execution. + /// + /// \param[in] finished + /// A callback that will be triggered when the phase has finished. + /// + /// \return an active, running instance of the described phase. + template + using Activate = + std::function< + ActivePtr( + const Description& description, + std::function update, + std::function finished) + >; + + /// Add a callback to convert from a PhaseDescription into an active phase. + /// + /// \tparam Description + /// A class that implements the sequence::PhaseDescription interface + template + void add_activator(Activate activator); + + /// Activate a phase based on a description of the phase. + /// + /// \param[in] description + /// The description of the phase + /// + /// \param[in] update + /// A callback that will be triggered when the phase has a notable update. + /// The callback will be given a snapshot of the active phase. + /// + /// \param[in] finished + /// A callback that will be triggered when the task has finished. A + /// completed + /// + /// \return an active, running instance of the described task. + ActivePtr activate( + const Description& description, + std::function update, + std::function finished) const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Phase::Description +{ +public: + + /// An abstract interface for computing estimates and invariants related to + /// this phase. + class Model; + + /// Generate a Model for this phase based on its description, parameters, and + /// the invariants of its initial state. + virtual std::shared_ptr make_model( + State invariant_initial_state, + const Parameters& parameters) const = 0; + + /// Get the human-friendly description of this phase as pending + virtual const execute::Phase::Pending& pending() const = 0; + + // Virtual destructor + virtual ~Description() = default; +}; + +//============================================================================== +class Phase::Description::Model +{ +public: + + /// Estimate the state that the robot will have when the phase is finished. + /// + /// \param[in] initial_state + /// The expected initial state when the phase begins + /// + /// \param[in] constraints + /// Constraints on the robot during the phase + /// + /// \return an estimated state for the robot when the phase is finished. + virtual std::optional estimate_finish( + State initial_state, + const Constraints& constraints) const = 0; + + /// Estimate the invariant component of the request's duration. + virtual rmf_traffic::Duration invariant_duration() const = 0; + + /// Get the components of the finish state that this phase is guaranteed to + /// result in once the phase is finished. + virtual State invariant_finish_state() const = 0; + + // Virtual destructor + virtual ~Model() = default; +}; + +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASE_HPP diff --git a/rmf_task/include/rmf_task/sequence/PhaseDescription.hpp b/rmf_task/include/rmf_task/sequence/PhaseDescription.hpp deleted file mode 100644 index 80032858..00000000 --- a/rmf_task/include/rmf_task/sequence/PhaseDescription.hpp +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASEDESCRIPTION_HPP -#define RMF_TASK__SEQUENCE__PHASEDESCRIPTION_HPP - -#include -#include - -#include -#include -#include -#include - -namespace rmf_task { -namespace sequence { - -//============================================================================== -class PhaseDescription -{ -public: - - /// An abstract interface for computing estimates and invariants related to - /// this phase. - class Model - { - public: - - /// Estimate the state that the robot will have when the phase is finished. - /// - /// \param[in] initial_state - /// - virtual std::optional estimate_finish( - State initial_state, - const Constraints& constraints) const = 0; - - /// Estimate the invariant component of the request's duration. - virtual rmf_traffic::Duration invariant_duration() const = 0; - - /// Get the components of the finish state that this phase is guaranteed to - /// result in once the phase is finished. - virtual State invariant_finish_state() const = 0; - - // Virtual destructor - virtual ~Model() = default; - }; - - /// Generate a Model for this phase based on its description, parameters, and - /// the invariants of its initial state. - virtual std::shared_ptr make_model( - State invariant_initial_state, - const Parameters& parameters) const = 0; - - /// Get the human-friendly description of this phase as pending - virtual const execute::PendingPhase& pending() const = 0; - - // Virtual destructor - virtual ~PhaseDescription() = default; -}; - -using ConstPhaseDescriptionPtr = std::shared_ptr; - -} // namespace sequence -} // namespace rmf_task - -#endif // RMF_TASK__SEQUENCE__PHASEDESCRIPTION_HPP diff --git a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp b/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp deleted file mode 100644 index 7dfb8781..00000000 --- a/rmf_task/include/rmf_task/sequence/PhaseFactory.hpp +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASEFACTORY_HPP -#define RMF_TASK__SEQUENCE__PHASEFACTORY_HPP - -#include -#include -#include - -namespace rmf_task { -namespace sequence { - -//============================================================================== -/// A factory for generating execute::Phase instances from descriptions. -class PhaseFactory -{ -public: - - /// Construct an empty PhaseFactory - PhaseFactory(); - - /// Signature for activating a phase - /// - /// \tparam Description - /// A class that implements the sequence::PhaseDescription interface - /// - /// \param[in] description - /// An immutable reference to the relevant Description instance - /// - /// \param[in] update - /// A callback that will be triggered when the phase has a significant - /// update in its status. - /// - /// \param[in] finished - /// A callback that will be triggered when the phase has finished. - /// - /// \return an active, running instance of the described phase. - template - using Activate = - std::function< - execute::ConstActivePhasePtr( - const Description& description, - std::function update, - std::function finished) - >; - - /// Add a callback to convert from a PhaseDescription into an active phase. - /// - /// \tparam Description - /// A class that implements the sequence::PhaseDescription interface - template - void add_activator(Activate activator); - - /// Activate a Task object based on a phase sequence description. - /// - /// \param[in] description - /// The description of the phase sequence - /// - /// \param[in] update - /// A callback that will be triggered when the task has a significant update - /// - /// \param[in] finished - /// A callback that will be triggered when the task has finished - /// - /// \return an active, running instance of the described task. - std::shared_ptr activate( - const Request& request, - ConstTaskDescriptionPtr description, - std::function update, - std::function finished) const; - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; - - -} // namespace sequence -} // namespace rmf_task - -#endif // RMF_TASK__SEQUENCE__PHASEFACTORY_HPP diff --git a/rmf_task/include/rmf_task/sequence/Task.hpp b/rmf_task/include/rmf_task/sequence/Task.hpp index fc364b7c..83fc3327 100644 --- a/rmf_task/include/rmf_task/sequence/Task.hpp +++ b/rmf_task/include/rmf_task/sequence/Task.hpp @@ -18,36 +18,100 @@ #ifndef RMF_TASK__SEQUENCE__TASK_HPP #define RMF_TASK__SEQUENCE__TASK_HPP +#include #include +#include namespace rmf_task { namespace sequence { //============================================================================== -class Task : public execute::Task +class Task { public: - // Documentation inherited - const std::vector& completed_phases() const final; + // Declaration + class Builder; + + // Declaration + class Active; + using ConstActivePtr = std::shared_ptr; + + // Declaration + class Description; + using ConstDescriptionPtr = std::shared_ptr; + + using Update = std::function; + using PhaseFinished = std::function; + using TaskFinished = std::function; + + /// Activate a phase sequence task + /// + /// \param[in] activator + /// A phase activator + static ConstActivePtr activate( + std::shared_ptr activator, + const Description& description, + Update update, + PhaseFinished phase_finished, + TaskFinished task_finished); + +}; + +//============================================================================== +class Task::Builder +{ +public: + + /// Get the builder ready. + Builder(); + + /// Add a phase to the sequence. + /// + /// \param[in] description + /// A description of the phase + /// + /// \param[in] cancellation_sequence + /// This phase sequence will be run if the task is cancelled during this + /// phase. + void add_phase( + Phase::ConstDescriptionPtr description, + std::vector cancellation_sequence); + + /// Generate a TaskDescription instance from the phases that have been given + /// to the builder. + ConstDescriptionPtr build(); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Task::Active + : public execute::Task, + public std::enable_shared_from_this +{ +public: // Documentation inherited - execute::ConstActivePhasePtr active_phase() const final; + const std::vector& + completed_phases() const final; // Documentation inherited - std::vector pending_phases() const final; + execute::Phase::ConstActivePtr active_phase() const final; // Documentation inherited - std::string id() const final; + std::vector pending_phases() const final; // Documentation inherited - std::string category() const final; + const Request::ConstTagPtr& tag() const final; // Documentation inherited - std::string detail() const final; + const std::string& category() const final; // Documentation inherited - const Request& original_request() const final; + const std::string& detail() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; @@ -66,6 +130,41 @@ class Task : public execute::Task rmf_utils::unique_impl_ptr _pimpl; }; +//============================================================================== +class Task::Description : public Request::Description +{ +public: + class Model; + + // Documentation inherited + std::shared_ptr make_model( + rmf_traffic::Time earliest_start_time, + const Parameters& parameters) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Task::Description::Model : public Request::Model +{ +public: + + // Documentation inherited + std::optional estimate_finish( + const State& initial_state, + const Constraints& task_planning_constraints, + EstimateCache& estimate_cache) const final; + + // Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + } // namespace sequence } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/sequence/TaskDescription.hpp b/rmf_task/include/rmf_task/sequence/TaskDescription.hpp deleted file mode 100644 index 1e9280a9..00000000 --- a/rmf_task/include/rmf_task/sequence/TaskDescription.hpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__TASKDESCRIPTION_HPP -#define RMF_TASK__SEQUENCE__TASKDESCRIPTION_HPP - -#include -#include - -#include - -namespace rmf_task { -namespace sequence { - -//============================================================================== -/// The Request::Description implementation for a phase sequence task. -class TaskDescription : public Request::Description -{ -public: - - class Builder; - class Model; - - // Documentation inherited - std::shared_ptr make_model( - rmf_traffic::Time earliest_start_time, - const Parameters& parameters) const final; - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; - -using ConstTaskDescriptionPtr = std::shared_ptr; - -//============================================================================== -class TaskDescription::Builder -{ -public: - - /// Get the builder ready. - Builder(); - - /// Add a phase to the sequence. - /// - /// \param[in] description - /// A description of the phase - /// - /// \param[in] cancellation_sequence - /// This phase sequence will be run if the task is cancelled during this - /// phase. - void add_phase( - ConstPhaseDescriptionPtr description, - std::vector cancellation_sequence); - - /// Generate a TaskDescription instance from the phases that have been given - /// to the builder. - ConstTaskDescriptionPtr build(); - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; - -//============================================================================== -/// The implementation of a task description model for a phase sequence task. -class TaskDescription::Model : public Request::Model -{ -public: - - // Documentation inherited - std::optional estimate_finish( - const State& initial_state, - const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; - - // Documentation inherited - rmf_traffic::Duration invariant_duration() const final; - - class Implementation; -private: - rmf_utils::unique_impl_ptr _pimpl; -}; - -} // namespace sequence -} // namespace rmf_task - - -#endif // RMF_TASK__SEQUENCE__TASKDESCRIPTION_HPP diff --git a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp index bc5690dc..a9749fa8 100644 --- a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp +++ b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp @@ -35,7 +35,7 @@ auto BinaryPriorityCostCalculator::compute_g_assignment( return rmf_traffic::time::to_seconds( assignment.finish_state().time().value() - - assignment.request()->earliest_start_time()); + - assignment.request()->tag()->earliest_start_time()); } //============================================================================== @@ -140,7 +140,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( const auto& assignments = node.assigned_tasks[i]; for (const auto& a : assignments) { - if (a.assignment.request()->priority() != nullptr) + if (a.assignment.request()->tag()->priority() != nullptr) priority_count[i] += 1; } } @@ -175,7 +175,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( return true; } - auto prev_priority = it->assignment.request()->priority(); + auto prev_priority = it->assignment.request()->tag()->priority(); ++it; for (; it != agent.end(); ++it) { @@ -183,7 +183,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( const rmf_task::requests::ChargeBattery::Description>( it->assignment.request()->description())) continue; - auto curr_priority = it->assignment.request()->priority(); + auto curr_priority = it->assignment.request()->tag()->priority(); if ((prev_priority == nullptr) && (curr_priority != nullptr)) return false; diff --git a/rmf_task/src/rmf_task/Request.cpp b/rmf_task/src/rmf_task/Request.cpp index cf830ecd..67d420e1 100644 --- a/rmf_task/src/rmf_task/Request.cpp +++ b/rmf_task/src/rmf_task/Request.cpp @@ -23,11 +23,8 @@ namespace rmf_task { class Request::Implementation { public: - std::string id; - rmf_traffic::Time earliest_start_time; - rmf_task::ConstPriorityPtr priority; + ConstTagPtr tag; ConstDescriptionPtr description; - bool automatic; }; //============================================================================== @@ -39,44 +36,75 @@ Request::Request( bool automatic) : _pimpl(rmf_utils::make_impl( Implementation { - id, - earliest_start_time, - std::move(priority), - std::move(description), - automatic + std::make_shared( + id, earliest_start_time, std::move(priority), automatic), + std::move(description) + })) +{ + // Do nothing +} + +//============================================================================== +class Request::Tag::Implementation +{ +public: + std::string id; + rmf_traffic::Time earliest_start_time; + rmf_task::ConstPriorityPtr priority; + bool automatic; +}; + +//============================================================================== +Request::Tag::Tag( + std::string id_, + rmf_traffic::Time earliest_start_time_, + ConstPriorityPtr priority_, + bool automatic_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(id_), + earliest_start_time_, + std::move(priority_), + automatic_ })) { // Do nothing } //============================================================================== -const std::string& Request::id() const +const std::string& Request::Tag::id() const { return _pimpl->id; } //============================================================================== -rmf_traffic::Time Request::earliest_start_time() const +rmf_traffic::Time Request::Tag::earliest_start_time() const { return _pimpl->earliest_start_time; } //============================================================================== -ConstPriorityPtr Request::priority() const +ConstPriorityPtr Request::Tag::priority() const { return _pimpl->priority; } //============================================================================== -const Request::ConstDescriptionPtr& Request::description() const +bool Request::Tag::automatic() const { - return _pimpl->description; + return _pimpl->automatic; } //============================================================================== -bool Request::automatic() const +const Request::ConstTagPtr& Request::tag() const { - return _pimpl->automatic; + return _pimpl->tag; +} + +//============================================================================== +const Request::ConstDescriptionPtr& Request::description() const +{ + return _pimpl->description; } } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 29f6b42a..8e93b89f 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -513,7 +513,7 @@ class TaskPlanner::Implementation // If so the cost function for a node will be modified accordingly. for (const auto& request : requests) { - if (request->priority()) + if (request->tag()->priority()) { check_priority = true; break; @@ -722,7 +722,7 @@ class TaskPlanner::Implementation entry.previous_state.time().value()); const auto charge_battery_model = charge_battery->description()->make_model( - charge_battery->earliest_start_time(), + charge_battery->tag()->earliest_start_time(), config.parameters()); auto battery_estimate = charge_battery_model->estimate_finish( entry.previous_state, constraints, *estimate_cache); @@ -801,7 +801,7 @@ class TaskPlanner::Implementation const auto charge_battery_model = charge_battery->description()->make_model( - charge_battery->earliest_start_time(), + charge_battery->tag()->earliest_start_time(), config.parameters()); auto battery_estimate = charge_battery_model->estimate_finish( entry.state, constraints, *estimate_cache); @@ -887,7 +887,7 @@ class TaskPlanner::Implementation auto charge_battery = make_charging_request(state.time().value()); const auto charge_battery_model = charge_battery->description()->make_model( - charge_battery->earliest_start_time(), + charge_battery->tag()->earliest_start_time(), config.parameters()); auto estimate = charge_battery_model->estimate_finish( state, config.constraints(), *estimate_cache); diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp index 37d5f42e..19eda242 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp @@ -201,7 +201,7 @@ std::shared_ptr PendingTask::make( { const auto earliest_start_time = std::max( start_time, - request_->earliest_start_time()); + request_->tag()->earliest_start_time()); const auto model = request_->description()->make_model( earliest_start_time, parameters); diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp index 25e1832f..4ca5bc38 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp @@ -163,7 +163,7 @@ struct Node for (const auto& u : unassigned_tasks) { double earliest_start_time = rmf_traffic::time::to_seconds( - u.second.request->earliest_start_time().time_since_epoch()); + u.second.request->tag()->earliest_start_time().time_since_epoch()); const auto invariant_duration = u.second.model->invariant_duration(); double earliest_finish_time = earliest_start_time diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 17acb527..41531487 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -116,14 +116,14 @@ inline void display_solution( { const auto& s = a.finish_state(); const double request_seconds = - a.request()->earliest_start_time().time_since_epoch().count(); + a.request()->tag()->earliest_start_time().time_since_epoch().count(); const double start_seconds = a.deployment_time().time_since_epoch().count(); const rmf_traffic::Time finish_time = s.time().value(); const double finish_seconds = finish_time.time_since_epoch().count(); std::cout << std::fixed - << " <" << a.request()->id() << ": " << request_seconds - << ", " << start_seconds + << " <" << a.request()->tag()->id() << ": " + << request_seconds << ", " << start_seconds << ", "<< finish_seconds << ", " << 100*s.battery_soc().value() << "%>" << std::endl; } @@ -860,7 +860,7 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the back of the assignment queue - CHECK(optimal_assignments.front().back().request()->id() == "3"); + CHECK(optimal_assignments.front().back().request()->tag()->id() == "3"); THEN("When replanning with high priority for request with task_id:3") { @@ -896,7 +896,7 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the front of the assignment queue - CHECK(new_optimal_assignments.front().front().request()->id() == "3"); + CHECK(new_optimal_assignments.front().front().request()->tag()->id() == "3"); } WHEN("Planning for one robot, three high priority tasks") @@ -962,7 +962,7 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the back of the assignment queue - CHECK(optimal_assignments.front().back().request()->id() == "3"); + CHECK(optimal_assignments.front().back().request()->tag()->id() == "3"); } WHEN("Planning for 1 robot, two high priority and two low priority tasks") @@ -1039,7 +1039,7 @@ SCENARIO("Grid World") const auto& assignments = optimal_assignments.front(); std::unordered_map index_map = {}; for (std::size_t i = 0; i < assignments.size(); ++i) - index_map.insert({assignments[i].request()->id(), i}); + index_map.insert({assignments[i].request()->tag()->id(), i}); CHECK(index_map["1"] < index_map["2"]); CHECK(index_map["1"] < index_map["3"]); CHECK(index_map["4"] < index_map["2"]); @@ -1121,7 +1121,7 @@ SCENARIO("Grid World") const auto& assignments = optimal_assignments.front(); std::unordered_map index_map = {}; for (std::size_t i = 0; i < assignments.size(); ++i) - index_map.insert({assignments[i].request()->id(), i}); + index_map.insert({assignments[i].request()->tag()->id(), i}); CHECK(index_map["1"] < index_map["2"]); CHECK(index_map["1"] < index_map["3"]); CHECK(index_map["1"] < index_map["4"]); @@ -1202,8 +1202,8 @@ SCENARIO("Grid World") REQUIRE(optimal_assignments.size() == 2); const auto& agent_0_assignments = optimal_assignments[0]; const auto& agent_1_assignments = optimal_assignments[1]; - CHECK(agent_0_assignments.front().request()->id() == "2"); - CHECK(agent_1_assignments.front().request()->id() == "1"); + CHECK(agent_0_assignments.front().request()->tag()->id() == "2"); + CHECK(agent_1_assignments.front().request()->tag()->id() == "1"); THEN("When task 3 & 4 are assigned high priority") { @@ -1267,8 +1267,8 @@ SCENARIO("Grid World") REQUIRE(optimal_assignments.size() == 2); const auto& agent_0_assignments = optimal_assignments[0]; const auto& agent_1_assignments = optimal_assignments[1]; - CHECK(agent_0_assignments.front().request()->id() == "4"); - CHECK(agent_1_assignments.front().request()->id() == "3"); + CHECK(agent_0_assignments.front().request()->tag()->id() == "4"); + CHECK(agent_1_assignments.front().request()->tag()->id() == "3"); } } @@ -1348,7 +1348,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { if (!agent.empty()) - first_assignments.push_back(agent.front().request()->id()); + first_assignments.push_back(agent.front().request()->tag()->id()); } std::size_t id_count = 0; for (const auto& id : first_assignments) @@ -1422,7 +1422,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { if (!agent.empty()) - first_assignments.push_back(agent.front().request()->id()); + first_assignments.push_back(agent.front().request()->tag()->id()); } std::size_t id_count = 0; for (const auto& id : first_assignments) @@ -1859,7 +1859,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { const auto last_assignment = agent.back(); - CHECK_FALSE(last_assignment.request()->automatic()); + CHECK_FALSE(last_assignment.request()->tag()->automatic()); const auto& state = last_assignment.finish_state(); CHECK_FALSE(state.waypoint() == state.dedicated_charging_waypoint()); } @@ -1895,7 +1895,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { const auto last_assignment = agent.back(); - CHECK(last_assignment.request()->automatic()); + CHECK(last_assignment.request()->tag()->automatic()); const auto& state = last_assignment.finish_state(); CHECK(state.waypoint() == state.dedicated_charging_waypoint()); } From 873693a942d9f2ab435c8eafa957201e96e8ce27 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 6 Sep 2021 19:48:11 +0800 Subject: [PATCH 09/85] Satisfy uncrustify Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/sequence/Task.hpp | 8 +------- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 3 ++- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/rmf_task/include/rmf_task/sequence/Task.hpp b/rmf_task/include/rmf_task/sequence/Task.hpp index 83fc3327..087df748 100644 --- a/rmf_task/include/rmf_task/sequence/Task.hpp +++ b/rmf_task/include/rmf_task/sequence/Task.hpp @@ -90,7 +90,7 @@ class Task::Builder //============================================================================== class Task::Active : public execute::Task, - public std::enable_shared_from_this + public std::enable_shared_from_this { public: @@ -107,12 +107,6 @@ class Task::Active // Documentation inherited const Request::ConstTagPtr& tag() const final; - // Documentation inherited - const std::string& category() const final; - - // Documentation inherited - const std::string& detail() const final; - // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 41531487..1798ee12 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -896,7 +896,8 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the front of the assignment queue - CHECK(new_optimal_assignments.front().front().request()->tag()->id() == "3"); + CHECK(new_optimal_assignments.front().front().request()->tag()->id() == + "3"); } WHEN("Planning for one robot, three high priority tasks") From 67f9398190c48f37ac40ede600ba2406ecec8466 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 6 Sep 2021 23:27:43 +0800 Subject: [PATCH 10/85] Beginning to implement GoToPlace::Description Signed-off-by: Michael X. Grey --- rmf_task/CMakeLists.txt | 2 + rmf_task/include/rmf_task/execute/Phase.hpp | 15 ++ rmf_task/include/rmf_task/sequence/Phase.hpp | 24 +- .../rmf_task/sequence/phases/GoToPlace.hpp | 73 ++++++ .../rmf_task/sequence/phases/GoToPlace.cpp | 209 ++++++++++++++++++ 5 files changed, 316 insertions(+), 7 deletions(-) create mode 100644 rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp create mode 100644 rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index a883fbc4..5705c3d1 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -31,6 +31,8 @@ find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Tasks library file(GLOB lib_srcs "src/rmf_task/agv/*.cpp" + "src/rmf_task/sequence/*.cpp" + "src/rmf_task/sequence/phases/*.cpp" "src/rmf_task/requests/*.cpp" "src/rmf_task/requests/factory/*.cpp" "src/rmf_task/*.cpp" diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/execute/Phase.hpp index 443dd3d5..98e8e30e 100644 --- a/rmf_task/include/rmf_task/execute/Phase.hpp +++ b/rmf_task/include/rmf_task/execute/Phase.hpp @@ -58,6 +58,21 @@ class Phase::Tag { public: + /// Constructor + /// + /// \param[in] name_ + /// Name of the phase + /// + /// \param[in] detail_ + /// Details about the phase + /// + /// \param[in] estimate_ + /// The original (ideal) estimate of how long the phase will last + Tag( + std::string name_, + std::string detail_, + rmf_traffic::Duration estimate_); + /// Name of the phase const std::string& name() const; diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task/include/rmf_task/sequence/Phase.hpp index 05a550f5..d7d41995 100644 --- a/rmf_task/include/rmf_task/sequence/Phase.hpp +++ b/rmf_task/include/rmf_task/sequence/Phase.hpp @@ -43,6 +43,9 @@ class Phase class Description; using ConstDescriptionPtr = std::shared_ptr; + + class Model; + using ConstModelPtr = std::shared_ptr; }; //============================================================================== @@ -161,25 +164,32 @@ class Phase::Description { public: - /// An abstract interface for computing estimates and invariants related to - /// this phase. - class Model; - /// Generate a Model for this phase based on its description, parameters, and /// the invariants of its initial state. - virtual std::shared_ptr make_model( + /// + /// \param[in] invariant_initial_state + /// A partial state that represents the state components which will + /// definitely be true when this phase begins. + /// + /// \param[in] parameters + /// The parameters for the robot. + /// + /// \return a model based on the given start state and parameters. + virtual ConstModelPtr make_model( State invariant_initial_state, const Parameters& parameters) const = 0; /// Get the human-friendly description of this phase as pending - virtual const execute::Phase::Pending& pending() const = 0; + virtual execute::Phase::ConstTagPtr make_tag( + const State& initial_state, + const Parameters& parameters) const = 0; // Virtual destructor virtual ~Description() = default; }; //============================================================================== -class Phase::Description::Model +class Phase::Model { public: diff --git a/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp b/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp new file mode 100644 index 00000000..53614e70 --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP +#define RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP + +#include + +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +class GoToPlace +{ +public: + using Goal = rmf_traffic::agv::Plan::Goal; + + class Description; + class Model; +}; + +//============================================================================== +class GoToPlace::Description : public Phase::Description +{ +public: + + /// Make a GoToPlace description using a goal. + static std::shared_ptr make(Goal goal); + + // Documentation inherited + Phase::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + execute::Phase::ConstTagPtr make_tag( + const State& initial_state, + const Parameters& parameters) const final; + + /// Get the current goal for this description. + const Goal& goal() const; + + /// Set the current goal for this description. + Description& goal(Goal new_goal); + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace phases +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP diff --git a/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp b/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp new file mode 100644 index 00000000..7481cee1 --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { +namespace sequence { +namespace phases { + +namespace { +//============================================================================== +std::optional estimate_duration( + const std::shared_ptr& planner, + const State& initial_state, + const GoToPlace::Goal& goal) +{ + const auto result = + planner->setup(initial_state.project_plan_start().value(), goal); + + // TODO(MXG): Perhaps print errors/warnings about these failure conditions + if (result.disconnected()) + return std::nullopt; + + if (!result.ideal_cost().has_value()) + return std::nullopt; + + return rmf_traffic::time::from_seconds(*result.ideal_cost()); +} +} // anonymous namespace + +//============================================================================== +class GoToPlace::Model : public Phase::Model +{ +public: + + static Phase::ConstModelPtr make( + State invariant_initial_state, + const Parameters& parameters, + Goal goal); + + // Documentation inherited + std::optional estimate_finish( + State initial_state, + const Constraints& constraints) const final; + + // Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + // Documentation inherited + State invariant_finish_state() const final; + +private: + + Model( + State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + const Parameters& parameters, + Goal goal); + + State _invariant_finish_state; + rmf_traffic::Duration _invariant_duration; + Parameters _parameters; + Goal _goal; +}; + +//============================================================================== +Phase::ConstModelPtr GoToPlace::Model::make( + State invariant_initial_state, + const Parameters& parameters, + Goal goal) +{ + auto invariant_finish_state = invariant_initial_state; + invariant_finish_state.waypoint(goal.waypoint()); + + if (goal.orientation()) + invariant_finish_state.orientation(*goal.orientation()); + else + invariant_finish_state.erase(); + + auto invariant_duration = rmf_traffic::Duration(0); + if (invariant_initial_state.waypoint().has_value()) + { + const auto invariant_duration_opt = estimate_duration( + parameters.planner(), + invariant_initial_state, + goal); + + if (!invariant_duration_opt.has_value()) + return nullptr; + + invariant_duration = *invariant_duration_opt; + } + + return std::shared_ptr( + new Model( + std::move(invariant_finish_state), + invariant_duration, + parameters, + std::move(goal))); +} + +//============================================================================== +std::optional GoToPlace::Model::estimate_finish( + State initial_state, + const Constraints& constraints) const +{ + auto finish = initial_state; + finish.waypoint(_goal.waypoint()); + +} + +//============================================================================== +class GoToPlace::Description::Implementation +{ +public: + + rmf_traffic::agv::Plan::Goal goal; + +}; + +//============================================================================== +auto GoToPlace::Description::make(Goal goal) +-> std::shared_ptr +{ + auto desc = std::shared_ptr(new Description); + desc->_pimpl = rmf_utils::make_impl( + Implementation{ + std::move(goal) + }); + + return desc; +} + +//============================================================================== +Phase::ConstModelPtr GoToPlace::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return Model::make( + std::move(invariant_initial_state), + parameters, + _pimpl->goal); +} + +//============================================================================== +execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( + const State& initial_state, + const Parameters& parameters) const +{ + const auto start_wp_opt = initial_state.waypoint(); + if (!start_wp_opt) + return nullptr; + + const auto start_wp = *start_wp_opt; + + const auto& graph = parameters.planner()->get_configuration().graph(); + if (graph.num_waypoints() <= start_wp) + return nullptr; + + auto get_wp_name = [&graph](std::size_t index) -> std::string + { + if (const auto* name = graph.get_waypoint(index).name()) + return *name; + + return "#" + std::to_string(index); + }; + + const auto start_name = get_wp_name(start_wp); + + if (graph.num_waypoints() <= _pimpl->goal.waypoint()) + return nullptr; + + const auto goal_name = get_wp_name(_pimpl->goal.waypoint()); + + const auto estimate = estimate_duration( + parameters.planner(), initial_state, _pimpl->goal); + + if (!estimate.has_value()) + return nullptr; + + return std::make_shared( + "Go to [" + goal_name + "]", + "Moving the robot from [" + start_name + "] to [" + goal_name + "]", + *estimate); +} + +//============================================================================== +GoToPlace::Description::Description() +{ + // Do nothing +} + +} // namespace phases +} // namespace sequence +} // namespace rmf_task From 68f2fda7dbbf2116d7a54bab74780a4c0e83d4b5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 7 Sep 2021 15:37:16 +0800 Subject: [PATCH 11/85] Redesigning travel estimation cache Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Estimate.hpp | 46 +++--- rmf_task/include/rmf_task/Request.hpp | 2 +- rmf_task/include/rmf_task/TaskPlanner.hpp | 3 - rmf_task/include/rmf_task/detail/Backup.hpp | 66 +++++++++ .../include/rmf_task/execute/Condition.hpp | 9 ++ rmf_task/include/rmf_task/execute/Phase.hpp | 10 ++ rmf_task/include/rmf_task/execute/Task.hpp | 78 +++++++--- .../{TaskFactory.hpp => TaskActivator.hpp} | 59 ++++++-- rmf_task/include/rmf_task/sequence/Phase.hpp | 60 +++++++- rmf_task/src/rmf_task/Estimate.cpp | 136 ++++++++++++++---- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 33 ++--- .../rmf_task/agv/internal_task_planning.cpp | 13 +- .../rmf_task/agv/internal_task_planning.hpp | 4 +- .../src/rmf_task/requests/ChargeBattery.cpp | 72 +++------- rmf_task/src/rmf_task/requests/Clean.cpp | 4 +- 15 files changed, 420 insertions(+), 175 deletions(-) create mode 100644 rmf_task/include/rmf_task/detail/Backup.hpp rename rmf_task/include/rmf_task/execute/{TaskFactory.hpp => TaskActivator.hpp} (57%) diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index 42174de2..a5f39a87 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -22,7 +22,9 @@ #include #include +#include #include +#include #include namespace rmf_task { @@ -64,37 +66,45 @@ class Estimate }; //============================================================================== -/// A class to cache the computed estimates between pairs of waypoints -class EstimateCache +/// A class to estimate the cost of travelling between any two points in a +/// navigation graph. Results will be memoized for efficiency. +class TravelEstimator { public: - /// Constructs an EstimateCache + + /// Constructor /// - /// \param[in] N - /// The maximum number of waypoints in the navigation graph - EstimateCache(std::size_t N); + /// \param[in] parameters + /// The parameters for the robot + TravelEstimator(const Parameters& parameters); - /// Struct containing the estimated duration and charge required to travel between - /// a waypoint pair. - struct CacheElement + class Result { - rmf_traffic::Duration duration; - double dsoc; // Positive if charge is consumed - }; + public: - /// Returns the saved estimate values for the path between the supplied waypoints, - /// if present. - std::optional get(std::pair waypoints) const; + rmf_traffic::Duration duration() const; - /// Saves the estimated duration and change in charge between the supplied waypoints. - void set(std::pair waypoints, - rmf_traffic::Duration duration, double dsoc); + double change_in_state_of_charge() const; + + class Implementation; + private: + Result(); + rmf_utils::impl_ptr _pimpl; + }; + + /// Estimate the cost of travelling + std::optional estimate( + const rmf_traffic::agv::Plan::Start& start, + const rmf_traffic::agv::Plan::Goal& goal) const; class Implementation; private: rmf_utils::unique_impl_ptr _pimpl; }; +//============================================================================== +using ConstTravelEstimatorPtr = std::shared_ptr; + } // namespace rmf_task #endif // RMF_TASK__ESTIMATE_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 38972e03..b9014a51 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -90,7 +90,7 @@ class Request virtual std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const = 0; + const TravelEstimator& travel_estimator) const = 0; /// Estimate the invariant component of the request's duration virtual rmf_traffic::Duration invariant_duration() const = 0; diff --git a/rmf_task/include/rmf_task/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp index 8ab5f63a..08d6ab05 100644 --- a/rmf_task/include/rmf_task/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/TaskPlanner.hpp @@ -247,9 +247,6 @@ class TaskPlanner /// Compute the cost of a set of assignments double compute_cost(const Assignments& assignments) const; - /// Retrieve the task planner cache - const std::shared_ptr& estimate_cache() const; - class Implementation; private: diff --git a/rmf_task/include/rmf_task/detail/Backup.hpp b/rmf_task/include/rmf_task/detail/Backup.hpp new file mode 100644 index 00000000..7dd0a1d8 --- /dev/null +++ b/rmf_task/include/rmf_task/detail/Backup.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__DETAIL__BACKUP_HPP +#define RMF_TASK__DETAIL__BACKUP_HPP + +#include + +#include +#include + +namespace rmf_task { +namespace detail { + +//============================================================================== +class Backup +{ +public: + /// Back a Backup state + /// + /// \param[in] seq + /// Sequence number. The Backup from this phase with the highest sequence + /// number will be held onto until a Backup with a higher sequence number is + /// issued. + /// + /// \param[in] state + /// A serialization of the phase's state. This will be used by TaskActivator + /// when restoring a Task. + static Backup make(uint64_t seq, std::string state); + + /// Get the sequence number for this backup. + uint64_t sequence() const; + + /// Set the sequence number for this backup. + Backup& sequence(uint64_t seq); + + /// Get the serialized state for this backup. + const std::string& state() const; + + /// Set the serialized state for this backup. + Backup& state(std::string new_state); + + class Implementation; +private: + Backup(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace detail +} // namespace rmf_task + +#endif // RMF_TASK__DETAIL__BACKUP_HPP diff --git a/rmf_task/include/rmf_task/execute/Condition.hpp b/rmf_task/include/rmf_task/execute/Condition.hpp index 60ea4824..9e7cc546 100644 --- a/rmf_task/include/rmf_task/execute/Condition.hpp +++ b/rmf_task/include/rmf_task/execute/Condition.hpp @@ -66,6 +66,15 @@ class Condition /// deal with. Manual intervention is needed to get the task back on track. Error, + /// An operator has instructed this condition to be skipped + Skipped, + + /// An operator has instructed this condition to be canceled + Canceled, + + /// An operator has instructed this condition to be killed + Killed, + /// The condition cannot ever be satisfied, even with manual intervention. /// This may mean that the Task cannot be completed if it does not have /// an automated way to recover from this failure state. diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/execute/Phase.hpp index 98e8e30e..d51ac518 100644 --- a/rmf_task/include/rmf_task/execute/Phase.hpp +++ b/rmf_task/include/rmf_task/execute/Phase.hpp @@ -60,6 +60,9 @@ class Phase::Tag /// Constructor /// + /// \param[in] id_ + /// ID of the phase. This phase ID must be unique within its Task instance. + /// /// \param[in] name_ /// Name of the phase /// @@ -69,10 +72,14 @@ class Phase::Tag /// \param[in] estimate_ /// The original (ideal) estimate of how long the phase will last Tag( + uint64_t id_, std::string name_, std::string detail_, rmf_traffic::Duration estimate_); + /// Unique ID of the phase + uint64_t id() const; + /// Name of the phase const std::string& name() const; @@ -158,6 +165,9 @@ class Phase::Pending /// Tag of the phase const ConstTagPtr& tag() const; + /// Check if this phase is set to be skipped + bool will_be_skipped() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/execute/Task.hpp index 6fe9466f..c95c365a 100644 --- a/rmf_task/include/rmf_task/execute/Task.hpp +++ b/rmf_task/include/rmf_task/execute/Task.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__EXECUTE__TASK_HPP #include +#include #include #include @@ -34,27 +35,17 @@ class Task { public: - /// Basic static information about the task. - class Tag - { - public: + // Declarations + class Tag; - /// The original request that this Task is carrying out - const Request::ConstTagPtr& request() const; - - /// The category of this Task. - const std::string& category() const; - - /// Details about this Task. - const std::string& detail() const; - - /// The original finish estimate of this Task. - rmf_traffic::Time original_finish_estimate() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; + /// Backup data for the task. The state of the task is represented by a + /// string. The meaning and format of the string is up to the Task + /// implementation to decide. + /// + /// Each Backup is tagged with a sequence number. As the Task makes progress, + /// it can issue new Backups with higher sequence numbers. Only the Backup + /// with the highest sequence number will be kept. + class Backup : public detail::Backup {}; /// Descriptions of the phases that have been completed virtual const std::vector& @@ -72,6 +63,9 @@ class Task /// Estimate the overall finishing time of the task virtual rmf_traffic::Time estimate_finish_time() const = 0; + /// Get a backup for this Task + virtual Backup backup() const = 0; + /// The Resume class keeps track of when the Task is allowed to Resume. /// You can either call the Resume object's operator() or let the object /// expire to tell the Task that it may resume. @@ -122,6 +116,27 @@ class Task /// calling kill() will have no effect. virtual void kill() = 0; + /// Skip a specific phase within the task. This can be issued by operators if + /// manual intervention is needed to unblock a task. + /// + /// If a pending phase is specified, that phase will be skipped when the Task + /// reaches it. + /// + /// \param[in] phase_id + /// The ID of the phase that should be skipped. + /// + /// \param[in] value + /// True if the phase should be skipped, false otherwise. + virtual void skip(uint64_t phase_id, bool value=true) = 0; + + /// Rewind the Task to a specific phase. This can be issued by operators if + /// a phase did not actually go as intended and needs to be repeated. + /// + /// It is possible that the Task will rewind further back than the specified + /// phase_id if the specified phase depends on an earlier one. This is up to + /// the discretion of the Task implementation. + virtual void rewind(uint64_t phase_id) = 0; + // Virtual destructor virtual ~Task() = default; @@ -137,6 +152,29 @@ class Task using ConstTaskPtr = std::shared_ptr; +//============================================================================== +/// Basic static information about the task. +class Task::Tag +{ +public: + + /// The original request that this Task is carrying out + const Request::ConstTagPtr& request() const; + + /// The category of this Task. + const std::string& category() const; + + /// Details about this Task. + const std::string& detail() const; + + /// The original finish estimate of this Task. + rmf_traffic::Time original_finish_estimate() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + } // namespace execute } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/execute/TaskFactory.hpp b/rmf_task/include/rmf_task/execute/TaskActivator.hpp similarity index 57% rename from rmf_task/include/rmf_task/execute/TaskFactory.hpp rename to rmf_task/include/rmf_task/execute/TaskActivator.hpp index b0a25c27..bd68fe09 100644 --- a/rmf_task/include/rmf_task/execute/TaskFactory.hpp +++ b/rmf_task/include/rmf_task/execute/TaskActivator.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__TASKFACTORY_HPP -#define RMF_TASK__TASKFACTORY_HPP +#ifndef RMF_TASK__TASKACTIVATOR_HPP +#define RMF_TASK__TASKACTIVATOR_HPP #include #include @@ -27,12 +27,12 @@ namespace execute { //============================================================================== /// A factory for generating Task instances from requests. -class TaskFactory +class TaskActivator { public: /// Construct an empty TaskFactory - TaskFactory(); + TaskActivator(); /// Signature for activating a task /// @@ -45,6 +45,11 @@ class TaskFactory /// \param[in] description /// The down-casted description of the task /// + /// \param[in] backup_state + /// The serialized backup state of the Task, if the Task is being restored + /// from a crash or disconnection. If the Task is not being restored, a + /// std::nullopt will be passed in here. + /// /// \param[in] update /// A callback that will be triggered when the task has a significant /// update in its status. @@ -59,14 +64,19 @@ class TaskFactory execute::ConstTaskPtr( const Request::ConstTagPtr& request, const Description& description, - std::function update, - std::function finished) + std::optional backup_state, + std::function update, + std::function phase_finished, + std::function task_finished) >; /// Add a callback to convert from a Request into an active Task. /// /// \tparam Description /// A class that implements the Request::Description interface + /// + /// \param[in] activator + /// A callback that activates a Task matching the Description template void add_activator(Activate activator); @@ -78,14 +88,43 @@ class TaskFactory /// \param[in] update /// A callback that will be triggered when the task has a significant update /// - /// \param[in] finished + /// \param[in] phase_finished + /// A callback that will be triggered whenever a task phase is finished + /// + /// \param[in] task_finished /// A callback that will be triggered when the task has finished /// /// \return an active, running instance of the requested task. std::shared_ptr activate( const Request& request, - std::function update, - std::function finished); + std::function update, + std::function phase_finished, + std::function task_finished); + + /// Restore a Task that crashed or disconnected. + /// + /// \param[in] request + /// The task request + /// + /// \param[in] backup_state + /// The serialized backup state of the Task + /// + /// \param[in] update + /// A callback that will be triggered when the task has a significant update + /// + /// \param[in] phase_finished + /// A callback that will be triggered whenever a task phase is finished + /// + /// \param[in] task_finished + /// A callback that will be triggered when the task has finished + /// + /// \return an active, running instance of the requested task. + std::shared_ptr restore( + const Request& request, + std::string backup_state, + std::function update, + std::function phase_finished, + std::function task_finished); class Implementation; private: @@ -95,4 +134,4 @@ class TaskFactory } // namespace execute } // namespace rmf_task -#endif // RMF_TASK__TASKFACTORY_HPP +#endif // RMF_TASK__TASKACTIVATOR_HPP diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task/include/rmf_task/sequence/Phase.hpp index d7d41995..51a47c31 100644 --- a/rmf_task/include/rmf_task/sequence/Phase.hpp +++ b/rmf_task/include/rmf_task/sequence/Phase.hpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace rmf_task { namespace sequence { @@ -54,6 +55,18 @@ class Phase::Active : public execute::Phase::Active { public: + /// Backup data for the Phase. The state of the phase is represented by a + /// string. The meaning and format of the string is up to the phase + /// implementation to decide. + /// + /// Each Backup is tagged with a sequence number. As the Phase makes progress, + /// it can issue new Backups with higher sequence numbers. Only the Backup + /// with the highest sequence number will be kept. + class Backup : public detail::Backup {}; + + /// Get a backup for this Phase + virtual Backup backup() const = 0; + /// The Resume class keeps track of when the phase is allowed to Resume. /// You can either call the Resume object's operator() or let the object /// expire to tell the phase that it may resume. @@ -109,6 +122,11 @@ class Phase::Activator /// \param[in] description /// An immutable reference to the relevant Description instance /// + /// \param[in] backup_state + /// The serialized backup state of the Phase, if the Phase is being restored + /// from a crash or disconnection. If the Phase is not being restored, a + /// std::nullopt will be passed in here. + /// /// \param[in] update /// A callback that will be triggered when the phase has a significant /// update in its status. The callback will be given a snapshot of the @@ -124,8 +142,9 @@ class Phase::Activator std::function< ActivePtr( const Description& description, + std::optional backup_state, std::function update, - std::function finished) + std::function finished) >; /// Add a callback to convert from a PhaseDescription into an active phase. @@ -145,14 +164,37 @@ class Phase::Activator /// The callback will be given a snapshot of the active phase. /// /// \param[in] finished - /// A callback that will be triggered when the task has finished. A - /// completed + /// A callback that will be triggered when the task has finished. The + /// callback will be given a copy of the completed phase. /// - /// \return an active, running instance of the described task. + /// \return an active, running instance of the described phase. ActivePtr activate( const Description& description, std::function update, - std::function finished) const; + std::function finished) const; + + /// Restore a phase based on a description of the phase and its backup state. + /// + /// \param[in] description + /// The description of the phase + /// + /// \param[in] backup_state + /// The serialized backup state of the phase + /// + /// \param[in] update + /// A callback that will triggered when the phase has a notable update. + /// The callback will be given a snapshot of the active phase. + /// + /// \param[in] finished + /// A callback that will be triggered when the task has finished. The + /// callback will be given a copy of the completed phase. + /// + /// \return an active, running instance of the described phase. + ActivePtr restore( + const Description& description, + const std::string& backup_state, + std::function update, + std::function finished) const; class Implementation; private: @@ -179,7 +221,13 @@ class Phase::Description State invariant_initial_state, const Parameters& parameters) const = 0; - /// Get the human-friendly description of this phase as pending + /// Get the human-friendly information about this phase + /// + /// \param[in] initial_state + /// The expected initial state when the phase begins + /// + /// \param[in] constraints + /// Constraints on the robot during the phase virtual execute::Phase::ConstTagPtr make_tag( const State& initial_state, const Parameters& parameters) const = 0; diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 0f643623..29e52fb9 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -70,16 +70,105 @@ Estimate& Estimate::wait_until(rmf_traffic::Time new_wait_until) } //============================================================================== -class EstimateCache::Implementation +class TravelEstimator::Result::Implementation { public: - Implementation(std::size_t N) - : _cache(N, PairHash(N)) + static Result make( + rmf_traffic::Duration duration_, + double change_in_state_of_charge_) { + Result output; + output._pimpl = rmf_utils::make_impl( + Implementation{duration_, change_in_state_of_charge_}); + return output; } + rmf_traffic::Duration duration; + double change_in_state_of_charge; +}; + +//============================================================================== +class TravelEstimator::Implementation +{ +public: + + Implementation(const Parameters& parameters) + : planner(parameters.planner()), + motion_sink(parameters.motion_sink()), + ambient_sink(parameters.ambient_sink()), + cache(make_cache(planner)) + { + // Do nothing + } + + std::optional estimate( + const rmf_traffic::agv::Plan::Start& start, + const rmf_traffic::agv::Plan::Goal& goal) const + { + std::pair insertion; + { + std::unique_lock lock(mutex, std::defer_lock); + while (!lock.try_lock()) {} + + Key wps{start.waypoint(), goal.waypoint()}; + insertion = cache.insert(std::make_pair(wps, Value())); + } + + if (!insertion.second) + return insertion.first->second; + + auto result = calculate_result(start, goal); + { + std::unique_lock lock(mutex, std::defer_lock); + while (!lock.try_lock()) {} + insertion.first->second = result; + } + + return result; + } + + std::optional calculate_result( + const rmf_traffic::agv::Plan::Start& start, + const rmf_traffic::agv::Plan::Goal& goal) const + { + const auto plan = planner->plan(start, goal); + if (!plan.success()) + return std::nullopt; + + // We assume we can always compute a plan + const auto itinerary_start_time = start.time(); + double battery_drain = 0.0; + for (const auto& itinerary : plan->get_itinerary()) + { + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + // Compute battery drain + const auto dSOC_motion = + motion_sink->compute_change_in_charge(trajectory); + + const auto dSOC_device = ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(itinerary_duration)); + + battery_drain += dSOC_motion + dSOC_device; + } + + const auto duration = + plan->get_itinerary().back().trajectory().back().time() + - itinerary_start_time; + + return Result::Implementation::make(duration, battery_drain); + } + +private: + std::shared_ptr planner; + rmf_battery::ConstMotionPowerSinkPtr motion_sink; + rmf_battery::ConstDevicePowerSinkPtr ambient_sink; + struct PairHash { PairHash(std::size_t N) @@ -95,38 +184,27 @@ class EstimateCache::Implementation std::size_t _shift; }; - using Cache = std::unordered_map, - CacheElement, PairHash>; + using Key = std::pair; + using Value = std::optional; + using Cache = std::unordered_map, PairHash>; + mutable Cache cache; - Cache _cache; - mutable std::mutex _mutex; -}; - -//============================================================================== -EstimateCache::EstimateCache(std::size_t N) -: _pimpl(rmf_utils::make_unique_impl(N)) -{ -} - -//============================================================================== -std::optional EstimateCache::get( - std::pair waypoints) const -{ - std::lock_guard guard(_pimpl->_mutex); - auto it = _pimpl->_cache.find(waypoints); - if (it != _pimpl->_cache.end()) + static Cache make_cache( + const std::shared_ptr& planner) { - return it->second; + const auto N = planner->get_configuration().graph().num_waypoints(); + return Cache(N, PairHash(N)); } - return std::nullopt; -} + + mutable std::mutex mutex; +}; //============================================================================== -void EstimateCache::set(std::pair waypoints, - rmf_traffic::Duration duration, double dsoc) +auto TravelEstimator::estimate( + const rmf_traffic::agv::Plan::Start& start, + const rmf_traffic::agv::Plan::Goal& goal) const -> std::optional { - std::lock_guard guard(_pimpl->_mutex); - _pimpl->_cache[waypoints] = CacheElement{duration, dsoc}; + return _pimpl->estimate(start, goal); } } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 8e93b89f..d3c7af23 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -372,7 +372,7 @@ class TaskPlanner::Implementation Configuration config; Options default_options; - std::shared_ptr estimate_cache; + ConstTravelEstimatorPtr travel_estimator; bool check_priority = false; ConstCostCalculatorPtr cost_calculator = nullptr; @@ -442,7 +442,7 @@ class TaskPlanner::Implementation state.time().value(), config.parameters()); auto estimate = model->estimate_finish( - state, config.constraints(), *estimate_cache); + state, config.constraints(), *travel_estimator); if (estimate.has_value()) { agent.push_back( @@ -465,7 +465,7 @@ class TaskPlanner::Implementation config.parameters()); const auto charge_battery_estimate = charge_battery_model->estimate_finish( - state, config.constraints(), *estimate_cache); + state, config.constraints(), *travel_estimator); if (charge_battery_estimate.has_value()) { model = request->description()->make_model( @@ -474,7 +474,7 @@ class TaskPlanner::Implementation estimate = model->estimate_finish( charge_battery_estimate.value().finish_state(), config.constraints(), - *estimate_cache); + *travel_estimator); if (estimate.has_value()) { // Append the ChargeBattery and finishing request @@ -623,7 +623,7 @@ class TaskPlanner::Implementation config.constraints(), config.parameters(), request, - *estimate_cache, + *travel_estimator, error); if (!pending_task) @@ -725,7 +725,7 @@ class TaskPlanner::Implementation charge_battery->tag()->earliest_start_time(), config.parameters()); auto battery_estimate = charge_battery_model->estimate_finish( - entry.previous_state, constraints, *estimate_cache); + entry.previous_state, constraints, *travel_estimator); if (battery_estimate.has_value()) { assignments.push_back( @@ -755,7 +755,7 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.model->estimate_finish( - entry.state, constraints, *estimate_cache); + entry.state, constraints, *travel_estimator); if (finish.has_value()) { @@ -804,7 +804,7 @@ class TaskPlanner::Implementation charge_battery->tag()->earliest_start_time(), config.parameters()); auto battery_estimate = charge_battery_model->estimate_finish( - entry.state, constraints, *estimate_cache); + entry.state, constraints, *travel_estimator); if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( @@ -820,7 +820,7 @@ class TaskPlanner::Implementation const auto finish = new_u.second.model->estimate_finish( battery_estimate.value().finish_state(), - constraints, *estimate_cache); + constraints, *travel_estimator); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -890,7 +890,7 @@ class TaskPlanner::Implementation charge_battery->tag()->earliest_start_time(), config.parameters()); auto estimate = charge_battery_model->estimate_finish( - state, config.constraints(), *estimate_cache); + state, config.constraints(), *travel_estimator); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( @@ -909,7 +909,7 @@ class TaskPlanner::Implementation const auto finish = new_u.second.model->estimate_finish( estimate.value().finish_state(), - config.constraints(), *estimate_cache); + config.constraints(), *travel_estimator); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -1091,9 +1091,7 @@ TaskPlanner::TaskPlanner( Implementation{ configuration, default_options, - std::make_shared( - configuration.parameters().planner()-> - get_configuration().graph().num_waypoints()) + std::make_shared(configuration.parameters()) })) { // Do nothing @@ -1139,13 +1137,6 @@ auto TaskPlanner::compute_cost(const Assignments& assignments) const -> double const auto cost_calculator = rmf_task::BinaryPriorityScheme::make_cost_calculator(); return cost_calculator->compute_cost(assignments); - -} - -// ============================================================================ -const std::shared_ptr& TaskPlanner::estimate_cache() const -{ - return _pimpl->estimate_cache; } // ============================================================================ diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp index 19eda242..16ee6d47 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp @@ -101,7 +101,7 @@ std::shared_ptr Candidates::make( const Constraints& constraints, const Parameters& parameters, const Request::Model& request_model, - EstimateCache& estimate_cache, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error) { Map initial_map; @@ -109,7 +109,7 @@ std::shared_ptr Candidates::make( { const auto& state = initial_states[i]; const auto finish = request_model.estimate_finish( - state, constraints, estimate_cache); + state, constraints, travel_estimator); if (finish.has_value()) { initial_map.insert({ @@ -130,13 +130,13 @@ std::shared_ptr Candidates::make( parameters); auto battery_estimate = battery_model->estimate_finish( - state, constraints, estimate_cache); + state, constraints, travel_estimator); if (battery_estimate.has_value()) { auto new_finish = request_model.estimate_finish( battery_estimate.value().finish_state(), constraints, - estimate_cache); + travel_estimator); if (new_finish.has_value()) { initial_map.insert( @@ -196,7 +196,7 @@ std::shared_ptr PendingTask::make( const Constraints& constraints, const Parameters& parameters, const ConstRequestPtr request_, - EstimateCache& estimate_cache, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error) { const auto earliest_start_time = std::max( @@ -206,8 +206,7 @@ std::shared_ptr PendingTask::make( earliest_start_time, parameters); const auto candidates = Candidates::make(start_time, initial_states, - constraints, parameters, *model, estimate_cache, - error); + constraints, parameters, *model, travel_estimator, error); if (!candidates) return nullptr; diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp index 4ca5bc38..a7371462 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp @@ -76,7 +76,7 @@ class Candidates const Constraints& constraints, const Parameters& parameters, const Request::Model& request_model, - EstimateCache& estimate_cache, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error); Candidates(const Candidates& other); @@ -116,7 +116,7 @@ class PendingTask const Constraints& constraints, const Parameters& parameters, const ConstRequestPtr request_, - EstimateCache& estimate_cache, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error); rmf_task::ConstRequestPtr request; diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 3c34254c..46a7b377 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -54,7 +54,7 @@ class ChargeBattery::Model : public Request::Model std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const TravelEstimator& travel_estimator) const final; rmf_traffic::Duration invariant_duration() const final; @@ -80,17 +80,16 @@ ChargeBattery::Model::Model( //============================================================================== std::optional -ChargeBattery::Model::estimate_finish( - const State& initial_state, +ChargeBattery::Model::estimate_finish(const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const TravelEstimator& travel_estimator) const { // Important to return nullopt if a charging task is not needed. In the task // planner, if a charging task is added, the node's latest time may be set to // the finishing time of the charging task, and consequently fall below the - // segmentation threshold, causing `solve` to return. This may cause an infinite - // loop as a new identical charging task is added in each call to `solve` before - // returning. + // segmentation threshold, causing `solve` to return. This may cause an + // infinite loop as a new identical charging task is added in each call to + // `solve` before returning. const auto recharge_soc = task_planning_constraints.recharge_soc(); if (initial_state.battery_soc() >= recharge_soc - 1e-3 && initial_state.waypoint().value() @@ -109,60 +108,21 @@ ChargeBattery::Model::estimate_finish( initial_state.dedicated_charging_waypoint().value(), initial_state.battery_soc().value()); - const auto start_time = initial_state.time().value(); - double battery_soc = initial_state.battery_soc().value(); - double dSOC_motion = 0.0; - double dSOC_device = 0.0; - double variant_battery_drain = 0.0; rmf_traffic::Duration variant_duration(0); - const bool drain_battery = task_planning_constraints.drain_battery(); - const auto& planner = *_parameters.planner(); - const auto& motion_sink = *_parameters.motion_sink(); - const auto& ambient_sink = *_parameters.ambient_sink(); if (initial_state.waypoint() != initial_state.dedicated_charging_waypoint()) { - const auto endpoints = std::make_pair( - initial_state.waypoint().value(), - initial_state.dedicated_charging_waypoint().value()); - - const auto& cache_result = estimate_cache.get(endpoints); - // Use memoized values if possible - if (cache_result) - { - variant_duration = cache_result->duration; - battery_soc = battery_soc - cache_result->dsoc; - } - else - { - // Compute plan to charging waypoint along with battery drain - rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result = planner.plan( - initial_state.extract_plan_start().value(), goal); - auto itinerary_start_time = start_time; - for (const auto& itinerary : result->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - if (drain_battery) - { - dSOC_motion = motion_sink.compute_change_in_charge( - trajectory); - dSOC_device = ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - variant_battery_drain += dSOC_motion + dSOC_device; - battery_soc = battery_soc - dSOC_motion - dSOC_device; - } - itinerary_start_time = finish_time; - variant_duration += itinerary_duration; - } - estimate_cache.set(endpoints, variant_duration, - variant_battery_drain); - } + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), + rmf_traffic::agv::Plan::Goal( + initial_state.dedicated_charging_waypoint().value())); + + if (!travel.has_value()) + return std::nullopt; + + variant_duration = travel->duration(); + battery_soc = battery_soc - travel->change_in_state_of_charge(); // If a robot cannot reach its charging dock given its initial battery soc if (battery_soc <= task_planning_constraints.threshold_soc()) diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index d8061ea7..66a0ea24 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -30,7 +30,7 @@ class Clean::Model : public Request::Model std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const TravelEstimator& estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; @@ -87,7 +87,7 @@ Clean::Model::Model( std::optional Clean::Model::estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const TravelEstimator& estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.time().value(), From 1707f4f99511e041d24d7995e99215a380942183 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 8 Sep 2021 00:45:24 +0800 Subject: [PATCH 12/85] Redesigning travel estimation Signed-off-by: Michael X. Grey --- rmf_task/CMakeLists.txt | 1 + rmf_task/include/rmf_task/Estimate.hpp | 5 +- rmf_task/include/rmf_task/execute/Phase.hpp | 6 +- rmf_task/include/rmf_task/sequence/Phase.hpp | 5 +- .../rmf_task/sequence/phases/GoToPlace.hpp | 1 + rmf_task/src/rmf_task/Estimate.cpp | 27 ++- rmf_task/src/rmf_task/execute/Phase.cpp | 78 +++++++++ .../src/rmf_task/requests/ChargeBattery.cpp | 4 +- rmf_task/src/rmf_task/requests/Clean.cpp | 115 +++---------- rmf_task/src/rmf_task/requests/Delivery.cpp | 133 ++++----------- rmf_task/src/rmf_task/requests/Loop.cpp | 154 +++++------------- .../rmf_task/sequence/phases/GoToPlace.cpp | 55 ++++++- 12 files changed, 263 insertions(+), 321 deletions(-) create mode 100644 rmf_task/src/rmf_task/execute/Phase.cpp diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 5705c3d1..f019343a 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Tasks library file(GLOB lib_srcs "src/rmf_task/agv/*.cpp" + "src/rmf_task/execute/*.cpp" "src/rmf_task/sequence/*.cpp" "src/rmf_task/sequence/phases/*.cpp" "src/rmf_task/requests/*.cpp" diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index a5f39a87..67d6f206 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -78,13 +78,16 @@ class TravelEstimator /// The parameters for the robot TravelEstimator(const Parameters& parameters); + /// The result of a travel estimation class Result { public: + /// How long the travelling will take rmf_traffic::Duration duration() const; - double change_in_state_of_charge() const; + /// How much the battery will drain while travelling + double change_in_charge() const; class Implementation; private: diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/execute/Phase.hpp index d51ac518..7c976000 100644 --- a/rmf_task/include/rmf_task/execute/Phase.hpp +++ b/rmf_task/include/rmf_task/execute/Phase.hpp @@ -58,6 +58,8 @@ class Phase::Tag { public: + using Id = uint64_t; + /// Constructor /// /// \param[in] id_ @@ -72,13 +74,13 @@ class Phase::Tag /// \param[in] estimate_ /// The original (ideal) estimate of how long the phase will last Tag( - uint64_t id_, + Id id_, std::string name_, std::string detail_, rmf_traffic::Duration estimate_); /// Unique ID of the phase - uint64_t id() const; + Id id() const; /// Name of the phase const std::string& name() const; diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task/include/rmf_task/sequence/Phase.hpp index 51a47c31..0e27523e 100644 --- a/rmf_task/include/rmf_task/sequence/Phase.hpp +++ b/rmf_task/include/rmf_task/sequence/Phase.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -229,6 +230,7 @@ class Phase::Description /// \param[in] constraints /// Constraints on the robot during the phase virtual execute::Phase::ConstTagPtr make_tag( + execute::Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const = 0; @@ -252,7 +254,8 @@ class Phase::Model /// \return an estimated state for the robot when the phase is finished. virtual std::optional estimate_finish( State initial_state, - const Constraints& constraints) const = 0; + const Constraints& constraints, + const TravelEstimator& travel_estimator) const = 0; /// Estimate the invariant component of the request's duration. virtual rmf_traffic::Duration invariant_duration() const = 0; diff --git a/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp b/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp index 53614e70..3acaeef2 100644 --- a/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp +++ b/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp @@ -51,6 +51,7 @@ class GoToPlace::Description : public Phase::Description // Documentation inherited execute::Phase::ConstTagPtr make_tag( + execute::Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const final; diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 29e52fb9..6389c30a 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -86,7 +86,7 @@ class TravelEstimator::Result::Implementation } rmf_traffic::Duration duration; - double change_in_state_of_charge; + double change_in_charge; }; //============================================================================== @@ -199,6 +199,31 @@ class TravelEstimator::Implementation mutable std::mutex mutex; }; +//============================================================================== +TravelEstimator::TravelEstimator(const Parameters& parameters) +: _pimpl(rmf_utils::make_unique_impl(parameters)) +{ + // Do nothing +} + +//============================================================================== +rmf_traffic::Duration TravelEstimator::Result::duration() const +{ + return _pimpl->duration; +} + +//============================================================================== +double TravelEstimator::Result::change_in_charge() const +{ + return _pimpl->change_in_charge; +} + +//============================================================================== +TravelEstimator::Result::Result() +{ + // Do nothing +} + //============================================================================== auto TravelEstimator::estimate( const rmf_traffic::agv::Plan::Start& start, diff --git a/rmf_task/src/rmf_task/execute/Phase.cpp b/rmf_task/src/rmf_task/execute/Phase.cpp new file mode 100644 index 00000000..7d15596d --- /dev/null +++ b/rmf_task/src/rmf_task/execute/Phase.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { +namespace execute { + +//============================================================================== +class Phase::Tag::Implementation +{ +public: + + Id id; + std::string name; + std::string detail; + rmf_traffic::Duration duration; + +}; + +//============================================================================== +Phase::Tag::Tag( + Id id_, + std::string name_, + std::string detail_, + rmf_traffic::Duration estimate_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + id_, + std::move(name_), + std::move(detail_), + estimate_ + })) +{ + // Do nothing +} + +//============================================================================== +auto Phase::Tag::id() const -> Id +{ + return _pimpl->id; +} + +//============================================================================== +const std::string& Phase::Tag::name() const +{ + return _pimpl->name; +} + +//============================================================================== +const std::string& Phase::Tag::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +rmf_traffic::Duration Phase::Tag::original_duration_estimate() const +{ + return _pimpl->duration; +} + + +} // namespace execute +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 46a7b377..a4465434 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -103,6 +103,7 @@ ChargeBattery::Model::estimate_finish(const State& initial_state, initial_state.time().value(), initial_state.dedicated_charging_waypoint().value(), initial_state.orientation().value()}; + auto state = State().load_basic( std::move(final_plan_start), initial_state.dedicated_charging_waypoint().value(), @@ -122,7 +123,8 @@ ChargeBattery::Model::estimate_finish(const State& initial_state, return std::nullopt; variant_duration = travel->duration(); - battery_soc = battery_soc - travel->change_in_state_of_charge(); + if (task_planning_constraints.drain_battery()) + battery_soc = battery_soc - travel->change_in_charge(); // If a robot cannot reach its charging dock given its initial battery soc if (battery_soc <= task_planning_constraints.threshold_soc()) diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 66a0ea24..0f393943 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -87,12 +87,13 @@ Clean::Model::Model( std::optional Clean::Model::estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - const TravelEstimator& estimate_cache) const + const TravelEstimator& travel_estimator) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.time().value(), _end_waypoint, initial_state.orientation().value()}; + auto state = State().load_basic( std::move(final_plan_start), initial_state.dedicated_charging_waypoint().value(), @@ -101,62 +102,22 @@ std::optional Clean::Model::estimate_finish( rmf_traffic::Duration variant_duration(0); rmf_traffic::Duration end_duration(0); - const rmf_traffic::Time start_time = initial_state.time().value(); double battery_soc = initial_state.battery_soc().value(); - double dSOC_motion = 0.0; - double dSOC_ambient = 0.0; const bool drain_battery = task_planning_constraints.drain_battery(); - const auto& planner = *_parameters.planner(); - const auto& motion_sink = *_parameters.motion_sink(); const auto& ambient_sink = *_parameters.ambient_sink(); if (initial_state.waypoint() != _start_waypoint) { - const auto endpoints = std::make_pair( - initial_state.waypoint().value(), + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), _start_waypoint); - const auto& cache_result = estimate_cache.get(endpoints); - if (cache_result) - { - variant_duration = cache_result->duration; - if (drain_battery) - battery_soc = battery_soc - cache_result->dsoc; - } - else - { - rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - - const auto result_to_start = planner.plan( - initial_state.extract_plan_start().value(), goal); - // We assume we can always compute a plan - auto itinerary_start_time = start_time; - double variant_battery_drain = 0.0; - for (const auto& itinerary : result_to_start->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - if (drain_battery) - { - // Compute battery drain - dSOC_motion = motion_sink.compute_change_in_charge( - trajectory); - dSOC_ambient = - ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_ambient; - variant_battery_drain += dSOC_motion + dSOC_ambient; - } - itinerary_start_time = finish_time; - variant_duration += itinerary_duration; - } - - estimate_cache.set(endpoints, variant_duration, - variant_battery_drain); - } + if (!travel.has_value()) + return std::nullopt; + + variant_duration = travel->duration(); + if (drain_battery) + battery_soc = battery_soc - travel->change_in_charge(); if (battery_soc <= task_planning_constraints.threshold_soc()) return std::nullopt; @@ -182,7 +143,7 @@ std::optional Clean::Model::estimate_finish( { rmf_traffic::Duration wait_duration( wait_until - initial_state.time().value()); - dSOC_ambient = ambient_sink.compute_change_in_charge( + const double dSOC_ambient = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_ambient; @@ -203,52 +164,20 @@ std::optional Clean::Model::estimate_finish( return std::nullopt; // Check if the robot has enough charge to head back to nearest charger - double retreat_battery_drain = 0.0; if (_end_waypoint != state.dedicated_charging_waypoint().value()) { - const auto endpoints = std::make_pair(_end_waypoint, - state.dedicated_charging_waypoint().value()); - const auto& cache_result = estimate_cache.get(endpoints); - if (cache_result) - { - retreat_battery_drain = cache_result->dsoc; - } - else - { - rmf_traffic::agv::Planner::Start start{ - state.time().value(), - endpoints.first, - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - - const auto result_to_charger = planner.plan(start, goal); - // We assume we can always compute a plan - auto itinerary_start_time = state.time().value(); - rmf_traffic::Duration retreat_duration(0); - for (const auto& itinerary : result_to_charger->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - dSOC_motion = motion_sink.compute_change_in_charge( - trajectory); - dSOC_ambient = ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - retreat_battery_drain += dSOC_motion + dSOC_ambient; - - itinerary_start_time = finish_time; - retreat_duration += itinerary_duration; - } - estimate_cache.set(endpoints, retreat_duration, retreat_battery_drain); - } - } + const auto travel = travel_estimator.estimate( + state.extract_plan_start().value(), + state.dedicated_charging_waypoint().value()); - if (battery_soc - retreat_battery_drain <= - task_planning_constraints.threshold_soc()) - return std::nullopt; + if (!travel.has_value()) + return std::nullopt; + + const double retreat_battery_drain = travel->change_in_charge(); + const double threshold = task_planning_constraints.threshold_soc(); + if (battery_soc - retreat_battery_drain <= threshold) + return std::nullopt; + } state.battery_soc(battery_soc); } diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 97777dbc..9fa299a5 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -30,7 +30,7 @@ class Delivery::Model : public Request::Model std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const TravelEstimator& travel_estimator) const final; rmf_traffic::Duration invariant_duration() const final; @@ -46,9 +46,7 @@ class Delivery::Model : public Request::Model rmf_traffic::Time _earliest_start_time; Parameters _parameters; std::size_t _pickup_waypoint; - rmf_traffic::Duration _pickup_wait; std::size_t _dropoff_waypoint; - rmf_traffic::Duration _dropoff_wait; rmf_traffic::Duration _invariant_duration; double _invariant_battery_drain; @@ -109,12 +107,13 @@ Delivery::Model::Model( std::optional Delivery::Model::estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const TravelEstimator& travel_estimator) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.time().value(), _dropoff_waypoint, initial_state.orientation().value()}; + auto state = State().load_basic( std::move(final_plan_start), initial_state.dedicated_charging_waypoint().value(), @@ -122,65 +121,26 @@ std::optional Delivery::Model::estimate_finish( rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.time().value(); double battery_soc = initial_state.battery_soc().value(); - double dSOC_motion = 0.0; - double dSOC_device = 0.0; const bool drain_battery = task_planning_constraints.drain_battery(); - const auto& planner = *_parameters.planner(); - const auto& motion_sink = *_parameters.motion_sink(); const auto& ambient_sink = *_parameters.ambient_sink(); + const double battery_threshold = task_planning_constraints.threshold_soc(); // Factor in battery drain while moving to start waypoint of task if (initial_state.waypoint() != _pickup_waypoint) { - const auto endpoints = std::make_pair( - initial_state.waypoint().value(), + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), _pickup_waypoint); - const auto& cache_result = estimate_cache.get(endpoints); - // Use previously memoized values if possible - if (cache_result) - { - variant_duration = cache_result->duration; - if (drain_battery) - battery_soc = battery_soc - cache_result->dsoc; - } - else - { - // Compute plan to pickup waypoint along with battery drain - rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result_to_pickup = planner.plan( - initial_state.extract_plan_start().value(), goal); - // We assume we can always compute a plan - auto itinerary_start_time = start_time; - double variant_battery_drain = 0.0; - for (const auto& itinerary : result_to_pickup->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - if (drain_battery) - { - // Compute battery drain - dSOC_motion = motion_sink.compute_change_in_charge( - trajectory); - dSOC_device = - ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; - variant_battery_drain += dSOC_device + dSOC_motion; - } - itinerary_start_time = finish_time; - variant_duration += itinerary_duration; - } - estimate_cache.set(endpoints, variant_duration, - variant_battery_drain); - } + if (!travel) + return std::nullopt; - if (battery_soc <= task_planning_constraints.threshold_soc()) + variant_duration = travel->duration(); + if (drain_battery) + battery_soc = battery_soc - travel->change_in_charge(); + + if (battery_soc <= battery_threshold) return std::nullopt; } @@ -194,16 +154,16 @@ std::optional Delivery::Model::estimate_finish( if (drain_battery && wait_until > initial_state.time().value() && initial_state.waypoint() != initial_state.dedicated_charging_waypoint()) { - rmf_traffic::Duration wait_duration( + const rmf_traffic::Duration wait_duration( wait_until - initial_state.time().value()); - dSOC_device = ambient_sink.compute_change_in_charge( + + const double dSOC_device = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_device; - if (battery_soc <= task_planning_constraints.threshold_soc()) - { + if (battery_soc <= battery_threshold) return std::nullopt; - } } // Factor in invariants @@ -211,58 +171,25 @@ std::optional Delivery::Model::estimate_finish( if (drain_battery) { + // Calculate how much battery is drained while waiting for the pickup and + // waiting for the dropoff battery_soc -= _invariant_battery_drain; - if (battery_soc <= task_planning_constraints.threshold_soc()) + if (battery_soc <= battery_threshold) return std::nullopt; // Check if the robot has enough charge to head back to nearest charger - double retreat_battery_drain = 0.0; if (_dropoff_waypoint != state.dedicated_charging_waypoint().value()) { - const auto endpoints = std::make_pair(_dropoff_waypoint, - state.dedicated_charging_waypoint().value()); - const auto& cache_result = estimate_cache.get(endpoints); - if (cache_result) - { - retreat_battery_drain = cache_result->dsoc; - } - else - { - rmf_traffic::agv::Planner::Start start{ - state.time().value(), - endpoints.first, - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - - const auto result_to_charger = planner.plan(start, goal); - // We assume we can always compute a plan - auto itinerary_start_time = state.time().value(); - rmf_traffic::Duration retreat_duration(0); - for (const auto& itinerary : result_to_charger->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - dSOC_motion = - motion_sink.compute_change_in_charge(trajectory); - dSOC_device = ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - retreat_battery_drain += dSOC_motion + dSOC_device; - - itinerary_start_time = finish_time; - retreat_duration += itinerary_duration; - } - estimate_cache.set(endpoints, retreat_duration, - retreat_battery_drain); - } - } + const auto travel = travel_estimator.estimate( + state.extract_plan_start().value(), + state.dedicated_charging_waypoint().value()); - if (battery_soc - retreat_battery_drain <= - task_planning_constraints.threshold_soc()) - return std::nullopt; + if (!travel.has_value()) + return std::nullopt; + + if (battery_soc - travel->change_in_charge() <= battery_threshold) + return std::nullopt; + } state.battery_soc(battery_soc); } diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 30ca7c44..eb16f31c 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -28,7 +28,7 @@ class Loop::Model : public Request::Model std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const TravelEstimator& travel_estimator) const final; rmf_traffic::Duration invariant_duration() const final; @@ -100,76 +100,36 @@ Loop::Model::Model( _invariant_battery_drain = (2 * num_loops - 1) * forward_battery_drain; } - } //============================================================================== std::optional Loop::Model::estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const TravelEstimator& travel_estimator) const { rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.time().value(); double battery_soc = initial_state.battery_soc().value(); - double dSOC_motion = 0.0; - double dSOC_device = 0.0; const bool drain_battery = task_planning_constraints.drain_battery(); - const auto& planner = *_parameters.planner(); - const auto& motion_sink = *_parameters.motion_sink(); const auto& ambient_sink = *_parameters.ambient_sink(); + const auto battery_threshold = task_planning_constraints.threshold_soc(); // Check if a plan has to be generated from finish location to start_waypoint if (initial_state.waypoint() != _start_waypoint) { - auto endpoints = std::make_pair( - initial_state.waypoint().value(), + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), _start_waypoint); - const auto& cache_result = estimate_cache.get(endpoints); - // Use previously memoized values if possible - if (cache_result) - { - variant_duration = cache_result->duration; - if (drain_battery) - battery_soc = battery_soc - cache_result->dsoc; - } - else - { - // Compute plan to start_waypoint along with battery drain - rmf_traffic::agv::Planner::Goal loop_start_goal{endpoints.second}; - const auto plan_to_start = planner.plan( - initial_state.extract_plan_start().value(), loop_start_goal); - // We assume we can always compute a plan - auto itinerary_start_time = start_time; - double variant_battery_drain = 0.0; - for (const auto& itinerary : plan_to_start->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - if (drain_battery) - { - // Compute battery drain - dSOC_motion = - motion_sink.compute_change_in_charge(trajectory); - dSOC_device = - ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; - variant_battery_drain += dSOC_motion + dSOC_device; - } - itinerary_start_time = finish_time; - variant_duration += itinerary_duration; - } - estimate_cache.set(endpoints, variant_duration, - variant_battery_drain); - } + if (!travel.has_value()) + return std::nullopt; - if (battery_soc <= task_planning_constraints.threshold_soc()) + variant_duration = travel->duration(); + if (drain_battery) + battery_soc = battery_soc - travel->change_in_charge(); + + if (battery_soc <= battery_threshold) return std::nullopt; } @@ -187,78 +147,25 @@ std::optional Loop::Model::estimate_finish( { rmf_traffic::Duration wait_duration( wait_until - initial_state.time().value()); - dSOC_device = ambient_sink.compute_change_in_charge( + + const auto dSOC_device = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); - battery_soc = battery_soc - dSOC_device; - if (battery_soc <= task_planning_constraints.threshold_soc()) - { + battery_soc = battery_soc - dSOC_device; + if (battery_soc <= battery_threshold) return std::nullopt; - } } // Compute finish time const rmf_traffic::Time state_finish_time = - wait_until + variant_duration + _invariant_duration; + wait_until + variant_duration + _invariant_duration; - // Subtract invariant battery drain and check if robot can return to its charger - double retreat_battery_drain = 0.0; + // Subtract invariant battery drain if (drain_battery) { battery_soc -= _invariant_battery_drain; - if (battery_soc <= task_planning_constraints.threshold_soc()) + if (battery_soc <= battery_threshold) return std::nullopt; - - if (_finish_waypoint != initial_state.dedicated_charging_waypoint().value()) - { - const auto endpoints = std::make_pair( - _finish_waypoint, - initial_state.dedicated_charging_waypoint().value()); - - const auto& cache_result = estimate_cache.get(endpoints); - if (cache_result) - { - retreat_battery_drain = cache_result->dsoc; - } - else - { - rmf_traffic::agv::Planner::Start retreat_start{ - state_finish_time, - endpoints.first, - 0.0}; - - rmf_traffic::agv::Planner::Goal charger_goal{ - endpoints.second}; - - const auto result_to_charger = planner.plan( - retreat_start, charger_goal); - // We assume we can always compute a plan - auto itinerary_start_time = state_finish_time; - rmf_traffic::Duration retreat_duration(0); - for (const auto& itinerary : result_to_charger->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - dSOC_motion = motion_sink.compute_change_in_charge( - trajectory); - dSOC_device = ambient_sink.compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - retreat_battery_drain += dSOC_motion + dSOC_device; - - itinerary_start_time = finish_time; - retreat_duration += itinerary_duration; - } - estimate_cache.set(endpoints, retreat_duration, - retreat_battery_drain); - } - - if (battery_soc - retreat_battery_drain <= - task_planning_constraints.threshold_soc()) - return std::nullopt; - } } // Return Estimate @@ -266,12 +173,31 @@ std::optional Loop::Model::estimate_finish( state_finish_time, _finish_waypoint, initial_state.orientation().value()}; - auto state = State().load_basic( + + auto finish_state = State().load_basic( std::move(location), initial_state.dedicated_charging_waypoint().value(), battery_soc); - return Estimate(state, wait_until); + // Check if robot can return to its charger + if (drain_battery) + { + if (_finish_waypoint != initial_state.dedicated_charging_waypoint().value()) + { + const auto travel = travel_estimator.estimate( + finish_state.extract_plan_start().value(), + finish_state.dedicated_charging_waypoint().value()); + + if (!travel.has_value()) + return std::nullopt; + + const auto retreat_battery_drain = travel->change_in_charge(); + if (battery_soc - retreat_battery_drain <= battery_threshold) + return std::nullopt; + } + } + + return Estimate(finish_state, wait_until); } //============================================================================== diff --git a/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp b/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp index 7481cee1..7b3b895f 100644 --- a/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp +++ b/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp @@ -55,7 +55,8 @@ class GoToPlace::Model : public Phase::Model // Documentation inherited std::optional estimate_finish( State initial_state, - const Constraints& constraints) const final; + const Constraints& constraints, + const TravelEstimator& ) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; @@ -68,12 +69,10 @@ class GoToPlace::Model : public Phase::Model Model( State invariant_finish_state, rmf_traffic::Duration invariant_duration, - const Parameters& parameters, Goal goal); State _invariant_finish_state; rmf_traffic::Duration _invariant_duration; - Parameters _parameters; Goal _goal; }; @@ -109,18 +108,62 @@ Phase::ConstModelPtr GoToPlace::Model::make( new Model( std::move(invariant_finish_state), invariant_duration, - parameters, std::move(goal))); } //============================================================================== std::optional GoToPlace::Model::estimate_finish( State initial_state, - const Constraints& constraints) const + const Constraints& constraints, + const TravelEstimator& travel_estimator) const { auto finish = initial_state; finish.waypoint(_goal.waypoint()); + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), + _goal); + + if (!travel.has_value()) + return std::nullopt; + + finish.time(finish.time().value() + travel->duration()); + auto battery_soc = finish.battery_soc().value(); + + if (constraints.drain_battery()) + battery_soc = battery_soc - travel->change_in_charge(); + + finish.battery_soc(battery_soc); + + const auto battery_threshold = constraints.threshold_soc(); + if (battery_soc <= battery_threshold) + return std::nullopt; + + return finish; +} + +//============================================================================== +rmf_traffic::Duration GoToPlace::Model::invariant_duration() const +{ + return _invariant_duration; +} + +//============================================================================== +State GoToPlace::Model::invariant_finish_state() const +{ + return _invariant_finish_state; +} + +//============================================================================== +GoToPlace::Model::Model( + State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + Goal goal) +: _invariant_finish_state(std::move(invariant_finish_state)), + _invariant_duration(invariant_duration), + _goal(std::move(goal)) +{ + // Do nothing } //============================================================================== @@ -158,6 +201,7 @@ Phase::ConstModelPtr GoToPlace::Description::make_model( //============================================================================== execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( + execute::Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const { @@ -193,6 +237,7 @@ execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( return nullptr; return std::make_shared( + id, "Go to [" + goal_name + "]", "Moving the robot from [" + start_name + "] to [" + goal_name + "]", *estimate); From 45d62a6dff9c13ef6fbff0be6194464d78d18195 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 8 Sep 2021 16:38:23 +0800 Subject: [PATCH 13/85] Prototyping PickUp phase Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Payload.hpp | 57 ++++++++++ .../rmf_task/sequence/phases/PickUp.hpp | 87 +++++++++++++++ .../sequence/phases/PayloadTransfer.cpp | 103 ++++++++++++++++++ .../src/rmf_task/sequence/phases/PickUp.cpp | 17 +++ .../phases/internal_PayloadTransfer.hpp | 79 ++++++++++++++ 5 files changed, 343 insertions(+) create mode 100644 rmf_task/include/rmf_task/Payload.hpp create mode 100644 rmf_task/include/rmf_task/sequence/phases/PickUp.hpp create mode 100644 rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp create mode 100644 rmf_task/src/rmf_task/sequence/phases/PickUp.cpp create mode 100644 rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp diff --git a/rmf_task/include/rmf_task/Payload.hpp b/rmf_task/include/rmf_task/Payload.hpp new file mode 100644 index 00000000..4e421674 --- /dev/null +++ b/rmf_task/include/rmf_task/Payload.hpp @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__PAYLOAD_HPP +#define RMF_TASK__PAYLOAD_HPP + +#include +#include + +namespace rmf_task { + +//============================================================================== +class Payload +{ +public: + + class Component; + + /// Components in the payload + const std::vector& components() const; + +private: + +}; + +//============================================================================== +class Payload::Component +{ +public: + + /// Stock Keeping Unit (SKU) for this component of the payload + const std::string& sku() const; + + /// The quantity of the specified item in this component of the payload + uint32_t quantity() const; + + /// The name of the compartment + const std::string& compartment() const; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__PAYLOAD_HPP diff --git a/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp b/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp new file mode 100644 index 00000000..8db098fc --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP +#define RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP + +#include + +#include +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +class PickUp +{ +public: + + using Location = rmf_traffic::agv::Plan::Goal; + + class Description; + class Model; +}; + +//============================================================================== +class PickUp::Description : public Phase::Description +{ +public: + + /// Make a Pickup phase + /// + /// \param[in] pickup_location + /// The location that the robot needs to get to for the pickup + /// + /// \param[in] from_dispenser + /// The dispenser that will take care of loading the items. We will + /// communicate with this dispenser to verify the success of loading the + /// items. + /// + /// \param[in] payload + /// A description of should be loaded into the robot during the pick-up. + /// + /// \param[in] loading_duration_estimate + /// An estimate for how long it will likely take to load the items. + static std::shared_ptr make( + Location pickup_location, + std::string from_dispenser, + Payload payload, + rmf_traffic::Duration loading_duration_estimate); + + // Documentation inherited + Phase::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + execute::Phase::ConstTagPtr make_tag( + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace phases +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP diff --git a/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp b/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp new file mode 100644 index 00000000..3e473414 --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2021 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_PayloadTransfer.hpp" + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +Phase::ConstModelPtr PayloadTransfer::Model::make( + State invariant_initial_state, + const Parameters& parameters, + const Phase::ConstDescriptionPtr& go_to_place, + rmf_traffic::Duration transfer_duration) +{ + auto go_to_place_model = go_to_place->make_model( + std::move(invariant_initial_state), + parameters); + + if (!go_to_place_model) + return nullptr; + + const auto transfer_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(transfer_duration)); + + return std::shared_ptr( + new Model( + std::move(go_to_place_model), + transfer_duration, + transfer_battery_drain)); +} + +//============================================================================== +std::optional PayloadTransfer::Model::estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const +{ + // TODO(MXG): Consider adding the changed Payload to the state + auto finish_state = + _go_to_place->estimate_finish(initial_state, constraints, travel_estimator); + + if (!finish_state.has_value()) + return std::nullopt; + + finish_state->time(finish_state->time().value() + _transfer_duration); + + if (constraints.drain_battery()) + { + finish_state->battery_soc( + finish_state->battery_soc().value() - _transfer_battery_drain); + } + + const auto battery_threshold = constraints.threshold_soc(); + if (finish_state->battery_soc().value() <= battery_threshold) + return std::nullopt; + + return finish_state; +} + +//============================================================================== +rmf_traffic::Duration PayloadTransfer::Model::invariant_duration() const +{ + return _transfer_duration; +} + +//============================================================================== +State PayloadTransfer::Model::invariant_finish_state() const +{ + return _go_to_place->invariant_finish_state(); +} + +//============================================================================== +PayloadTransfer::Model::Model( + Phase::ConstModelPtr go_to_place, + rmf_traffic::Duration transfer_duration, + double transfer_battery_drain) +: _go_to_place(std::move(go_to_place)), + _transfer_duration(transfer_duration), + _transfer_battery_drain(transfer_battery_drain) +{ + // Do nothing +} + +} // namespace phases +} // namespace sequence +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp b/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp new file mode 100644 index 00000000..feb743c5 --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp @@ -0,0 +1,17 @@ +/* + * Copyright (C) 2021 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. + * +*/ + diff --git a/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp b/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp new file mode 100644 index 00000000..d8b2c81a --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2021 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_PAYLOADTRANSFER_HPP +#define SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP + +#include +#include +#include + +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +class PayloadTransfer +{ +public: + + using Location = rmf_traffic::agv::Plan::Goal; + + std::string target; + rmf_traffic::Duration transfer_duration; + Phase::ConstDescriptionPtr go_to_place; + + class Model : public Phase::Model + { + public: + + static Phase::ConstModelPtr make( + State invariant_initial_state, + const Parameters& parameters, + const Phase::ConstDescriptionPtr& go_to_place, + rmf_traffic::Duration transfer_duration); + + std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + rmf_traffic::Duration invariant_duration() const final; + + State invariant_finish_state() const final; + + private: + + Model( + Phase::ConstModelPtr go_to_place, + rmf_traffic::Duration transfer_duration, + double transfer_battery_drain); + + Phase::ConstModelPtr _go_to_place; + rmf_traffic::Duration _transfer_duration; + double _transfer_battery_drain; + }; + +}; + +} // namespace phases +} // namespace sequence +} // namespace rmf_task + +#endif // SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP From 81380c8808b13f5f65ef487924fd67311335dec0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 9 Sep 2021 17:14:17 +0800 Subject: [PATCH 14/85] Finish implementation of PickUp and begin API of DropOff Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/CompositeData.hpp | 5 + rmf_task/include/rmf_task/Payload.hpp | 24 ++- rmf_task/include/rmf_task/sequence/Phase.hpp | 51 ++++++ .../rmf_task/sequence/phases/DropOff.hpp | 116 +++++++++++++ .../rmf_task/sequence/phases/GoToPlace.hpp | 9 +- .../rmf_task/sequence/phases/PickUp.hpp | 37 ++++- .../rmf_task/sequence/phases/WaitFor.hpp | 87 ++++++++++ rmf_task/src/rmf_task/Payload.cpp | 142 ++++++++++++++++ rmf_task/src/rmf_task/sequence/Phase.cpp | 99 +++++++++++ .../rmf_task/sequence/phases/GoToPlace.cpp | 52 ++++-- .../sequence/phases/PayloadTransfer.cpp | 95 ++++------- .../src/rmf_task/sequence/phases/PickUp.cpp | 116 +++++++++++++ .../src/rmf_task/sequence/phases/WaitFor.cpp | 156 ++++++++++++++++++ .../phases/internal_PayloadTransfer.hpp | 55 +++--- 14 files changed, 925 insertions(+), 119 deletions(-) create mode 100644 rmf_task/include/rmf_task/sequence/phases/DropOff.hpp create mode 100644 rmf_task/include/rmf_task/sequence/phases/WaitFor.hpp create mode 100644 rmf_task/src/rmf_task/Payload.cpp create mode 100644 rmf_task/src/rmf_task/sequence/Phase.cpp create mode 100644 rmf_task/src/rmf_task/sequence/phases/WaitFor.cpp diff --git a/rmf_task/include/rmf_task/CompositeData.hpp b/rmf_task/include/rmf_task/CompositeData.hpp index c8bb0410..ac28e1fa 100644 --- a/rmf_task/include/rmf_task/CompositeData.hpp +++ b/rmf_task/include/rmf_task/CompositeData.hpp @@ -26,6 +26,11 @@ namespace rmf_task { //============================================================================== +/// A class that can store and return arbitrary data structures, as long as they +/// are copyable. +// +// TODO(MXG): Should this class move to rmf_utils? It is not very specific to +// task planning or management. class CompositeData { public: diff --git a/rmf_task/include/rmf_task/Payload.hpp b/rmf_task/include/rmf_task/Payload.hpp index 4e421674..084da6af 100644 --- a/rmf_task/include/rmf_task/Payload.hpp +++ b/rmf_task/include/rmf_task/Payload.hpp @@ -18,6 +18,8 @@ #ifndef RMF_TASK__PAYLOAD_HPP #define RMF_TASK__PAYLOAD_HPP +#include + #include #include @@ -30,11 +32,21 @@ class Payload class Component; + /// Constructor + Payload(std::vector components); + /// Components in the payload const std::vector& components() const; -private: + /// A brief human-friendly description of the payload + /// + /// \param[in] compartment_prefix + /// The prefix to use when describing the compartments + std::string brief(const std::string& compartment_prefix = "in") const; + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; }; //============================================================================== @@ -42,6 +54,12 @@ class Payload::Component { public: + /// Constructor + Component( + std::string sku, + uint32_t quantity, + std::string compartment); + /// Stock Keeping Unit (SKU) for this component of the payload const std::string& sku() const; @@ -50,6 +68,10 @@ class Payload::Component /// The name of the compartment const std::string& compartment() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; }; } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task/include/rmf_task/sequence/Phase.hpp index 0e27523e..61917780 100644 --- a/rmf_task/include/rmf_task/sequence/Phase.hpp +++ b/rmf_task/include/rmf_task/sequence/Phase.hpp @@ -48,6 +48,8 @@ class Phase class Model; using ConstModelPtr = std::shared_ptr; + + class SequenceModel; }; //============================================================================== @@ -251,6 +253,8 @@ class Phase::Model /// \param[in] constraints /// Constraints on the robot during the phase /// + /// \param[in] + /// /// \return an estimated state for the robot when the phase is finished. virtual std::optional estimate_finish( State initial_state, @@ -268,6 +272,53 @@ class Phase::Model virtual ~Model() = default; }; +//============================================================================== +/// An implementation of a Phase::Model that models a sequence of +/// Phase::Descriptions. +class Phase::SequenceModel : public Phase::Model +{ +public: + + /// Make a SequenceModel by providing a vector of descriptions and the + /// arguments that are given to Phase::Description::make_model(~). + /// + /// \param[in] descriptions + /// The Phase descriptions that are being modelled. The ordering of the + /// descriptions may impact model outcomes. The order of the descriptions + /// in the vector should reflect the actual order that the phases would + /// occur in. + /// + /// \param[in] invariant_initial_state + /// A partial state that represents the state components which will + /// definitely be true when this phase begins. + /// + /// \param[in] parameters + /// The parameters for the robot + /// + /// \return A Phase::Model implemented as a SequenceModel. + static Phase::ConstModelPtr make( + const std::vector& descriptions, + State invariant_initial_state, + const Parameters& parameters); + + // Documentation inherited + std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + // Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + // Documentation inherited + State invariant_finish_state() const final; + + class Implementation; +private: + SequenceModel(); + rmf_utils::unique_impl_ptr _pimpl; +}; + } // namespace sequence } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/sequence/phases/DropOff.hpp b/rmf_task/include/rmf_task/sequence/phases/DropOff.hpp new file mode 100644 index 00000000..051b3afc --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/phases/DropOff.hpp @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP +#define RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP + +#include + +#include +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +/// A DropOff phase encompasses going to a location and transferring a payload +/// off of the robot. +class DropOff +{ +public: + + using Location = rmf_traffic::agv::Plan::Goal; + + class Description; + using DescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class DropOff::Description : public Phase::Description +{ +public: + + /// Make a DropOff phase description + /// + /// \param[in] drop_off_location + /// The location that the robot needs to get to for the drop-off + /// + /// \param[in] into_ingestor + /// The ingestor that will take care of unloading the items. We will + /// communicate with this ingestor to verify the success of unloading the + /// items. + /// + /// \param[in] payload + /// A description of what should be unloaded from the robot during drop-off + /// + /// \param[in] unloading_duration_estimate + /// An estimate for how long it will likely take to unload the items. + static DescriptionPtr make( + Location drop_off_location, + std::string into_ingestor, + Payload payload, + rmf_traffic::Duration unloading_duration_estimate); + + /// Get the drop-off location + const Location& drop_off_location() const; + + /// Set the drop-off location + Description& drop_off_location(Location new_location); + + /// Get the ingestor to drop off into + const std::string& into_ingestor() const; + + /// Set the ingestor to drop off into + Description& into_ingestor(std::string new_ingestor); + + /// Get the Payload to drop off + const Payload& payload() const; + + /// Set the Payload to drop off + Description& payload(Payload new_payload); + + /// Get the unloading duration estimate + rmf_traffic::Duration unloading_duration_estimate() const; + + /// Set the unloading duration estimate + Description& unloading_duration_estimate(rmf_traffic::Duration new_duration); + + // Documentation inherited + Phase::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + execute::Phase::ConstTagPtr make_tag( + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace phases +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP diff --git a/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp b/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp index 3acaeef2..6fcbc8e7 100644 --- a/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp +++ b/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp @@ -33,6 +33,9 @@ class GoToPlace using Goal = rmf_traffic::agv::Plan::Goal; class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + class Model; }; @@ -42,7 +45,7 @@ class GoToPlace::Description : public Phase::Description public: /// Make a GoToPlace description using a goal. - static std::shared_ptr make(Goal goal); + static DescriptionPtr make(Goal goal); // Documentation inherited Phase::ConstModelPtr make_model( @@ -61,6 +64,10 @@ class GoToPlace::Description : public Phase::Description /// Set the current goal for this description. Description& goal(Goal new_goal); + /// Get the name of the goal. If the goal does not have an explicit name, it + /// will be referred to as "#x" where "x" is the index number of the waypoint. + std::string goal_name(const Parameters& parameters) const; + class Implementation; private: Description(); diff --git a/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp b/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp index 8db098fc..dd019700 100644 --- a/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp +++ b/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp @@ -28,6 +28,8 @@ namespace sequence { namespace phases { //============================================================================== +/// A PickUp phase encompasses going to a location and transferring a payload +/// into/onto the robot. class PickUp { public: @@ -35,6 +37,8 @@ class PickUp using Location = rmf_traffic::agv::Plan::Goal; class Description; + using DescriptionPtr = std::shared_ptr; + class Model; }; @@ -43,7 +47,7 @@ class PickUp::Description : public Phase::Description { public: - /// Make a Pickup phase + /// Make a PickUp phase description /// /// \param[in] pickup_location /// The location that the robot needs to get to for the pickup @@ -54,16 +58,40 @@ class PickUp::Description : public Phase::Description /// items. /// /// \param[in] payload - /// A description of should be loaded into the robot during the pick-up. + /// A description of what should be loaded into the robot during the pick-up /// /// \param[in] loading_duration_estimate /// An estimate for how long it will likely take to load the items. - static std::shared_ptr make( + static DescriptionPtr make( Location pickup_location, std::string from_dispenser, Payload payload, rmf_traffic::Duration loading_duration_estimate); + /// Get the pickup location + const Location& pickup_location() const; + + /// Change the pickup location + Description& pickup_location(Location new_location); + + /// Get the dispenser to pick up from + const std::string& from_dispenser() const; + + /// Change the dispenser to pick up from + Description& from_dispenser(std::string new_dispenser); + + /// Get the payload to pick up + const Payload& payload() const; + + /// Change the payload to pick up + Description& payload(Payload new_payload); + + /// Get the loading duration estimate + rmf_traffic::Duration loading_duration_estimate() const; + + /// Change the loading duration estimate + Description& loading_duration_estimate(rmf_traffic::Duration new_duration); + // Documentation inherited Phase::ConstModelPtr make_model( State invariant_initial_state, @@ -77,7 +105,8 @@ class PickUp::Description : public Phase::Description class Implementation; private: - rmf_utils::impl_ptr _pimpl; + Description(); + rmf_utils::unique_impl_ptr _pimpl; }; } // namespace phases diff --git a/rmf_task/include/rmf_task/sequence/phases/WaitFor.hpp b/rmf_task/include/rmf_task/sequence/phases/WaitFor.hpp new file mode 100644 index 00000000..4af5284c --- /dev/null +++ b/rmf_task/include/rmf_task/sequence/phases/WaitFor.hpp @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__SEQUENCE__PHASES__WAITFOR_HPP +#define RMF_TASK__SEQUENCE__PHASES__WAITFOR_HPP + +#include + +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +/// A WaitFor phase encompasses having the robot sit in place and wait for a +/// period of time to pass. +/// +/// The Model of this phase may be useful for calculating the Models of other +/// phases that include a period of time where the robot is waiting for a +/// process to finish. E.g. the PickUp and DropOff Models use WaitFor::Model to +/// calculate how much the robot's battery drains while waiting for the payload +/// to be transferred. +class WaitFor +{ +public: + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class WaitFor::Description : public Phase::Description +{ +public: + + /// Make a WaitFor phase description + /// + /// \param[in] wait_duration + /// The duration of time that the phase should be waiting. + static DescriptionPtr make(rmf_traffic::Duration wait_duration); + + /// Get the duration of the wait + rmf_traffic::Duration duration() const; + + /// Set the duration of the wait + Description& duration(rmf_traffic::Duration new_duration); + + // Documentation inherited + Phase::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + execute::Phase::ConstTagPtr make_tag( + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace phases +} // namespace sequence +} // namespace rmf_task + +#endif // RMF_TASK__SEQUENCE__PHASES__WAITFOR_HPP diff --git a/rmf_task/src/rmf_task/Payload.cpp b/rmf_task/src/rmf_task/Payload.cpp new file mode 100644 index 00000000..8a346adc --- /dev/null +++ b/rmf_task/src/rmf_task/Payload.cpp @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2021 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 +#include +#include + +#include + +namespace rmf_task { + +//============================================================================== +class Payload::Implementation +{ +public: + + std::vector components; + +}; + +//============================================================================== +Payload::Payload(std::vector components) +: _pimpl(rmf_utils::make_impl( + Implementation{std::move(components)})) +{ + // Do nothing +} + +//============================================================================== +auto Payload::components() const -> const std::vector& +{ + return _pimpl->components; +} + +//============================================================================== +std::string Payload::brief(const std::string& compartment_prefix) const +{ + std::unordered_map item_quantities; + std::unordered_set compartments; + + for (const auto& component : _pimpl->components) + { + auto it = item_quantities.insert({component.sku(), 0}).first; + it->second += component.quantity(); + + compartments.insert(component.compartment()); + } + + if (item_quantities.empty()) + return "nothing"; + + std::stringstream ss; + if (item_quantities.size() == 1) + { + const auto it = item_quantities.cbegin(); + const auto& type = it->first; + const auto quantity = it->second; + ss << quantity << " of [" << type << "]"; + } + else + { + const auto num_types = item_quantities.size(); + std::size_t total_quantity = 0; + for (const auto& item : item_quantities) + total_quantity += item.second; + + ss << num_types << " types of items (" << total_quantity << " total units)"; + } + + if (compartments.size() == 1) + { + const auto it = compartments.cbegin(); + ss << " " << compartment_prefix << " [" << *it << "]"; + } + else + { + const auto quantity = compartments.size(); + ss << " " << compartment_prefix << " " << quantity << " compartments"; + } + + return ss.str(); +} + +//============================================================================== +class Payload::Component::Implementation +{ +public: + + std::string sku; + uint32_t quantity; + std::string compartment; + +}; + +//============================================================================== +Payload::Component::Component( + std::string sku, + uint32_t quantity, + std::string compartment) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(sku), + quantity, + std::move(compartment) + })) +{ + // Do nothing +} + +//============================================================================== +const std::string& Payload::Component::sku() const +{ + return _pimpl->sku; +} + +//============================================================================== +uint32_t Payload::Component::quantity() const +{ + return _pimpl->quantity; +} + +//============================================================================== +const std::string& Payload::Component::compartment() const +{ + return _pimpl->compartment; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/sequence/Phase.cpp b/rmf_task/src/rmf_task/sequence/Phase.cpp new file mode 100644 index 00000000..3b211b2a --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/Phase.cpp @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { +namespace sequence { + +//============================================================================== +class Phase::SequenceModel::Implementation +{ +public: + std::vector models; + State invariant_finish_state; + rmf_traffic::Duration invariant_duration; +}; + +//============================================================================== +Phase::ConstModelPtr Phase::SequenceModel::make( + const std::vector& descriptions, + State invariant_initial_state, + const Parameters& parameters) +{ + std::vector models; + State invariant_finish_state = invariant_initial_state; + rmf_traffic::Duration invariant_duration = rmf_traffic::Duration(0); + for (const auto& desc : descriptions) + { + auto next_model = desc->make_model(invariant_finish_state, parameters); + invariant_finish_state = next_model->invariant_finish_state(); + invariant_duration += next_model->invariant_duration(); + + models.emplace_back(std::move(next_model)); + } + + auto output = std::shared_ptr(new SequenceModel); + output->_pimpl = rmf_utils::make_unique_impl( + Implementation{ + std::move(models), + std::move(invariant_finish_state), + invariant_duration + }); + + return output; +} + +//============================================================================== +std::optional Phase::SequenceModel::estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const +{ + std::optional finish_state = std::move(initial_state); + for (const auto& model : _pimpl->models) + { + finish_state = model->estimate_finish( + std::move(*finish_state), constraints, travel_estimator); + + if (!finish_state.has_value()) + return std::nullopt; + } + + return *finish_state; +} + +//============================================================================== +rmf_traffic::Duration Phase::SequenceModel::invariant_duration() const +{ + return _pimpl->invariant_duration; +} + +//============================================================================== +State Phase::SequenceModel::invariant_finish_state() const +{ + return _pimpl->invariant_finish_state; +} + +//============================================================================== +Phase::SequenceModel::SequenceModel() +{ + // Do nothing +} + +} // namespace sequence +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp b/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp index 7b3b895f..8ab547ef 100644 --- a/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp +++ b/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp @@ -173,11 +173,23 @@ class GoToPlace::Description::Implementation rmf_traffic::agv::Plan::Goal goal; + static std::string waypoint_name( + const std::size_t index, + const Parameters& parameters) + { + const auto& graph = parameters.planner()->get_configuration().graph(); + if (index < graph.num_waypoints()) + { + if (const auto* name = graph.get_waypoint(index).name()) + return *name; + } + + return "#" + std::to_string(index); + } }; //============================================================================== -auto GoToPlace::Description::make(Goal goal) --> std::shared_ptr +auto GoToPlace::Description::make(Goal goal) -> DescriptionPtr { auto desc = std::shared_ptr(new Description); desc->_pimpl = rmf_utils::make_impl( @@ -215,20 +227,12 @@ execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( if (graph.num_waypoints() <= start_wp) return nullptr; - auto get_wp_name = [&graph](std::size_t index) -> std::string - { - if (const auto* name = graph.get_waypoint(index).name()) - return *name; - - return "#" + std::to_string(index); - }; - - const auto start_name = get_wp_name(start_wp); + const auto start_name = Implementation::waypoint_name(start_wp, parameters); if (graph.num_waypoints() <= _pimpl->goal.waypoint()) return nullptr; - const auto goal_name = get_wp_name(_pimpl->goal.waypoint()); + const auto goal_name_ = goal_name(parameters); const auto estimate = estimate_duration( parameters.planner(), initial_state, _pimpl->goal); @@ -238,11 +242,31 @@ execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( return std::make_shared( id, - "Go to [" + goal_name + "]", - "Moving the robot from [" + start_name + "] to [" + goal_name + "]", + "Go to [" + goal_name_ + "]", + "Moving the robot from [" + start_name + "] to [" + goal_name_ + "]", *estimate); } +//============================================================================== +auto GoToPlace::Description::goal() const -> const Goal& +{ + return _pimpl->goal; +} + +//============================================================================== +auto GoToPlace::Description::goal(Goal new_goal) -> Description& +{ + _pimpl->goal = std::move(new_goal); + return *this; +} + +//============================================================================== +std::string GoToPlace::Description::goal_name( + const Parameters& parameters) const +{ + return Implementation::waypoint_name(_pimpl->goal.waypoint(), parameters); +} + //============================================================================== GoToPlace::Description::Description() { diff --git a/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp b/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp index 3e473414..c6f02e49 100644 --- a/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp +++ b/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp @@ -22,80 +22,45 @@ namespace sequence { namespace phases { //============================================================================== -Phase::ConstModelPtr PayloadTransfer::Model::make( - State invariant_initial_state, - const Parameters& parameters, - const Phase::ConstDescriptionPtr& go_to_place, - rmf_traffic::Duration transfer_duration) -{ - auto go_to_place_model = go_to_place->make_model( - std::move(invariant_initial_state), - parameters); - - if (!go_to_place_model) - return nullptr; - - const auto transfer_battery_drain = - parameters.ambient_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(transfer_duration)); - - return std::shared_ptr( - new Model( - std::move(go_to_place_model), - transfer_duration, - transfer_battery_drain)); -} - -//============================================================================== -std::optional PayloadTransfer::Model::estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const -{ - // TODO(MXG): Consider adding the changed Payload to the state - auto finish_state = - _go_to_place->estimate_finish(initial_state, constraints, travel_estimator); - - if (!finish_state.has_value()) - return std::nullopt; - - finish_state->time(finish_state->time().value() + _transfer_duration); - - if (constraints.drain_battery()) - { - finish_state->battery_soc( - finish_state->battery_soc().value() - _transfer_battery_drain); - } - - const auto battery_threshold = constraints.threshold_soc(); - if (finish_state->battery_soc().value() <= battery_threshold) - return std::nullopt; - - return finish_state; -} - -//============================================================================== -rmf_traffic::Duration PayloadTransfer::Model::invariant_duration() const +PayloadTransfer::PayloadTransfer( + Location location_, + std::string target_, + Payload payload_, + rmf_traffic::Duration loading_duration_estimate) + : target(std::move(target_)), + payload(std::move(payload_)), + go_to_place(GoToPlace::Description::make(std::move(location_))), + wait_for(WaitFor::Description::make(loading_duration_estimate)) { - return _transfer_duration; + descriptions = {go_to_place, wait_for}; } //============================================================================== -State PayloadTransfer::Model::invariant_finish_state() const +Phase::ConstModelPtr PayloadTransfer::make_model( + State invariant_initial_state, + const Parameters& parameters) const { - return _go_to_place->invariant_finish_state(); + return Phase::SequenceModel::make( + descriptions, + std::move(invariant_initial_state), + parameters); } //============================================================================== -PayloadTransfer::Model::Model( - Phase::ConstModelPtr go_to_place, - rmf_traffic::Duration transfer_duration, - double transfer_battery_drain) -: _go_to_place(std::move(go_to_place)), - _transfer_duration(transfer_duration), - _transfer_battery_drain(transfer_battery_drain) +execute::Phase::ConstTagPtr PayloadTransfer::make_tag( + const std::string& type, + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const { - // Do nothing + const auto model = make_model(initial_state, parameters); + + return std::make_shared( + id, + type, + type + " " + payload.brief("into") + " at " + + go_to_place->goal_name(parameters), + model->invariant_duration()); } } // namespace phases diff --git a/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp b/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp index feb743c5..728ef561 100644 --- a/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp +++ b/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp @@ -15,3 +15,119 @@ * */ +#include "internal_PayloadTransfer.hpp" + +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +class PickUp::Description::Implementation +{ +public: + PayloadTransfer transfer; +}; + +//============================================================================== +auto PickUp::Description::make( + Location pickup_location, + std::string from_dispenser, + Payload payload, + rmf_traffic::Duration loading_duration_estimate) -> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = rmf_utils::make_unique_impl( + Implementation{ + PayloadTransfer( + std::move(pickup_location), + std::move(from_dispenser), + std::move(payload), + loading_duration_estimate) + }); + + return output; +} + +//============================================================================== +auto PickUp::Description::pickup_location() const -> const Location& +{ + return _pimpl->transfer.go_to_place->goal(); +} + +//============================================================================== +auto PickUp::Description::pickup_location(Location new_location) -> Description& +{ + _pimpl->transfer.go_to_place->goal(std::move(new_location)); + return *this; +} + +//============================================================================== +const std::string& PickUp::Description::from_dispenser() const +{ + return _pimpl->transfer.target; +} + +//============================================================================== +auto PickUp::Description::from_dispenser(std::string new_dispenser) +-> Description& +{ + _pimpl->transfer.target = std::move(new_dispenser); + return *this; +} + +//============================================================================== +const Payload& PickUp::Description::payload() const +{ + return _pimpl->transfer.payload; +} + +//============================================================================== +auto PickUp::Description::payload(Payload new_payload) -> Description& +{ + _pimpl->transfer.payload = std::move(new_payload); + return *this; +} + +//============================================================================== +rmf_traffic::Duration PickUp::Description::loading_duration_estimate() const +{ + return _pimpl->transfer.wait_for->duration(); +} + +//============================================================================== +auto PickUp::Description::loading_duration_estimate( + const rmf_traffic::Duration new_duration) -> Description& +{ + _pimpl->transfer.wait_for->duration(new_duration); + return *this; +} + +//============================================================================== +Phase::ConstModelPtr PickUp::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.make_model( + std::move(invariant_initial_state), parameters); +} + +//============================================================================== +execute::Phase::ConstTagPtr PickUp::Description::make_tag( + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.make_tag("Pick up", id, initial_state, parameters); +} + +//============================================================================== +PickUp::Description::Description() +{ + // Do nothing +} + +} // namespace phases +} // namespace sequence +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/sequence/phases/WaitFor.cpp b/rmf_task/src/rmf_task/sequence/phases/WaitFor.cpp new file mode 100644 index 00000000..74c3eb8b --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/phases/WaitFor.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +class WaitFor::Model : public Phase::Model +{ +public: + + Model( + State invariant_initial_state, + rmf_traffic::Duration duration, + const Parameters& parameters); + + std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + rmf_traffic::Duration invariant_duration() const final; + + State invariant_finish_state() const final; + +private: + State _invariant_finish_state; + double _invariant_battery_drain; + rmf_traffic::Duration _duration; +}; + +//============================================================================== +class WaitFor::Description::Implementation +{ +public: + + rmf_traffic::Duration duration; + +}; + +//============================================================================== +auto WaitFor::Description::make(rmf_traffic::Duration wait_duration) +-> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = + rmf_utils::make_impl(Implementation{wait_duration}); + + return output; +} + +//============================================================================== +rmf_traffic::Duration WaitFor::Description::duration() const +{ + return _pimpl->duration; +} + +//============================================================================== +auto WaitFor::Description::duration(rmf_traffic::Duration new_duration) +-> Description& +{ + _pimpl->duration = new_duration; + return *this; +} + +//============================================================================== +Phase::ConstModelPtr WaitFor::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return std::make_shared( + invariant_initial_state, _pimpl->duration, parameters); +} + +//============================================================================== +execute::Phase::ConstTagPtr WaitFor::Description::make_tag( + execute::Phase::Tag::Id id, const State&, const Parameters&) const +{ + const auto seconds = std::chrono::duration_cast( + _pimpl->duration); + + return std::make_shared( + id, + "Waiting", + "Waiting for [" + std::to_string(seconds.count()) + "] seconds to elapse", + _pimpl->duration); +} + +//============================================================================== +WaitFor::Description::Description() +{ + // Do nothing +} + +//============================================================================== +WaitFor::Model::Model( + State invariant_initial_state, + rmf_traffic::Duration duration, + const Parameters& parameters) +: _invariant_finish_state(std::move(invariant_initial_state)), + _duration(duration) +{ + _invariant_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_duration)); +} + +//============================================================================== +std::optional WaitFor::Model::estimate_finish( + State state, + const Constraints& constraints, + const TravelEstimator&) const +{ + state.time(state.time().value() + _duration); + + if (constraints.drain_battery()) + state.battery_soc(state.battery_soc().value() - _invariant_battery_drain); + + if (state.battery_soc().value() <= _invariant_battery_drain) + return std::nullopt; + + return state; +} + +//============================================================================== +rmf_traffic::Duration WaitFor::Model::invariant_duration() const +{ + return _duration; +} + +//============================================================================== +State WaitFor::Model::invariant_finish_state() const +{ + return _invariant_finish_state; +} + +} // namespace phases +} // namespace sequence +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp b/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp index d8b2c81a..446a25be 100644 --- a/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp +++ b/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -36,40 +37,26 @@ class PayloadTransfer using Location = rmf_traffic::agv::Plan::Goal; std::string target; - rmf_traffic::Duration transfer_duration; - Phase::ConstDescriptionPtr go_to_place; - - class Model : public Phase::Model - { - public: - - static Phase::ConstModelPtr make( - State invariant_initial_state, - const Parameters& parameters, - const Phase::ConstDescriptionPtr& go_to_place, - rmf_traffic::Duration transfer_duration); - - std::optional estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const final; - - rmf_traffic::Duration invariant_duration() const final; - - State invariant_finish_state() const final; - - private: - - Model( - Phase::ConstModelPtr go_to_place, - rmf_traffic::Duration transfer_duration, - double transfer_battery_drain); - - Phase::ConstModelPtr _go_to_place; - rmf_traffic::Duration _transfer_duration; - double _transfer_battery_drain; - }; - + Payload payload; + GoToPlace::DescriptionPtr go_to_place; + WaitFor::DescriptionPtr wait_for; + std::vector descriptions; + + PayloadTransfer( + Location location_, + std::string target_, + Payload payload_, + rmf_traffic::Duration loading_duration_estimate); + + Phase::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const; + + execute::Phase::ConstTagPtr make_tag( + const std::string& type, + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const; }; } // namespace phases From 21054b9fac4087b9df93b4ba1672f8b65792758f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 10 Sep 2021 13:20:26 +0800 Subject: [PATCH 15/85] Implementing DropOff phase Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/execute/Task.hpp | 2 +- .../rmf_task/execute/TaskActivator.hpp | 2 +- rmf_task/include/rmf_task/sequence/Phase.hpp | 3 + rmf_task/include/rmf_task/sequence/Task.hpp | 51 +----- rmf_task/src/rmf_task/sequence/Task.cpp | 155 ++++++++++++++++++ .../src/rmf_task/sequence/phases/DropOff.cpp | 134 +++++++++++++++ 6 files changed, 299 insertions(+), 48 deletions(-) create mode 100644 rmf_task/src/rmf_task/sequence/Task.cpp create mode 100644 rmf_task/src/rmf_task/sequence/phases/DropOff.cpp diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/execute/Task.hpp index c95c365a..8d303aaf 100644 --- a/rmf_task/include/rmf_task/execute/Task.hpp +++ b/rmf_task/include/rmf_task/execute/Task.hpp @@ -150,7 +150,7 @@ class Task static Resume make_resumer(std::function callback); }; -using ConstTaskPtr = std::shared_ptr; +using TaskPtr = std::shared_ptr; //============================================================================== /// Basic static information about the task. diff --git a/rmf_task/include/rmf_task/execute/TaskActivator.hpp b/rmf_task/include/rmf_task/execute/TaskActivator.hpp index bd68fe09..5a2e9099 100644 --- a/rmf_task/include/rmf_task/execute/TaskActivator.hpp +++ b/rmf_task/include/rmf_task/execute/TaskActivator.hpp @@ -61,7 +61,7 @@ class TaskActivator template using Activate = std::function< - execute::ConstTaskPtr( + execute::TaskPtr( const Request::ConstTagPtr& request, const Description& description, std::optional backup_state, diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task/include/rmf_task/sequence/Phase.hpp index 61917780..c50c51fd 100644 --- a/rmf_task/include/rmf_task/sequence/Phase.hpp +++ b/rmf_task/include/rmf_task/sequence/Phase.hpp @@ -236,6 +236,9 @@ class Phase::Description const State& initial_state, const Parameters& parameters) const = 0; + /// Serialize this phase description into a string. + virtual std::string serialize() const = 0; + // Virtual destructor virtual ~Description() = default; }; diff --git a/rmf_task/include/rmf_task/sequence/Task.hpp b/rmf_task/include/rmf_task/sequence/Task.hpp index 087df748..6d31fdb9 100644 --- a/rmf_task/include/rmf_task/sequence/Task.hpp +++ b/rmf_task/include/rmf_task/sequence/Task.hpp @@ -20,6 +20,7 @@ #include #include +#include #include namespace rmf_task { @@ -35,7 +36,6 @@ class Task // Declaration class Active; - using ConstActivePtr = std::shared_ptr; // Declaration class Description; @@ -47,14 +47,10 @@ class Task /// Activate a phase sequence task /// - /// \param[in] activator + /// \param[in] phase_activator /// A phase activator - static ConstActivePtr activate( - std::shared_ptr activator, - const Description& description, - Update update, - PhaseFinished phase_finished, - TaskFinished task_finished); + static execute::TaskActivator::Activate make_activator( + Phase::ConstActivatorPtr phase_activator); }; @@ -87,43 +83,6 @@ class Task::Builder rmf_utils::impl_ptr _pimpl; }; -//============================================================================== -class Task::Active - : public execute::Task, - public std::enable_shared_from_this -{ -public: - - // Documentation inherited - const std::vector& - completed_phases() const final; - - // Documentation inherited - execute::Phase::ConstActivePtr active_phase() const final; - - // Documentation inherited - std::vector pending_phases() const final; - - // Documentation inherited - const Request::ConstTagPtr& tag() const final; - - // Documentation inherited - rmf_traffic::Time estimate_finish_time() const final; - - // Documentation inherited - Resume interrupt(std::function task_is_interrupted) final; - - // Documentation inherited - void cancel() final; - - // Documentation inherited - void kill() final; - - class Implementation; -private: - rmf_utils::unique_impl_ptr _pimpl; -}; - //============================================================================== class Task::Description : public Request::Description { @@ -149,7 +108,7 @@ class Task::Description::Model : public Request::Model std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const TravelEstimator& travel_estimator) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/src/rmf_task/sequence/Task.cpp b/rmf_task/src/rmf_task/sequence/Task.cpp new file mode 100644 index 00000000..83384ef1 --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/Task.cpp @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2021 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 + +#include + +namespace rmf_task { +namespace sequence { + +namespace { +//============================================================================== +struct Stage +{ + Phase:: + Phase::ConstDescriptionPtr description; + std::vector cancellation_sequence; +}; +} // anonymous namespace + +//============================================================================== +class Task::Builder::Implementation +{ +public: + std::vector stages; +}; + +//============================================================================== +class Task::Description::Implementation +{ +public: + + std::vector stages; + + static std::list get_stages(const Description& desc) + { + return std::list( + desc._pimpl->stages.begin(), + desc._pimpl->stages.end()); + } + +}; + +//============================================================================== +class Task::Active + : public execute::Task, + public std::enable_shared_from_this +{ +public: + + static execute::TaskPtr make( + Phase::ConstActivatorPtr phase_activator, + const Request::ConstTagPtr& tag, + const Description& description, + std::optional backup_state, + std::function update, + std::function phase_finished, + std::function task_finished) + { + auto task = std::shared_ptr( + new Active( + std::move(phase_activator), + tag, + description, + std::move(update), + std::move(phase_finished), + std::move(task_finished))); + + + } + + // Documentation inherited + const std::vector& + completed_phases() const final; + + // Documentation inherited + execute::Phase::ConstActivePtr active_phase() const final; + + // Documentation inherited + std::vector pending_phases() const final; + + // Documentation inherited + const Request::ConstTagPtr& tag() const final; + + // Documentation inherited + rmf_traffic::Time estimate_finish_time() const final; + + // Documentation inherited + Backup backup() const final; + + // Documentation inherited + Resume interrupt(std::function task_is_interrupted) final; + + // Documentation inherited + void cancel() final; + + // Documentation inherited + void kill() final; + + // Documentation inherited + void skip(uint64_t phase_id, bool value = true) final; + + // Documentation inherited + void rewind(uint64_t phase_id) final; + +private: + + Active( + Phase::ConstActivatorPtr phase_activator, + const Request::ConstTagPtr& request, + const Description& description, + std::function update, + std::function phase_finished, + std::function task_finished) + : _phase_activator(std::move(phase_activator)), + _tag(std::move(request)), + _remaining_stages(Description::Implementation::get_stages(description)), + _update(std::move(update)), + _phase_finished(std::move(phase_finished)), + _task_finished(std::move(task_finished)) + { + // Do nothing + } + + Phase::ConstActivatorPtr _phase_activator; + Request::ConstTagPtr _tag; + std::list _remaining_stages; + std::function _update; + std::function _phase_finished; + std::function _task_finished; +}; + +//============================================================================== +auto Task::make_activator(Phase::ConstActivatorPtr phase_activator) +-> execute::TaskActivator::Activate +{ + +} + +} // namespace sequence +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/sequence/phases/DropOff.cpp b/rmf_task/src/rmf_task/sequence/phases/DropOff.cpp new file mode 100644 index 00000000..baaff516 --- /dev/null +++ b/rmf_task/src/rmf_task/sequence/phases/DropOff.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2021 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_PayloadTransfer.hpp" + +#include + +namespace rmf_task { +namespace sequence { +namespace phases { + +//============================================================================== +class DropOff::Description::Implementation +{ +public: + PayloadTransfer transfer; +}; + +//============================================================================== +auto DropOff::Description::make( + Location drop_off_location, + std::string into_ingestor, + Payload payload, + rmf_traffic::Duration unloading_duration_estimate) -> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = rmf_utils::make_unique_impl( + Implementation{ + PayloadTransfer( + std::move(drop_off_location), + std::move(into_ingestor), + std::move(payload), + unloading_duration_estimate) + }); + + return output; +} + +//============================================================================== +auto DropOff::Description::drop_off_location() const -> const Location& +{ + return _pimpl->transfer.go_to_place->goal(); +} + +//============================================================================== +auto DropOff::Description::drop_off_location(Location new_location) +-> Description& +{ + _pimpl->transfer.go_to_place->goal(std::move(new_location)); + return *this; +} + +//============================================================================== +const std::string& DropOff::Description::into_ingestor() const +{ + return _pimpl->transfer.target; +} + +//============================================================================== +auto DropOff::Description::into_ingestor(std::string new_ingestor) +-> Description& +{ + _pimpl->transfer.target = std::move(new_ingestor); + return *this; +} + +//============================================================================== +const Payload& DropOff::Description::payload() const +{ + return _pimpl->transfer.payload; +} + +//============================================================================== +auto DropOff::Description::payload(Payload new_payload) -> Description& +{ + _pimpl->transfer.payload = std::move(new_payload); + return *this; +} + +//============================================================================== +rmf_traffic::Duration DropOff::Description::unloading_duration_estimate() const +{ + return _pimpl->transfer.wait_for->duration(); +} + +//============================================================================== +auto DropOff::Description::unloading_duration_estimate( + const rmf_traffic::Duration new_duration) -> Description& +{ + _pimpl->transfer.wait_for->duration(new_duration); + return *this; +} + +//============================================================================== +Phase::ConstModelPtr DropOff::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.make_model( + std::move(invariant_initial_state), parameters); +} + +//============================================================================== +execute::Phase::ConstTagPtr DropOff::Description::make_tag( + execute::Phase::Tag::Id id, + const State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.make_tag("Drop off", id, initial_state, parameters); +} + +//============================================================================== +DropOff::Description::Description() +{ + // Do nothing +} + +} // namespace phases +} // namespace sequence +} // namespace rmf_task From 3b83b5d24555e1791417f7674c1fca6720a11d48 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 10 Sep 2021 16:56:59 +0800 Subject: [PATCH 16/85] Reorganizing Signed-off-by: Michael X. Grey --- .../rmf_task/{execute => }/Condition.hpp | 4 +- .../include/rmf_task/{execute => }/Log.hpp | 2 - .../include/rmf_task/{execute => }/Phase.hpp | 4 +- rmf_task/include/rmf_task/Request.hpp | 112 +----------- .../include/rmf_task/{execute => }/Task.hpp | 169 ++++++++++++++---- .../rmf_task/requests/ChargeBattery.hpp | 6 +- rmf_task/include/rmf_task/requests/Clean.hpp | 6 +- .../include/rmf_task/requests/Delivery.hpp | 6 +- rmf_task/include/rmf_task/requests/Loop.hpp | 6 +- .../rmf_task/BinaryPriorityCostCalculator.cpp | 8 +- rmf_task/src/rmf_task/{execute => }/Phase.cpp | 5 +- rmf_task/src/rmf_task/Request.cpp | 65 +------ rmf_task/src/rmf_task/Task.cpp | 75 ++++++++ rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 8 +- .../rmf_task/agv/internal_task_planning.cpp | 13 +- .../rmf_task/agv/internal_task_planning.hpp | 8 +- .../src/rmf_task/requests/ChargeBattery.cpp | 6 +- rmf_task/src/rmf_task/requests/Clean.cpp | 6 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 6 +- rmf_task/src/rmf_task/requests/Loop.cpp | 6 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 32 ++-- rmf_task_sequence/CHANGELOG.md | 5 + rmf_task_sequence/CMakeLists.txt | 128 +++++++++++++ rmf_task_sequence/QUALITY_DECLARATION.md | 154 ++++++++++++++++ rmf_task_sequence/README.md | 7 + .../cmake/rmf_task_sequence-config.cmake.in | 14 ++ .../include/rmf_task_sequence}/Phase.hpp | 12 +- .../include/rmf_task_sequence}/Task.hpp | 0 .../rmf_task_sequence}/phases/DropOff.hpp | 0 .../rmf_task_sequence}/phases/GoToPlace.hpp | 0 .../rmf_task_sequence}/phases/PickUp.hpp | 0 .../rmf_task_sequence}/phases/WaitFor.hpp | 0 rmf_task_sequence/package.xml | 21 +++ .../src/rmf_task_sequence}/Phase.cpp | 8 +- .../src/rmf_task_sequence}/Task.cpp | 0 .../src/rmf_task_sequence}/phases/DropOff.cpp | 0 .../rmf_task_sequence}/phases/GoToPlace.cpp | 0 .../phases/PayloadTransfer.cpp | 0 .../src/rmf_task_sequence}/phases/PickUp.cpp | 0 .../src/rmf_task_sequence}/phases/WaitFor.cpp | 0 .../phases/internal_PayloadTransfer.hpp | 0 rmf_task_sequence/test/main.cpp | 21 +++ 42 files changed, 648 insertions(+), 275 deletions(-) rename rmf_task/include/rmf_task/{execute => }/Condition.hpp (98%) rename rmf_task/include/rmf_task/{execute => }/Log.hpp (99%) rename rmf_task/include/rmf_task/{execute => }/Phase.hpp (98%) rename rmf_task/include/rmf_task/{execute => }/Task.hpp (65%) rename rmf_task/src/rmf_task/{execute => }/Phase.cpp (95%) create mode 100644 rmf_task/src/rmf_task/Task.cpp create mode 100644 rmf_task_sequence/CHANGELOG.md create mode 100644 rmf_task_sequence/CMakeLists.txt create mode 100644 rmf_task_sequence/QUALITY_DECLARATION.md create mode 100644 rmf_task_sequence/README.md create mode 100644 rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in rename {rmf_task/include/rmf_task/sequence => rmf_task_sequence/include/rmf_task_sequence}/Phase.hpp (98%) rename {rmf_task/include/rmf_task/sequence => rmf_task_sequence/include/rmf_task_sequence}/Task.hpp (100%) rename {rmf_task/include/rmf_task/sequence => rmf_task_sequence/include/rmf_task_sequence}/phases/DropOff.hpp (100%) rename {rmf_task/include/rmf_task/sequence => rmf_task_sequence/include/rmf_task_sequence}/phases/GoToPlace.hpp (100%) rename {rmf_task/include/rmf_task/sequence => rmf_task_sequence/include/rmf_task_sequence}/phases/PickUp.hpp (100%) rename {rmf_task/include/rmf_task/sequence => rmf_task_sequence/include/rmf_task_sequence}/phases/WaitFor.hpp (100%) create mode 100644 rmf_task_sequence/package.xml rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/Phase.cpp (95%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/Task.cpp (100%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/phases/DropOff.cpp (100%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/phases/GoToPlace.cpp (100%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/phases/PayloadTransfer.cpp (100%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/phases/PickUp.cpp (100%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/phases/WaitFor.cpp (100%) rename {rmf_task/src/rmf_task/sequence => rmf_task_sequence/src/rmf_task_sequence}/phases/internal_PayloadTransfer.hpp (100%) create mode 100644 rmf_task_sequence/test/main.cpp diff --git a/rmf_task/include/rmf_task/execute/Condition.hpp b/rmf_task/include/rmf_task/Condition.hpp similarity index 98% rename from rmf_task/include/rmf_task/execute/Condition.hpp rename to rmf_task/include/rmf_task/Condition.hpp index 9e7cc546..921793e2 100644 --- a/rmf_task/include/rmf_task/execute/Condition.hpp +++ b/rmf_task/include/rmf_task/Condition.hpp @@ -18,7 +18,7 @@ #ifndef RMF_TASK__EXECUTE__CONDITION_HPP #define RMF_TASK__EXECUTE__CONDITION_HPP -#include +#include #include @@ -28,7 +28,6 @@ #include namespace rmf_task { -namespace execute { class Condition; using ConstConditionPtr = std::shared_ptr; @@ -141,7 +140,6 @@ class Condition::Snapshot : public Condition rmf_utils::impl_ptr _pimpl; }; -} // namespace execute } // namespace rmf_task #endif // RMF_TASK__EXECUTE__CONDITION_HPP diff --git a/rmf_task/include/rmf_task/execute/Log.hpp b/rmf_task/include/rmf_task/Log.hpp similarity index 99% rename from rmf_task/include/rmf_task/execute/Log.hpp rename to rmf_task/include/rmf_task/Log.hpp index 99833aa6..ce110a30 100644 --- a/rmf_task/include/rmf_task/execute/Log.hpp +++ b/rmf_task/include/rmf_task/Log.hpp @@ -25,7 +25,6 @@ #include namespace rmf_task { -namespace execute { class Log; using ConstLogPtr = std::shared_ptr; @@ -207,7 +206,6 @@ class Log::Reader::Iterable::iterator rmf_utils::impl_ptr _pimpl; }; -} // namespace execute } // namespace rmf_task #endif // RMF_TASK__EXECUTE__LOG_HPP diff --git a/rmf_task/include/rmf_task/execute/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp similarity index 98% rename from rmf_task/include/rmf_task/execute/Phase.hpp rename to rmf_task/include/rmf_task/Phase.hpp index 7c976000..14257967 100644 --- a/rmf_task/include/rmf_task/execute/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -18,7 +18,7 @@ #ifndef RMF_TASK__EXECUTE__PHASE_HPP #define RMF_TASK__EXECUTE__PHASE_HPP -#include +#include #include @@ -27,7 +27,6 @@ #include namespace rmf_task { -namespace execute { //============================================================================== class Phase @@ -175,7 +174,6 @@ class Phase::Pending rmf_utils::impl_ptr _pimpl; }; -} // namespace execute } // namespace rmf_task #endif // RMF_TASK__EXECUTE__PHASE_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index b9014a51..d48302fc 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -20,13 +20,7 @@ #include -#include -#include -#include -#include -#include - -#include +#include #include @@ -36,94 +30,6 @@ namespace rmf_task { class Request { public: - - class Tag - { - public: - - /// Constructor - /// - /// \param[in] id_ - /// The identify of the request - /// - /// \param[in] earliest_start_time_ - /// The earliest time that the request may begin - /// - /// \param[in] priority_ - /// The priority of the request - /// - /// \param[in] automatic_ - /// Whether this request was automatically generated - Tag( - std::string id_, - rmf_traffic::Time earliest_start_time_, - ConstPriorityPtr priority_, - bool automatic_ = false); - - /// The unique id for this request - const std::string& id() const; - - /// Get the earliest time that this request may begin - rmf_traffic::Time earliest_start_time() const; - - /// Get the priority of this request - ConstPriorityPtr priority() const; - - // Returns true if this request was automatically generated - bool automatic() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - using ConstTagPtr = std::shared_ptr; - - /// An abstract interface for computing the estimate and invariant durations - /// of this request - class Model - { - public: - - /// Estimate the state of the robot when the request is finished along with - /// the time the robot has to wait before commencing the request - virtual std::optional estimate_finish( - const State& initial_state, - const Constraints& task_planning_constraints, - const TravelEstimator& travel_estimator) const = 0; - - /// Estimate the invariant component of the request's duration - virtual rmf_traffic::Duration invariant_duration() const = 0; - - virtual ~Model() = default; - }; - - /// An abstract interface to define the specifics of this request. This - /// implemented description will differentiate this request from others. - class Description - { - public: - - /// Generate a Model for this request based on the unique traits of this - /// description - /// - /// \param[in] earliest_start_time - /// The earliest time this request should begin execution. This is usually - /// the requested start time for the request. - /// - /// \param[in] parameters - /// The parameters that describe this AGV - virtual std::shared_ptr make_model( - rmf_traffic::Time earliest_start_time, - const Parameters& parameters) const = 0; - - // Virtual destructor - virtual ~Description() = default; - }; - - using DescriptionPtr = std::shared_ptr; - using ConstDescriptionPtr = std::shared_ptr; - /// Constructor /// /// \param[in] earliest_start_time @@ -144,23 +50,25 @@ class Request const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - ConstDescriptionPtr description, + Task::ConstDescriptionPtr description, bool automatic = false); /// Constructor /// - /// \param[in] tag - /// Tag of the request + /// \param[in] booking + /// Booking information for this request /// /// \param[in] description /// Description for this request - Request(ConstTagPtr tag, ConstDescriptionPtr description); + Request( + Task::ConstBookingPtr booking, + Task::ConstDescriptionPtr description); /// Get the tag of this request - const ConstTagPtr& tag() const; + const Task::ConstBookingPtr& booking() const; /// Get the description of this request - const ConstDescriptionPtr& description() const; + const Task::ConstDescriptionPtr& description() const; class Implementation; private: @@ -169,8 +77,6 @@ class Request using RequestPtr = std::shared_ptr; using ConstRequestPtr = std::shared_ptr; -using DescriptionPtr = Request::DescriptionPtr; -using ConstDescriptionPtr = std::shared_ptr; } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/execute/Task.hpp b/rmf_task/include/rmf_task/Task.hpp similarity index 65% rename from rmf_task/include/rmf_task/execute/Task.hpp rename to rmf_task/include/rmf_task/Task.hpp index 8d303aaf..c41b7dda 100644 --- a/rmf_task/include/rmf_task/execute/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -18,16 +18,21 @@ #ifndef RMF_TASK__EXECUTE__TASK_HPP #define RMF_TASK__EXECUTE__TASK_HPP -#include +#include #include #include -#include +#include +#include +#include +#include +#include + +#include #include #include namespace rmf_task { -namespace execute { //============================================================================== /// Pure abstract interface for an executable Task @@ -36,8 +41,134 @@ class Task public: // Declarations + class Booking; + using ConstBookingPtr = std::shared_ptr; + class Tag; + using ConstTagPtr = std::shared_ptr; + + class Model; + using ConstModelPtr = std::shared_ptr; + + class Description; + using ConstDescriptionPtr = std::shared_ptr; + + class Active; + using ActivePtr = std::shared_ptr; +}; + +//============================================================================== +class Task::Booking +{ +public: + + /// Constructor + /// + /// \param[in] id_ + /// The identify of the request + /// + /// \param[in] earliest_start_time_ + /// The earliest time that the request may begin + /// + /// \param[in] priority_ + /// The priority of the request + /// + /// \param[in] automatic_ + /// Whether this request was automatically generated + Booking( + std::string id_, + rmf_traffic::Time earliest_start_time_, + ConstPriorityPtr priority_, + bool automatic_ = false); + + /// The unique id for this request + const std::string& id() const; + + /// Get the earliest time that this request may begin + rmf_traffic::Time earliest_start_time() const; + + /// Get the priority of this request + ConstPriorityPtr priority() const; + + // Returns true if this request was automatically generated + bool automatic() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// Basic static information about the task. +class Task::Tag +{ +public: + + /// The original request that this Task is carrying out + const ConstBookingPtr& booking() const; + + /// The category of this Task. + const std::string& category() const; + + /// Details about this Task. + const std::string& detail() const; + + /// The original finish estimate of this Task. + rmf_traffic::Time original_finish_estimate() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// An abstract interface for computing the estimate and invariant durations +/// of this request +class Task::Model +{ +public: + + /// Estimate the state of the robot when the request is finished along with + /// the time the robot has to wait before commencing the request + virtual std::optional estimate_finish( + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) const = 0; + /// Estimate the invariant component of the request's duration + virtual rmf_traffic::Duration invariant_duration() const = 0; + + virtual ~Model() = default; +}; + +//============================================================================== +/// An abstract interface to define the specifics of this request. This +/// implemented description will differentiate this request from others. +class Task::Description +{ +public: + + /// Generate a Model for this request based on the unique traits of this + /// description + /// + /// \param[in] earliest_start_time + /// The earliest time this request should begin execution. This is usually + /// the requested start time for the request. + /// + /// \param[in] parameters + /// The parameters that describe this AGV + virtual ConstModelPtr make_model( + rmf_traffic::Time earliest_start_time, + const Parameters& parameters) const = 0; + + // Virtual destructor + virtual ~Description() = default; +}; + +//============================================================================== +class Task::Active +{ +public: /// Backup data for the task. The state of the task is represented by a /// string. The meaning and format of the string is up to the Task /// implementation to decide. @@ -57,8 +188,8 @@ class Task /// Descriptions of the phases that are expected in the future virtual std::vector pending_phases() const = 0; - /// The request tag of this Task - virtual const Request::ConstTagPtr& tag() const = 0; + /// The tag of this Task + virtual const ConstTagPtr& tag() const = 0; /// Estimate the overall finishing time of the task virtual rmf_traffic::Time estimate_finish_time() const = 0; @@ -138,7 +269,7 @@ class Task virtual void rewind(uint64_t phase_id) = 0; // Virtual destructor - virtual ~Task() = default; + virtual ~Active() = default; protected: @@ -150,32 +281,6 @@ class Task static Resume make_resumer(std::function callback); }; -using TaskPtr = std::shared_ptr; - -//============================================================================== -/// Basic static information about the task. -class Task::Tag -{ -public: - - /// The original request that this Task is carrying out - const Request::ConstTagPtr& request() const; - - /// The category of this Task. - const std::string& category() const; - - /// Details about this Task. - const std::string& detail() const; - - /// The original finish estimate of this Task. - rmf_traffic::Time original_finish_estimate() const; - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; - -} // namespace execute } // namespace rmf_task #endif // RMF_TASK__TASK_HPP diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 45a0822e..a8b09dd6 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -46,15 +46,15 @@ class ChargeBattery // Forward declare the model for this request class Model; - class Description : public Request::Description + class Description : public Task::Description { public: /// Generate the description for this request - static DescriptionPtr make(); + static Task::ConstDescriptionPtr make(); /// Documentation inherited - std::shared_ptr make_model( + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 1ed42b83..bac6ec03 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -46,18 +46,18 @@ class Clean // Forward declare the model for this request class Model; - class Description : public Request::Description + class Description : public Task::Description { public: /// Generate the description for this request - static DescriptionPtr make( + static Task::ConstDescriptionPtr make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path); /// Documentation inherited - std::shared_ptr make_model( + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 758eece4..352086be 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -45,21 +45,21 @@ class Delivery // Forward declare the Model for this request class Model; - class Description : public Request::Description + class Description : public Task::Description { public: using Start = rmf_traffic::agv::Planner::Start; /// Generate the description for this request - static DescriptionPtr make( + static Task::ConstDescriptionPtr make( std::size_t pickup_waypoint, rmf_traffic::Duration pickup_duration, std::size_t dropoff_waypoint, rmf_traffic::Duration dropoff_duration); /// Documentation inherited - std::shared_ptr make_model( + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index b9e31735..633e5481 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -47,18 +47,18 @@ class Loop // Forward declare the Model for this request class Model; - class Description : public Request::Description + class Description : public Task::Description { public: /// Generate the description for this request - static DescriptionPtr make( + static Task::ConstDescriptionPtr make( std::size_t start_waypoint, std::size_t finish_waypoint, std::size_t num_loops); /// Documentation inherited - std::shared_ptr make_model( + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; diff --git a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp index a9749fa8..c3e523b7 100644 --- a/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp +++ b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp @@ -35,7 +35,7 @@ auto BinaryPriorityCostCalculator::compute_g_assignment( return rmf_traffic::time::to_seconds( assignment.finish_state().time().value() - - assignment.request()->tag()->earliest_start_time()); + - assignment.request()->booking()->earliest_start_time()); } //============================================================================== @@ -140,7 +140,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( const auto& assignments = node.assigned_tasks[i]; for (const auto& a : assignments) { - if (a.assignment.request()->tag()->priority() != nullptr) + if (a.assignment.request()->booking()->priority() != nullptr) priority_count[i] += 1; } } @@ -175,7 +175,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( return true; } - auto prev_priority = it->assignment.request()->tag()->priority(); + auto prev_priority = it->assignment.request()->booking()->priority(); ++it; for (; it != agent.end(); ++it) { @@ -183,7 +183,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( const rmf_task::requests::ChargeBattery::Description>( it->assignment.request()->description())) continue; - auto curr_priority = it->assignment.request()->tag()->priority(); + auto curr_priority = it->assignment.request()->booking()->priority(); if ((prev_priority == nullptr) && (curr_priority != nullptr)) return false; diff --git a/rmf_task/src/rmf_task/execute/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp similarity index 95% rename from rmf_task/src/rmf_task/execute/Phase.cpp rename to rmf_task/src/rmf_task/Phase.cpp index 7d15596d..f6eb0b59 100644 --- a/rmf_task/src/rmf_task/execute/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -15,10 +15,9 @@ * */ -#include +#include namespace rmf_task { -namespace execute { //============================================================================== class Phase::Tag::Implementation @@ -73,6 +72,4 @@ rmf_traffic::Duration Phase::Tag::original_duration_estimate() const return _pimpl->duration; } - -} // namespace execute } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Request.cpp b/rmf_task/src/rmf_task/Request.cpp index 67d420e1..9ffc5687 100644 --- a/rmf_task/src/rmf_task/Request.cpp +++ b/rmf_task/src/rmf_task/Request.cpp @@ -23,8 +23,8 @@ namespace rmf_task { class Request::Implementation { public: - ConstTagPtr tag; - ConstDescriptionPtr description; + Task::ConstBookingPtr booking; + Task::ConstDescriptionPtr description; }; //============================================================================== @@ -32,11 +32,11 @@ Request::Request( const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - ConstDescriptionPtr description, + Task::ConstDescriptionPtr description, bool automatic) : _pimpl(rmf_utils::make_impl( Implementation { - std::make_shared( + std::make_shared( id, earliest_start_time, std::move(priority), automatic), std::move(description) })) @@ -45,64 +45,13 @@ Request::Request( } //============================================================================== -class Request::Tag::Implementation +const Task::ConstBookingPtr& Request::booking() const { -public: - std::string id; - rmf_traffic::Time earliest_start_time; - rmf_task::ConstPriorityPtr priority; - bool automatic; -}; - -//============================================================================== -Request::Tag::Tag( - std::string id_, - rmf_traffic::Time earliest_start_time_, - ConstPriorityPtr priority_, - bool automatic_) -: _pimpl(rmf_utils::make_impl( - Implementation{ - std::move(id_), - earliest_start_time_, - std::move(priority_), - automatic_ - })) -{ - // Do nothing -} - -//============================================================================== -const std::string& Request::Tag::id() const -{ - return _pimpl->id; -} - -//============================================================================== -rmf_traffic::Time Request::Tag::earliest_start_time() const -{ - return _pimpl->earliest_start_time; -} - -//============================================================================== -ConstPriorityPtr Request::Tag::priority() const -{ - return _pimpl->priority; -} - -//============================================================================== -bool Request::Tag::automatic() const -{ - return _pimpl->automatic; -} - -//============================================================================== -const Request::ConstTagPtr& Request::tag() const -{ - return _pimpl->tag; + return _pimpl->booking; } //============================================================================== -const Request::ConstDescriptionPtr& Request::description() const +const Task::ConstDescriptionPtr& Request::description() const { return _pimpl->description; } diff --git a/rmf_task/src/rmf_task/Task.cpp b/rmf_task/src/rmf_task/Task.cpp new file mode 100644 index 00000000..9dadbadc --- /dev/null +++ b/rmf_task/src/rmf_task/Task.cpp @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { + + +//============================================================================== +class Task::Booking::Implementation +{ +public: + std::string id; + rmf_traffic::Time earliest_start_time; + rmf_task::ConstPriorityPtr priority; + bool automatic; +}; + +//============================================================================== +Task::Booking::Booking( + std::string id_, + rmf_traffic::Time earliest_start_time_, + ConstPriorityPtr priority_, + bool automatic_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(id_), + earliest_start_time_, + std::move(priority_), + automatic_ + })) +{ + // Do nothing +} + +//============================================================================== +const std::string& Task::Booking::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_traffic::Time Task::Booking::earliest_start_time() const +{ + return _pimpl->earliest_start_time; +} + +//============================================================================== +ConstPriorityPtr Task::Booking::priority() const +{ + return _pimpl->priority; +} + +//============================================================================== +bool Task::Booking::automatic() const +{ + return _pimpl->automatic; +} + + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d3c7af23..d60d6522 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -513,7 +513,7 @@ class TaskPlanner::Implementation // If so the cost function for a node will be modified accordingly. for (const auto& request : requests) { - if (request->tag()->priority()) + if (request->booking()->priority()) { check_priority = true; break; @@ -722,7 +722,7 @@ class TaskPlanner::Implementation entry.previous_state.time().value()); const auto charge_battery_model = charge_battery->description()->make_model( - charge_battery->tag()->earliest_start_time(), + charge_battery->booking()->earliest_start_time(), config.parameters()); auto battery_estimate = charge_battery_model->estimate_finish( entry.previous_state, constraints, *travel_estimator); @@ -801,7 +801,7 @@ class TaskPlanner::Implementation const auto charge_battery_model = charge_battery->description()->make_model( - charge_battery->tag()->earliest_start_time(), + charge_battery->booking()->earliest_start_time(), config.parameters()); auto battery_estimate = charge_battery_model->estimate_finish( entry.state, constraints, *travel_estimator); @@ -887,7 +887,7 @@ class TaskPlanner::Implementation auto charge_battery = make_charging_request(state.time().value()); const auto charge_battery_model = charge_battery->description()->make_model( - charge_battery->tag()->earliest_start_time(), + charge_battery->booking()->earliest_start_time(), config.parameters()); auto estimate = charge_battery_model->estimate_finish( state, config.constraints(), *travel_estimator); diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp index 16ee6d47..f4e0cb4d 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.cpp @@ -100,7 +100,7 @@ std::shared_ptr Candidates::make( const std::vector& initial_states, const Constraints& constraints, const Parameters& parameters, - const Request::Model& request_model, + const Task::Model& task_model, const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error) { @@ -108,7 +108,7 @@ std::shared_ptr Candidates::make( for (std::size_t i = 0; i < initial_states.size(); ++i) { const auto& state = initial_states[i]; - const auto finish = request_model.estimate_finish( + const auto finish = task_model.estimate_finish( state, constraints, travel_estimator); if (finish.has_value()) { @@ -133,7 +133,7 @@ std::shared_ptr Candidates::make( state, constraints, travel_estimator); if (battery_estimate.has_value()) { - auto new_finish = request_model.estimate_finish( + auto new_finish = task_model.estimate_finish( battery_estimate.value().finish_state(), constraints, travel_estimator); @@ -178,9 +178,8 @@ std::shared_ptr Candidates::make( } // ============================================================================ -PendingTask::PendingTask( - ConstRequestPtr request_, - std::shared_ptr model_, +PendingTask::PendingTask(ConstRequestPtr request_, + Task::ConstModelPtr model_, Candidates candidates_) : request(std::move(request_)), model(std::move(model_)), @@ -201,7 +200,7 @@ std::shared_ptr PendingTask::make( { const auto earliest_start_time = std::max( start_time, - request_->tag()->earliest_start_time()); + request_->booking()->earliest_start_time()); const auto model = request_->description()->make_model( earliest_start_time, parameters); diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp index a7371462..cbc0c96c 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp +++ b/rmf_task/src/rmf_task/agv/internal_task_planning.hpp @@ -75,7 +75,7 @@ class Candidates const std::vector& initial_states, const Constraints& constraints, const Parameters& parameters, - const Request::Model& request_model, + const Task::Model& task_model, const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error); @@ -120,13 +120,13 @@ class PendingTask TaskPlanner::TaskPlannerError& error); rmf_task::ConstRequestPtr request; - std::shared_ptr model; + Task::ConstModelPtr model; Candidates candidates; private: PendingTask( ConstRequestPtr request_, - std::shared_ptr model_, + Task::ConstModelPtr model_, Candidates candidates_); }; @@ -163,7 +163,7 @@ struct Node for (const auto& u : unassigned_tasks) { double earliest_start_time = rmf_traffic::time::to_seconds( - u.second.request->tag()->earliest_start_time().time_since_epoch()); + u.second.request->booking()->earliest_start_time().time_since_epoch()); const auto invariant_duration = u.second.model->invariant_duration(); double earliest_finish_time = earliest_start_time diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index a4465434..6bf7f0b4 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -47,7 +47,7 @@ std::string generate_uuid(const std::size_t length = 3) //============================================================================== // Definition for forward declared class -class ChargeBattery::Model : public Request::Model +class ChargeBattery::Model : public Task::Model { public: @@ -166,7 +166,7 @@ class ChargeBattery::Description::Implementation }; //============================================================================== -rmf_task::DescriptionPtr ChargeBattery::Description::make() +Task::ConstDescriptionPtr ChargeBattery::Description::make() { std::shared_ptr description( new Description()); @@ -181,7 +181,7 @@ ChargeBattery::Description::Description() } //============================================================================== -std::shared_ptr ChargeBattery::Description::make_model( +Task::ConstModelPtr ChargeBattery::Description::make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const { diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 0f393943..5529808d 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -23,7 +23,7 @@ namespace rmf_task { namespace requests { //============================================================================== -class Clean::Model : public Request::Model +class Clean::Model : public Task::Model { public: @@ -205,7 +205,7 @@ class Clean::Description::Implementation }; //============================================================================== -rmf_task::DescriptionPtr Clean::Description::make( +Task::ConstDescriptionPtr Clean::Description::make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path) @@ -226,7 +226,7 @@ Clean::Description::Description() } //============================================================================== -std::shared_ptr Clean::Description::make_model( +Task::ConstModelPtr Clean::Description::make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const { diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 9fa299a5..5420e9f7 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -23,7 +23,7 @@ namespace rmf_task { namespace requests { //============================================================================== -class Delivery::Model : public Request::Model +class Delivery::Model : public Task::Model { public: @@ -218,7 +218,7 @@ class Delivery::Description::Implementation }; //============================================================================== -rmf_task::DescriptionPtr Delivery::Description::make( +Task::ConstDescriptionPtr Delivery::Description::make( std::size_t pickup_waypoint, rmf_traffic::Duration pickup_wait, std::size_t dropoff_waypoint, @@ -241,7 +241,7 @@ Delivery::Description::Description() } //============================================================================== -std::shared_ptr Delivery::Description::make_model( +Task::ConstModelPtr Delivery::Description::make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const { diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index eb16f31c..0508e9ca 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -21,7 +21,7 @@ namespace rmf_task { namespace requests { //============================================================================== -class Loop::Model : public Request::Model +class Loop::Model : public Task::Model { public: @@ -220,7 +220,7 @@ class Loop::Description::Implementation }; //============================================================================== -DescriptionPtr Loop::Description::make( +Task::ConstDescriptionPtr Loop::Description::make( std::size_t start_waypoint, std::size_t finish_waypoint, std::size_t num_loops) @@ -241,7 +241,7 @@ Loop::Description::Description() } //============================================================================== -std::shared_ptr Loop::Description::make_model( +Task::ConstModelPtr Loop::Description::make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const { diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 1798ee12..2f728db2 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -116,13 +116,15 @@ inline void display_solution( { const auto& s = a.finish_state(); const double request_seconds = - a.request()->tag()->earliest_start_time().time_since_epoch().count(); + a.request()->booking()->earliest_start_time() + .time_since_epoch().count(); + const double start_seconds = a.deployment_time().time_since_epoch().count(); const rmf_traffic::Time finish_time = s.time().value(); const double finish_seconds = finish_time.time_since_epoch().count(); std::cout << std::fixed - << " <" << a.request()->tag()->id() << ": " + << " <" << a.request()->booking()->id() << ": " << request_seconds << ", " << start_seconds << ", "<< finish_seconds << ", " << 100*s.battery_soc().value() << "%>" << std::endl; @@ -860,7 +862,7 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the back of the assignment queue - CHECK(optimal_assignments.front().back().request()->tag()->id() == "3"); + CHECK(optimal_assignments.front().back().request()->booking()->id() == "3"); THEN("When replanning with high priority for request with task_id:3") { @@ -896,7 +898,7 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the front of the assignment queue - CHECK(new_optimal_assignments.front().front().request()->tag()->id() == + CHECK(new_optimal_assignments.front().front().request()->booking()->id() == "3"); } @@ -963,7 +965,7 @@ SCENARIO("Grid World") } // We expect request with task_id:3 to be at the back of the assignment queue - CHECK(optimal_assignments.front().back().request()->tag()->id() == "3"); + CHECK(optimal_assignments.front().back().request()->booking()->id() == "3"); } WHEN("Planning for 1 robot, two high priority and two low priority tasks") @@ -1040,7 +1042,7 @@ SCENARIO("Grid World") const auto& assignments = optimal_assignments.front(); std::unordered_map index_map = {}; for (std::size_t i = 0; i < assignments.size(); ++i) - index_map.insert({assignments[i].request()->tag()->id(), i}); + index_map.insert({assignments[i].request()->booking()->id(), i}); CHECK(index_map["1"] < index_map["2"]); CHECK(index_map["1"] < index_map["3"]); CHECK(index_map["4"] < index_map["2"]); @@ -1122,7 +1124,7 @@ SCENARIO("Grid World") const auto& assignments = optimal_assignments.front(); std::unordered_map index_map = {}; for (std::size_t i = 0; i < assignments.size(); ++i) - index_map.insert({assignments[i].request()->tag()->id(), i}); + index_map.insert({assignments[i].request()->booking()->id(), i}); CHECK(index_map["1"] < index_map["2"]); CHECK(index_map["1"] < index_map["3"]); CHECK(index_map["1"] < index_map["4"]); @@ -1203,8 +1205,8 @@ SCENARIO("Grid World") REQUIRE(optimal_assignments.size() == 2); const auto& agent_0_assignments = optimal_assignments[0]; const auto& agent_1_assignments = optimal_assignments[1]; - CHECK(agent_0_assignments.front().request()->tag()->id() == "2"); - CHECK(agent_1_assignments.front().request()->tag()->id() == "1"); + CHECK(agent_0_assignments.front().request()->booking()->id() == "2"); + CHECK(agent_1_assignments.front().request()->booking()->id() == "1"); THEN("When task 3 & 4 are assigned high priority") { @@ -1268,8 +1270,8 @@ SCENARIO("Grid World") REQUIRE(optimal_assignments.size() == 2); const auto& agent_0_assignments = optimal_assignments[0]; const auto& agent_1_assignments = optimal_assignments[1]; - CHECK(agent_0_assignments.front().request()->tag()->id() == "4"); - CHECK(agent_1_assignments.front().request()->tag()->id() == "3"); + CHECK(agent_0_assignments.front().request()->booking()->id() == "4"); + CHECK(agent_1_assignments.front().request()->booking()->id() == "3"); } } @@ -1349,7 +1351,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { if (!agent.empty()) - first_assignments.push_back(agent.front().request()->tag()->id()); + first_assignments.push_back(agent.front().request()->booking()->id()); } std::size_t id_count = 0; for (const auto& id : first_assignments) @@ -1423,7 +1425,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { if (!agent.empty()) - first_assignments.push_back(agent.front().request()->tag()->id()); + first_assignments.push_back(agent.front().request()->booking()->id()); } std::size_t id_count = 0; for (const auto& id : first_assignments) @@ -1860,7 +1862,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { const auto last_assignment = agent.back(); - CHECK_FALSE(last_assignment.request()->tag()->automatic()); + CHECK_FALSE(last_assignment.request()->booking()->automatic()); const auto& state = last_assignment.finish_state(); CHECK_FALSE(state.waypoint() == state.dedicated_charging_waypoint()); } @@ -1896,7 +1898,7 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { const auto last_assignment = agent.back(); - CHECK(last_assignment.request()->tag()->automatic()); + CHECK(last_assignment.request()->booking()->automatic()); const auto& state = last_assignment.finish_state(); CHECK(state.waypoint() == state.dedicated_charging_waypoint()); } diff --git a/rmf_task_sequence/CHANGELOG.md b/rmf_task_sequence/CHANGELOG.md new file mode 100644 index 00000000..464bd52f --- /dev/null +++ b/rmf_task_sequence/CHANGELOG.md @@ -0,0 +1,5 @@ +## Changelog for package rmf_task_sequence + +2.0.0 (2021-XX-YY) +------------------ +* First release diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt new file mode 100644 index 00000000..0a08f7a8 --- /dev/null +++ b/rmf_task_sequence/CMakeLists.txt @@ -0,0 +1,128 @@ +cmake_minimum_required(VERSION 3.5.0) + +project(rmf_task_sequence VERSION 2.0.0) + +set(CMAKE_EXPORT_COMPILE_COMMANDS on) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_BUILD_TYPE) + # Use the Release build type by default if the user has not specified one + set(CMAKE_BUILD_TYPE Release) +endif() + +include(GNUInstallDirs) + +find_package(rmf_task REQUIRED) +find_package(yaml-cpp REQUIRED) + +find_package(ament_cmake_catch2 QUIET) +find_package(rmf_cmake_uncrustify QUIET) + +# ===== RMF Task Sequence Library +file(GLOB lib_srcs + "src/rmf_task_sequence/*.cpp" +) + +add_library(rmf_task_sequence SHARED + ${lib_srcs} +) + +target_link_libraries(rmf_task_sequence + PUBLIC + rmf_task::rmf_task + yaml-cpp +) + +target_include_directories(rmf_task_sequence + PUBLIC + $ + $ +) + +if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) + file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + + ament_add_catch2( + test_rmf_task_sequence test/main.cpp ${unit_test_srcs} + TIMEOUT 300) + target_link_libraries(test_rmf_task_sequence + rmf_task_sequence + ) + + target_include_directories(test_rmf_task_sequence + PRIVATE + $ + ) + + find_file(uncrustify_config_file + NAMES "rmf_code_style.cfg" + PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") + + rmf_uncrustify( + ARGN include src test + CONFIG_FILE ${uncrustify_config_file} + MAX_LINE_LENGTH 80 + ) +endif() + + +# Create cmake config files +include(CMakePackageConfigHelpers) + +set(INSTALL_CONFIG_DIR "${CMAKE_INSTALL_LIBDIR}/rmf_task_sequence/cmake") +set(PACKAGE_CONFIG_VERSION_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_task_sequence-config-version.cmake") +set(PACKAGE_CONFIG_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_task_sequence-config.cmake") + +configure_package_config_file( + "${CMAKE_CURRENT_LIST_DIR}/cmake/rmf_task_sequence-config.cmake.in" + ${PACKAGE_CONFIG_FILE} + INSTALL_DESTINATION ${INSTALL_CONFIG_DIR} +) + +write_basic_package_version_file( + ${PACKAGE_CONFIG_VERSION_FILE} + COMPATIBILITY ExactVersion +) + +install( + TARGETS rmf_task_sequence + EXPORT rmf_task_sequence-targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +install( + DIRECTORY include/rmf_task_sequence + DESTINATION include +) + +install( + FILES + ${PACKAGE_CONFIG_VERSION_FILE} + ${PACKAGE_CONFIG_FILE} + DESTINATION ${INSTALL_CONFIG_DIR} +) + +install( + EXPORT rmf_task_sequence-targets + FILE rmf_task_sequence-targets.cmake + NAMESPACE rmf_task_sequence:: + DESTINATION ${INSTALL_CONFIG_DIR} +) + +export( + EXPORT rmf_task_sequence-targets + FILE ${CMAKE_CURRENT_BINARY_DIR}/rmf_task_sequence-targets.cmake + NAMESPACE rmf_task_sequence:: +) + +export(PACKAGE rmf_task_sequence) diff --git a/rmf_task_sequence/QUALITY_DECLARATION.md b/rmf_task_sequence/QUALITY_DECLARATION.md new file mode 100644 index 00000000..ca59f9e5 --- /dev/null +++ b/rmf_task_sequence/QUALITY_DECLARATION.md @@ -0,0 +1,154 @@ +This document is a declaration of software quality for the `rmf_task` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# `rmf_task` Quality Declaration + +The package `rmf_task` claims to be in the **Quality Level 4** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 4 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rmf_task` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). + +### Version Stability [1.ii] + +`rmf_task` is at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package. +Headers in any other folders are not installed and are considered private. + +All launch files in the installed `launch` directory are considered part of the public API. + +### API Stability Policy [1.iv] + +`rmf_task_sequence` will not break public API within a major version number. + +### ABI Stability Policy [1.v] + +`rmf_task_sequence` will not break public ABI within a major version number. + +### API and ABI Stability Within a Released ROS Distribution [1.vi] + +`rmf_task_sequence` will not break public API or ABI within a released ROS distribution, i.e. no major releases into the same ROS distribution once that ROS distribution is released. + +## Change Control Process [2] + +`rmf_task_sequence` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-requirements). + +### Change Requests [2.i] + +`rmf_task_sequence` requires that all changes occur through a pull request. + +### Contributor Origin [2.ii] + +`rmf_task_sequence` uses DCO as its confirmation of contributor origin policy. +More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +All pull requests must have at least 1 peer review. + +### Continuous Integration [2.iv] + +All pull requests must pass CI on all platforms supported by RMF. + +The most recent CI results can be seen on [the workflow page](https://github.com/open-rmf/rmf_task/actions). + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rmf_task_sequence` does not provide documentation. + +### Public API Documentation [3.ii] + +`rmf_task_sequence` documents its public API. +The documentation is not hosted. + +### License [3.iii] + +The license for `rmf_task_sequence` is Apache 2.0, the type is declared in the [package.xml](package.xml) manifest file, and a full copy of the license is in the repository level [LICENSE](../LICENSE) file. + +### Copyright Statement [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rmf_task_sequence`. + +### Quality declaration document [3.v] + +This quality declaration is linked in the [README file](README.md). + +This quality declaration has not been externally peer-reviewed and is not registered on any Level 4 lists. + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rmf_task_sequence` has corresponding tests which simulate typical usage. +They are located in the [`test`](https://github.com/open-rmf/rmf_task/tree/main/rmf_task_sequence/test) directory. +New features are required to have tests before being added. + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. +They are located in the [`test`](https://github.com/open-rmf/rmf_task/tree/main/rmf_task_sequence/test) directory. + +### Coverage [4.iii] + +`rmf_task_sequence` tracks code coverage statistics. +There is no coverage target currently, but new changes are required to make a best effort to keep or increase coverage before being accepted. +Decreases are allowed if properly justified and accepted by reviewers. +Code coverage will be improved in the future as time allows. + +Current coverage statistics can be viewed [here](https://codecov.io/gh/open-rmf/rmf_task). + +### Performance [4.iv] + +`rmf_task_sequence` does not test performance. + +### Linters and Static Analysis [4.v] + +`rmf_task_sequence` does not use the standard linters and static analysis tools for its CMake code to ensure it follows the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters). + +`rmf_task_sequence` uses a custom `uncrustify` configuration matching its coding style. + +## Dependencies [5] + +### Direct Runtime ROS Dependencies [5.i] + +Below are the required direct runtime ROS dependencies of `rmf_task_sequence` and their evaluations. + +#### rmf\_task + +`rmf_task` is [**Quality Level 4**](https://github.com/open-rmf/rmf_task/blob/main/rmf_task/QUALITY_DECLARATION.md). + +### Optional Direct Runtime ROS Dependencies [5.ii] + +`rmf_task_sequence` has no optional runtime ROS dependencies. + +### Direct Runtime non-ROS Dependency [5.iii] + +`rmf_task_sequence` has no direct runtime non-ROS dependencies. + +## Platform Support [6] + +### Target platforms [6.i] + +`rmf_task_sequence` does not support all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers). +`rmf_task_sequence` supports ROS Foxy. + +## Security [7] + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rmf_task_sequence/README.md b/rmf_task_sequence/README.md new file mode 100644 index 00000000..d65766fa --- /dev/null +++ b/rmf_task_sequence/README.md @@ -0,0 +1,7 @@ +# rmf\_task package + +This packages provides the implementation of phase-sequence tasks for the Robotics Middleware Framework + +## Quality Declaration + +This package claims to be in the **Quality Level 4** category. See the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in new file mode 100644 index 00000000..07e63da9 --- /dev/null +++ b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in @@ -0,0 +1,14 @@ +@PACKAGE_INIT@ + +get_filename_component(rmf_task_sequence_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) + +include(CMakeFindDependencyMacro) + +find_dependency(rmf_task REQUIRED) +find_dependency(yaml-cpp REQUIRED) + +if(NOT TARGET rmf_task_sequence::rmf_task_sequence) + include("${rmf_task_sequence_CMAKE_DIR}/rmf_task_sequence-targets.cmake") +endif() + +check_required_components(rmf_task_sequence) diff --git a/rmf_task/include/rmf_task/sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp similarity index 98% rename from rmf_task/include/rmf_task/sequence/Phase.hpp rename to rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index c50c51fd..2f6143e4 100644 --- a/rmf_task/include/rmf_task/sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__SEQUENCE__PHASE_HPP -#define RMF_TASK__SEQUENCE__PHASE_HPP +#ifndef RMF_TASK_SEQUENCE__PHASE_HPP +#define RMF_TASK_SEQUENCE__PHASE_HPP #include #include @@ -26,8 +26,7 @@ #include #include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { //============================================================================== /// A factory for generating execute::ActivePhase instances from descriptions. @@ -322,7 +321,6 @@ class Phase::SequenceModel : public Phase::Model rmf_utils::unique_impl_ptr _pimpl; }; -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence -#endif // RMF_TASK__SEQUENCE__PHASE_HPP +#endif // RMF_TASK_SEQUENCE__PHASE_HPP diff --git a/rmf_task/include/rmf_task/sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp similarity index 100% rename from rmf_task/include/rmf_task/sequence/Task.hpp rename to rmf_task_sequence/include/rmf_task_sequence/Task.hpp diff --git a/rmf_task/include/rmf_task/sequence/phases/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp similarity index 100% rename from rmf_task/include/rmf_task/sequence/phases/DropOff.hpp rename to rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp diff --git a/rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp similarity index 100% rename from rmf_task/include/rmf_task/sequence/phases/GoToPlace.hpp rename to rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp diff --git a/rmf_task/include/rmf_task/sequence/phases/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp similarity index 100% rename from rmf_task/include/rmf_task/sequence/phases/PickUp.hpp rename to rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp diff --git a/rmf_task/include/rmf_task/sequence/phases/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp similarity index 100% rename from rmf_task/include/rmf_task/sequence/phases/WaitFor.hpp rename to rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp diff --git a/rmf_task_sequence/package.xml b/rmf_task_sequence/package.xml new file mode 100644 index 00000000..e1a1c29c --- /dev/null +++ b/rmf_task_sequence/package.xml @@ -0,0 +1,21 @@ + + + + rmf_task_sequence + 2.0.0 + Implementation of phase-sequence tasks for the Robotics Middleware Framework + Grey + Marco A. Gutiérrez + Apache License 2.0 + Grey + + rmf_task + yaml-cpp + + ament_cmake_catch2 + rmf_cmake_uncrustify + + + cmake + + diff --git a/rmf_task/src/rmf_task/sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp similarity index 95% rename from rmf_task/src/rmf_task/sequence/Phase.cpp rename to rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index 3b211b2a..57c16cf8 100644 --- a/rmf_task/src/rmf_task/sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -15,10 +15,9 @@ * */ -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { //============================================================================== class Phase::SequenceModel::Implementation @@ -95,5 +94,4 @@ Phase::SequenceModel::SequenceModel() // Do nothing } -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence diff --git a/rmf_task/src/rmf_task/sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/Task.cpp rename to rmf_task_sequence/src/rmf_task_sequence/Task.cpp diff --git a/rmf_task/src/rmf_task/sequence/phases/DropOff.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/phases/DropOff.cpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp diff --git a/rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/phases/GoToPlace.cpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp diff --git a/rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/phases/PayloadTransfer.cpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp diff --git a/rmf_task/src/rmf_task/sequence/phases/PickUp.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/phases/PickUp.cpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp diff --git a/rmf_task/src/rmf_task/sequence/phases/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/phases/WaitFor.cpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp diff --git a/rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp similarity index 100% rename from rmf_task/src/rmf_task/sequence/phases/internal_PayloadTransfer.hpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp diff --git a/rmf_task_sequence/test/main.cpp b/rmf_task_sequence/test/main.cpp new file mode 100644 index 00000000..23e837ac --- /dev/null +++ b/rmf_task_sequence/test/main.cpp @@ -0,0 +1,21 @@ +/* + * Copyright (C) 2020 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. + * +*/ + +#define CATCH_CONFIG_MAIN +#include + +// This will create the main(int argc, char* argv[]) entry point for testing From 7c319944b6c3f83e280fc8759ace0ad51a269574 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 10 Sep 2021 18:01:10 +0800 Subject: [PATCH 17/85] More reorganization Signed-off-by: Michael X. Grey --- rmf_task/CMakeLists.txt | 5 +- .../TaskActivator.hpp => Activator.hpp} | 30 ++++---- rmf_task/include/rmf_task/Task.hpp | 11 ++- rmf_task/include/rmf_task/detail/Resume.hpp | 1 + .../src/rmf_task/{agv => }/Constraints.cpp | 0 rmf_task/src/rmf_task/CostCalculator.hpp | 2 +- .../src/rmf_task/{agv => }/Parameters.cpp | 0 rmf_task/src/rmf_task/{agv => }/State.cpp | 0 rmf_task/src/rmf_task/Task.cpp | 59 +++++++++++++++- .../src/rmf_task/{agv => }/TaskPlanner.cpp | 2 +- rmf_task/src/rmf_task/detail/Resume.cpp | 70 +++++++++++++++++++ .../src/rmf_task/detail/internal_Resume.hpp | 63 +++++++++++++++++ .../{agv => }/internal_task_planning.cpp | 0 .../{agv => }/internal_task_planning.hpp | 6 +- 14 files changed, 222 insertions(+), 27 deletions(-) rename rmf_task/include/rmf_task/{execute/TaskActivator.hpp => Activator.hpp} (85%) rename rmf_task/src/rmf_task/{agv => }/Constraints.cpp (100%) rename rmf_task/src/rmf_task/{agv => }/Parameters.cpp (100%) rename rmf_task/src/rmf_task/{agv => }/State.cpp (100%) rename rmf_task/src/rmf_task/{agv => }/TaskPlanner.cpp (99%) create mode 100644 rmf_task/src/rmf_task/detail/Resume.cpp create mode 100644 rmf_task/src/rmf_task/detail/internal_Resume.hpp rename rmf_task/src/rmf_task/{agv => }/internal_task_planning.cpp (100%) rename rmf_task/src/rmf_task/{agv => }/internal_task_planning.hpp (97%) diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index f019343a..9e132e3e 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -30,10 +30,7 @@ find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Tasks library file(GLOB lib_srcs - "src/rmf_task/agv/*.cpp" - "src/rmf_task/execute/*.cpp" - "src/rmf_task/sequence/*.cpp" - "src/rmf_task/sequence/phases/*.cpp" + "src/rmf_task/detail/*.cpp" "src/rmf_task/requests/*.cpp" "src/rmf_task/requests/factory/*.cpp" "src/rmf_task/*.cpp" diff --git a/rmf_task/include/rmf_task/execute/TaskActivator.hpp b/rmf_task/include/rmf_task/Activator.hpp similarity index 85% rename from rmf_task/include/rmf_task/execute/TaskActivator.hpp rename to rmf_task/include/rmf_task/Activator.hpp index 5a2e9099..5847ee6e 100644 --- a/rmf_task/include/rmf_task/execute/TaskActivator.hpp +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -15,24 +15,21 @@ * */ -#ifndef RMF_TASK__TASKACTIVATOR_HPP -#define RMF_TASK__TASKACTIVATOR_HPP +#ifndef RMF_TASK__ACTIVATOR_HPP +#define RMF_TASK__ACTIVATOR_HPP #include -#include - namespace rmf_task { -namespace execute { //============================================================================== -/// A factory for generating Task instances from requests. -class TaskActivator +/// A factory for generating Task::Active instances from requests. +class Activator { public: /// Construct an empty TaskFactory - TaskActivator(); + Activator(); /// Signature for activating a task /// @@ -61,8 +58,8 @@ class TaskActivator template using Activate = std::function< - execute::TaskPtr( - const Request::ConstTagPtr& request, + Task::ActivePtr( + const Task::ConstBookingPtr& request, const Description& description, std::optional backup_state, std::function update, @@ -97,8 +94,8 @@ class TaskActivator /// \return an active, running instance of the requested task. std::shared_ptr activate( const Request& request, - std::function update, - std::function phase_finished, + std::function update, + std::function phase_finished, std::function task_finished); /// Restore a Task that crashed or disconnected. @@ -122,8 +119,8 @@ class TaskActivator std::shared_ptr restore( const Request& request, std::string backup_state, - std::function update, - std::function phase_finished, + std::function update, + std::function phase_finished, std::function task_finished); class Implementation; @@ -131,7 +128,8 @@ class TaskActivator rmf_utils::impl_ptr _pimpl; }; -} // namespace execute + } // namespace rmf_task -#endif // RMF_TASK__TASKACTIVATOR_HPP + +#endif // RMF_TASK__ACTIVATOR_HPP diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index c41b7dda..83c69c7e 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -58,6 +58,8 @@ class Task }; //============================================================================== +/// Basic information about how the task was booked, e.g. what its name is, +/// when it should start, and what its priority is. class Task::Booking { public: @@ -104,6 +106,13 @@ class Task::Tag { public: + /// Constructor + Tag( + ConstBookingPtr booking_, + std::string category_, + std::string detail_, + rmf_traffic::Time original_finish_estimate_); + /// The original request that this Task is carrying out const ConstBookingPtr& booking() const; @@ -200,7 +209,7 @@ class Task::Active /// The Resume class keeps track of when the Task is allowed to Resume. /// You can either call the Resume object's operator() or let the object /// expire to tell the Task that it may resume. - class Resume : public detail::Resume {}; + using Resume = detail::Resume; /// Tell this Task that it needs to be interrupted. An interruption means /// the robot may be commanded to do other tasks before this task resumes. diff --git a/rmf_task/include/rmf_task/detail/Resume.hpp b/rmf_task/include/rmf_task/detail/Resume.hpp index a1413b3c..9c4fa73d 100644 --- a/rmf_task/include/rmf_task/detail/Resume.hpp +++ b/rmf_task/include/rmf_task/detail/Resume.hpp @@ -33,6 +33,7 @@ class Resume class Implementation; private: + Resume(); rmf_utils::unique_impl_ptr _pimpl; }; diff --git a/rmf_task/src/rmf_task/agv/Constraints.cpp b/rmf_task/src/rmf_task/Constraints.cpp similarity index 100% rename from rmf_task/src/rmf_task/agv/Constraints.cpp rename to rmf_task/src/rmf_task/Constraints.cpp diff --git a/rmf_task/src/rmf_task/CostCalculator.hpp b/rmf_task/src/rmf_task/CostCalculator.hpp index fe7f16ff..6023e2f0 100644 --- a/rmf_task/src/rmf_task/CostCalculator.hpp +++ b/rmf_task/src/rmf_task/CostCalculator.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_TASK__COSTCALCULATOR_HPP #define SRC__RMF_TASK__COSTCALCULATOR_HPP -#include "agv/internal_task_planning.hpp" +#include "internal_task_planning.hpp" #include diff --git a/rmf_task/src/rmf_task/agv/Parameters.cpp b/rmf_task/src/rmf_task/Parameters.cpp similarity index 100% rename from rmf_task/src/rmf_task/agv/Parameters.cpp rename to rmf_task/src/rmf_task/Parameters.cpp diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/State.cpp similarity index 100% rename from rmf_task/src/rmf_task/agv/State.cpp rename to rmf_task/src/rmf_task/State.cpp diff --git a/rmf_task/src/rmf_task/Task.cpp b/rmf_task/src/rmf_task/Task.cpp index 9dadbadc..80d68d0a 100644 --- a/rmf_task/src/rmf_task/Task.cpp +++ b/rmf_task/src/rmf_task/Task.cpp @@ -15,11 +15,12 @@ * */ +#include "detail/internal_Resume.hpp" + #include namespace rmf_task { - //============================================================================== class Task::Booking::Implementation { @@ -71,5 +72,61 @@ bool Task::Booking::automatic() const return _pimpl->automatic; } +//============================================================================== +class Task::Tag::Implementation +{ +public: + ConstBookingPtr booking; + std::string category; + std::string detail; + rmf_traffic::Time original_finish_estimate; +}; + +//============================================================================== +Task::Tag::Tag( + ConstBookingPtr booking_, + std::string category_, + std::string detail_, + rmf_traffic::Time original_finish_estimate_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(booking_), + std::move(category_), + std::move(detail_), + original_finish_estimate_ + })) +{ + // Do nothing +} + +//============================================================================== +auto Task::Tag::booking() const -> const ConstBookingPtr& +{ + return _pimpl->booking; +} + +//============================================================================== +const std::string& Task::Tag::category() const +{ + return _pimpl->category; +} + +//============================================================================== +const std::string& Task::Tag::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +rmf_traffic::Time Task::Tag::original_finish_estimate() const +{ + return _pimpl->original_finish_estimate; +} + +//============================================================================== +Task::Active::Resume Task::Active::make_resumer(std::function callback) +{ + return detail::Resume::Implementation::make(std::move(callback)); +} } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/TaskPlanner.cpp similarity index 99% rename from rmf_task/src/rmf_task/agv/TaskPlanner.cpp rename to rmf_task/src/rmf_task/TaskPlanner.cpp index d60d6522..e981cc1d 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -20,7 +20,7 @@ #include #include -#include "../BinaryPriorityCostCalculator.hpp" +#include "BinaryPriorityCostCalculator.hpp" #include diff --git a/rmf_task/src/rmf_task/detail/Resume.cpp b/rmf_task/src/rmf_task/detail/Resume.cpp new file mode 100644 index 00000000..624899d8 --- /dev/null +++ b/rmf_task/src/rmf_task/detail/Resume.cpp @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2021 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_Resume.hpp" + +namespace rmf_task { +namespace detail { + +//============================================================================== +void Resume::operator()() const +{ + _pimpl->trigger(); +} + +//============================================================================== +Resume::Resume() +{ + // Do nothing +} + +//============================================================================== +void Resume::Implementation::trigger() const +{ + std::lock_guard lock(mutex); + if (!called) + { + called = true; + callback(); + } +} + +//============================================================================== +Resume Resume::Implementation::make(std::function callback) +{ + Resume resumer; + resumer._pimpl = + rmf_utils::make_unique_impl(std::move(callback)); + + return resumer; +} + +//============================================================================== +Resume::Implementation::Implementation(std::function callback_) +: callback(std::move(callback_)) +{ + // Do nothing +} + +//============================================================================== +Resume::Implementation::~Implementation() +{ + trigger(); +} + +} // namespace detail +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/detail/internal_Resume.hpp b/rmf_task/src/rmf_task/detail/internal_Resume.hpp new file mode 100644 index 00000000..122a989d --- /dev/null +++ b/rmf_task/src/rmf_task/detail/internal_Resume.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2021 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__DETAIL__INTERNAL_RESUME_HPP +#define SRC__RMF_TASK__DETAIL__INTERNAL_RESUME_HPP + +#include + +#include +#include + +namespace rmf_task { +namespace detail { + +//============================================================================== +class Resume::Implementation +{ +public: + + std::function callback; + + // We use a recursive mutex in case triggering the callback leads to a chain + // that causes the Resume object to be triggered again or destroyed. We have + // no way to prevent such a behavior from the implementation here, but we can + // at least ensure that it does not cause a deadlock or an infinitely + // recursive loop. + mutable std::recursive_mutex mutex; + mutable bool called = false; + + void trigger() const; + + static Resume make(std::function callback); + + Implementation(std::function callback); + + Implementation(const Implementation&) = delete; + Implementation(Implementation&&) = delete; + Implementation& operator=(const Implementation&) = delete; + Implementation& operator=(Implementation&&) = delete; + + ~Implementation(); +}; + +} // namespace detail +} // namespace rmf_task + + +#endif // SRC__RMF_TASK__DETAIL__INTERNAL_RESUME_HPP diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp b/rmf_task/src/rmf_task/internal_task_planning.cpp similarity index 100% rename from rmf_task/src/rmf_task/agv/internal_task_planning.cpp rename to rmf_task/src/rmf_task/internal_task_planning.cpp diff --git a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp b/rmf_task/src/rmf_task/internal_task_planning.hpp similarity index 97% rename from rmf_task/src/rmf_task/agv/internal_task_planning.hpp rename to rmf_task/src/rmf_task/internal_task_planning.hpp index cbc0c96c..b4b239b6 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp +++ b/rmf_task/src/rmf_task/internal_task_planning.hpp @@ -15,8 +15,8 @@ * */ -#ifndef SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP -#define SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP +#ifndef SRC__RMF_TASK__INTERNAL_TASK_PLANNING_HPP +#define SRC__RMF_TASK__INTERNAL_TASK_PLANNING_HPP #include @@ -213,4 +213,4 @@ struct LowestCostEstimate } // namespace rmf_task -#endif // SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP +#endif // SRC__RMF_TASK__INTERNAL_TASK_PLANNING_HPP From 9103c90a2faa09bf4072e6d0cfe7bf6f9b9a1aa2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 11 Sep 2021 01:59:17 +0800 Subject: [PATCH 18/85] Gradually ironing out the transition to the reorganization Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Phase.hpp | 3 + rmf_task/include/rmf_task/Task.hpp | 34 +++---- rmf_task/src/rmf_task/Phase.cpp | 29 ++++++ rmf_task_sequence/CMakeLists.txt | 1 + .../include/rmf_task_sequence/Phase.hpp | 62 ++++++------ .../include/rmf_task_sequence/Task.hpp | 54 +++++----- .../rmf_task_sequence/phases/GoToPlace.hpp | 22 ++--- .../rmf_task_sequence/phases/WaitFor.hpp | 29 +++--- .../src/rmf_task_sequence/Phase.cpp | 20 ++-- .../src/rmf_task_sequence/Task.cpp | 98 +++++++++++++------ .../src/rmf_task_sequence/phases/WaitFor.cpp | 24 +++-- 11 files changed, 225 insertions(+), 151 deletions(-) diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 14257967..1301d2ae 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -163,6 +163,9 @@ class Phase::Pending { public: + /// Constructor + Pending(ConstTagPtr tag); + /// Tag of the phase const ConstTagPtr& tag() const; diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 83c69c7e..7b48dbbf 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -67,32 +67,32 @@ class Task::Booking /// Constructor /// /// \param[in] id_ - /// The identify of the request + /// The identity of the booking /// /// \param[in] earliest_start_time_ - /// The earliest time that the request may begin + /// The earliest time that the task may begin /// /// \param[in] priority_ - /// The priority of the request + /// The priority of the booking /// /// \param[in] automatic_ - /// Whether this request was automatically generated + /// Whether this booking was automatically generated Booking( std::string id_, rmf_traffic::Time earliest_start_time_, ConstPriorityPtr priority_, bool automatic_ = false); - /// The unique id for this request + /// The unique id for this booking const std::string& id() const; - /// Get the earliest time that this request may begin + /// Get the earliest time that this booking may begin rmf_traffic::Time earliest_start_time() const; - /// Get the priority of this request + /// Get the priority of this booking ConstPriorityPtr priority() const; - // Returns true if this request was automatically generated + // Returns true if this booking was automatically generated bool automatic() const; class Implementation; @@ -113,7 +113,7 @@ class Task::Tag std::string detail_, rmf_traffic::Time original_finish_estimate_); - /// The original request that this Task is carrying out + /// The booking information of the request that this Task is carrying out const ConstBookingPtr& booking() const; /// The category of this Task. @@ -137,32 +137,32 @@ class Task::Model { public: - /// Estimate the state of the robot when the request is finished along with - /// the time the robot has to wait before commencing the request + /// Estimate the state of the robot when the task is finished along with + /// the time the robot has to wait before commencing the task virtual std::optional estimate_finish( const State& initial_state, const Constraints& task_planning_constraints, const TravelEstimator& travel_estimator) const = 0; - /// Estimate the invariant component of the request's duration + /// Estimate the invariant component of the task's duration virtual rmf_traffic::Duration invariant_duration() const = 0; virtual ~Model() = default; }; //============================================================================== -/// An abstract interface to define the specifics of this request. This -/// implemented description will differentiate this request from others. +/// An abstract interface to define the specifics of this task. This +/// implemented description will differentiate this task from others. class Task::Description { public: - /// Generate a Model for this request based on the unique traits of this + /// Generate a Model for the task based on the unique traits of this /// description /// /// \param[in] earliest_start_time - /// The earliest time this request should begin execution. This is usually - /// the requested start time for the request. + /// The earliest time this task should begin execution. This is usually + /// the requested start time for the task. /// /// \param[in] parameters /// The parameters that describe this AGV diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index f6eb0b59..4613d138 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -72,4 +72,33 @@ rmf_traffic::Duration Phase::Tag::original_duration_estimate() const return _pimpl->duration; } +//============================================================================== +class Phase::Pending::Implementation +{ +public: + + ConstTagPtr tag; + bool will_be_skipped = false; + +}; + +//============================================================================== +Phase::Pending::Pending(ConstTagPtr tag) +: _pimpl(rmf_utils::make_impl(Implementation{std::move(tag)})) +{ + // Do nothing +} + +//============================================================================== +auto Phase::Pending::tag() const -> const ConstTagPtr& +{ + return _pimpl->tag; +} + +//============================================================================== +bool Phase::Pending::will_be_skipped() const +{ + return _pimpl->will_be_skipped; +} + } // namespace rmf_task diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 0a08f7a8..208e7c1f 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Task Sequence Library file(GLOB lib_srcs "src/rmf_task_sequence/*.cpp" + "src/rmf_task_sequence/phases/*.cpp" ) add_library(rmf_task_sequence SHARED diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 2f6143e4..697b5ed5 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -22,15 +22,17 @@ #include #include #include -#include +#include #include #include +#include + namespace rmf_task_sequence { //============================================================================== /// A factory for generating execute::ActivePhase instances from descriptions. -class Phase +class Phase : public rmf_task::Phase { public: @@ -53,7 +55,7 @@ class Phase //============================================================================== /// The interface for an Active phase within a phase sequence task. -class Phase::Active : public execute::Phase::Active +class Phase::Active : public rmf_task::Phase::Active { public: @@ -64,7 +66,7 @@ class Phase::Active : public execute::Phase::Active /// Each Backup is tagged with a sequence number. As the Phase makes progress, /// it can issue new Backups with higher sequence numbers. Only the Backup /// with the highest sequence number will be kept. - class Backup : public detail::Backup {}; + using Backup = rmf_task::detail::Backup; /// Get a backup for this Phase virtual Backup backup() const = 0; @@ -72,7 +74,7 @@ class Phase::Active : public execute::Phase::Active /// The Resume class keeps track of when the phase is allowed to Resume. /// You can either call the Resume object's operator() or let the object /// expire to tell the phase that it may resume. - class Resume : public detail::Resume {}; + using Resume = rmf_task::detail::Resume; /// Tell this phase that it needs to be interrupted. An interruption means /// the robot may be commanded to do other tasks before this phase resumes. @@ -145,8 +147,8 @@ class Phase::Activator ActivePtr( const Description& description, std::optional backup_state, - std::function update, - std::function finished) + std::function update, + std::function finished) >; /// Add a callback to convert from a PhaseDescription into an active phase. @@ -172,8 +174,8 @@ class Phase::Activator /// \return an active, running instance of the described phase. ActivePtr activate( const Description& description, - std::function update, - std::function finished) const; + std::function update, + std::function finished) const; /// Restore a phase based on a description of the phase and its backup state. /// @@ -195,8 +197,8 @@ class Phase::Activator ActivePtr restore( const Description& description, const std::string& backup_state, - std::function update, - std::function finished) const; + std::function update, + std::function finished) const; class Implementation; private: @@ -220,8 +222,8 @@ class Phase::Description /// /// \return a model based on the given start state and parameters. virtual ConstModelPtr make_model( - State invariant_initial_state, - const Parameters& parameters) const = 0; + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const = 0; /// Get the human-friendly information about this phase /// @@ -230,13 +232,13 @@ class Phase::Description /// /// \param[in] constraints /// Constraints on the robot during the phase - virtual execute::Phase::ConstTagPtr make_tag( - execute::Phase::Tag::Id id, - const State& initial_state, - const Parameters& parameters) const = 0; + virtual rmf_task::Phase::ConstTagPtr make_tag( + rmf_task::Phase::Tag::Id id, + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const = 0; /// Serialize this phase description into a string. - virtual std::string serialize() const = 0; + virtual YAML::Node serialize() const = 0; // Virtual destructor virtual ~Description() = default; @@ -258,17 +260,17 @@ class Phase::Model /// \param[in] /// /// \return an estimated state for the robot when the phase is finished. - virtual std::optional estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const = 0; + virtual std::optional estimate_finish( + rmf_task::State initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const = 0; /// Estimate the invariant component of the request's duration. virtual rmf_traffic::Duration invariant_duration() const = 0; /// Get the components of the finish state that this phase is guaranteed to /// result in once the phase is finished. - virtual State invariant_finish_state() const = 0; + virtual rmf_task::State invariant_finish_state() const = 0; // Virtual destructor virtual ~Model() = default; @@ -300,20 +302,20 @@ class Phase::SequenceModel : public Phase::Model /// \return A Phase::Model implemented as a SequenceModel. static Phase::ConstModelPtr make( const std::vector& descriptions, - State invariant_initial_state, - const Parameters& parameters); + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters); // Documentation inherited - std::optional estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const final; + std::optional estimate_finish( + rmf_task::State initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; // Documentation inherited - State invariant_finish_state() const final; + rmf_task::State invariant_finish_state() const final; class Implementation; private: diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index 6d31fdb9..4e2d05a8 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -19,15 +19,14 @@ #define RMF_TASK__SEQUENCE__TASK_HPP #include -#include -#include -#include +#include +#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { //============================================================================== -class Task +class Task : public rmf_task::Task { public: @@ -41,15 +40,18 @@ class Task class Description; using ConstDescriptionPtr = std::shared_ptr; - using Update = std::function; - using PhaseFinished = std::function; + class Model; + + using Update = std::function; + using PhaseFinished = std::function; using TaskFinished = std::function; - /// Activate a phase sequence task + /// Make an activator for a phase sequence task. This activator can be given + /// to /// /// \param[in] phase_activator /// A phase activator - static execute::TaskActivator::Activate make_activator( + static rmf_task::Activator::Activate make_activator( Phase::ConstActivatorPtr phase_activator); }; @@ -70,13 +72,21 @@ class Task::Builder /// \param[in] cancellation_sequence /// This phase sequence will be run if the task is cancelled during this /// phase. - void add_phase( + Builder& add_phase( Phase::ConstDescriptionPtr description, std::vector cancellation_sequence); /// Generate a TaskDescription instance from the phases that have been given /// to the builder. - ConstDescriptionPtr build(); + /// + /// \param[in] category + /// Task category information that will go into the Task::Tag + /// + /// \param[in] detail + /// Any detailed information that will go into the Task::Tag + ConstDescriptionPtr build( + std::string category, + std::string detail); class Implementation; private: @@ -84,15 +94,14 @@ class Task::Builder }; //============================================================================== -class Task::Description : public Request::Description +class Task::Description : public rmf_task::Task::Description { public: - class Model; // Documentation inherited - std::shared_ptr make_model( + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, - const Parameters& parameters) const final; + const rmf_task::Parameters& parameters) const final; class Implementation; private: @@ -100,15 +109,15 @@ class Task::Description : public Request::Description }; //============================================================================== -class Task::Description::Model : public Request::Model +class Task::Model : public rmf_task::Task::Model { public: // Documentation inherited - std::optional estimate_finish( - const State& initial_state, - const Constraints& task_planning_constraints, - const TravelEstimator& travel_estimator) const final; + std::optional estimate_finish( + const rmf_task::State& initial_state, + const rmf_task::Constraints& task_planning_constraints, + const rmf_task::TravelEstimator& travel_estimator) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; @@ -118,7 +127,6 @@ class Task::Description::Model : public Request::Model rmf_utils::unique_impl_ptr _pimpl; }; -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence #endif // RMF_TASK__SEQUENCE__TASK_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp index 6fcbc8e7..64ea13e8 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp @@ -20,10 +20,9 @@ #include -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -49,14 +48,14 @@ class GoToPlace::Description : public Phase::Description // Documentation inherited Phase::ConstModelPtr make_model( - State invariant_initial_state, - const Parameters& parameters) const final; + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const final; // Documentation inherited - execute::Phase::ConstTagPtr make_tag( - execute::Phase::Tag::Id id, - const State& initial_state, - const Parameters& parameters) const final; + Phase::ConstTagPtr make_tag( + Phase::Tag::Id id, + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const final; /// Get the current goal for this description. const Goal& goal() const; @@ -66,7 +65,7 @@ class GoToPlace::Description : public Phase::Description /// Get the name of the goal. If the goal does not have an explicit name, it /// will be referred to as "#x" where "x" is the index number of the waypoint. - std::string goal_name(const Parameters& parameters) const; + std::string goal_name(const rmf_task::Parameters& parameters) const; class Implementation; private: @@ -75,7 +74,6 @@ class GoToPlace::Description : public Phase::Description }; } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence #endif // RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp index 4af5284c..f960327d 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp @@ -15,15 +15,14 @@ * */ -#ifndef RMF_TASK__SEQUENCE__PHASES__WAITFOR_HPP -#define RMF_TASK__SEQUENCE__PHASES__WAITFOR_HPP +#ifndef RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP +#define RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP #include -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -65,14 +64,17 @@ class WaitFor::Description : public Phase::Description // Documentation inherited Phase::ConstModelPtr make_model( - State invariant_initial_state, - const Parameters& parameters) const final; + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const final; // Documentation inherited - execute::Phase::ConstTagPtr make_tag( - execute::Phase::Tag::Id id, - const State& initial_state, - const Parameters& parameters) const final; + Phase::ConstTagPtr make_tag( + Phase::Tag::Id id, + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const final; + + // Documentation inherited + YAML::Node serialize() const final; class Implementation; private: @@ -81,7 +83,6 @@ class WaitFor::Description : public Phase::Description }; } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence -#endif // RMF_TASK__SEQUENCE__PHASES__WAITFOR_HPP +#endif // RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index 57c16cf8..00ef3a6a 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -24,18 +24,18 @@ class Phase::SequenceModel::Implementation { public: std::vector models; - State invariant_finish_state; + rmf_task::State invariant_finish_state; rmf_traffic::Duration invariant_duration; }; //============================================================================== Phase::ConstModelPtr Phase::SequenceModel::make( const std::vector& descriptions, - State invariant_initial_state, - const Parameters& parameters) + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) { std::vector models; - State invariant_finish_state = invariant_initial_state; + rmf_task::State invariant_finish_state = invariant_initial_state; rmf_traffic::Duration invariant_duration = rmf_traffic::Duration(0); for (const auto& desc : descriptions) { @@ -58,12 +58,12 @@ Phase::ConstModelPtr Phase::SequenceModel::make( } //============================================================================== -std::optional Phase::SequenceModel::estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const +std::optional Phase::SequenceModel::estimate_finish( + rmf_task::State initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const { - std::optional finish_state = std::move(initial_state); + std::optional finish_state = std::move(initial_state); for (const auto& model : _pimpl->models) { finish_state = model->estimate_finish( @@ -83,7 +83,7 @@ rmf_traffic::Duration Phase::SequenceModel::invariant_duration() const } //============================================================================== -State Phase::SequenceModel::invariant_finish_state() const +rmf_task::State Phase::SequenceModel::invariant_finish_state() const { return _pimpl->invariant_finish_state; } diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 83384ef1..faea2085 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -17,26 +17,26 @@ #include -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace { //============================================================================== struct Stage { - Phase:: + Phase::Tag::Id id; Phase::ConstDescriptionPtr description; std::vector cancellation_sequence; }; +using ConstStagePtr = std::shared_ptr; } // anonymous namespace //============================================================================== class Task::Builder::Implementation { public: - std::vector stages; + std::vector stages; }; //============================================================================== @@ -44,11 +44,11 @@ class Task::Description::Implementation { public: - std::vector stages; + std::vector stages; - static std::list get_stages(const Description& desc) + static std::list get_stages(const Description& desc) { - return std::list( + return std::list( desc._pimpl->stages.begin(), desc._pimpl->stages.end()); } @@ -57,44 +57,49 @@ class Task::Description::Implementation //============================================================================== class Task::Active - : public execute::Task, + : public rmf_task::Task::Active, public std::enable_shared_from_this { public: - static execute::TaskPtr make( + static Task::ActivePtr make( Phase::ConstActivatorPtr phase_activator, - const Request::ConstTagPtr& tag, + const ConstBookingPtr& booking, const Description& description, std::optional backup_state, - std::function update, - std::function phase_finished, + std::function update, + std::function phase_finished, std::function task_finished) { auto task = std::shared_ptr( new Active( std::move(phase_activator), - tag, + booking, description, std::move(update), std::move(phase_finished), std::move(task_finished))); + // TODO(MXG): Make use of backup_state to fast forward the task to the + // relevant stage + task->generate_pending_phases(); + task->begin_next_stage(); + + return task; } // Documentation inherited - const std::vector& - completed_phases() const final; + const std::vector& completed_phases() const final; // Documentation inherited - execute::Phase::ConstActivePtr active_phase() const final; + Phase::ConstActivePtr active_phase() const final; // Documentation inherited - std::vector pending_phases() const final; + std::vector pending_phases() const final; // Documentation inherited - const Request::ConstTagPtr& tag() const final; + const ConstTagPtr& tag() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; @@ -119,37 +124,66 @@ class Task::Active private: + void generate_pending_phases(); + + void begin_next_stage(); + Active( Phase::ConstActivatorPtr phase_activator, - const Request::ConstTagPtr& request, + const ConstBookingPtr& booking, const Description& description, - std::function update, - std::function phase_finished, + std::function update, + std::function phase_finished, std::function task_finished) : _phase_activator(std::move(phase_activator)), - _tag(std::move(request)), - _remaining_stages(Description::Implementation::get_stages(description)), + _booking(std::move(booking)), _update(std::move(update)), _phase_finished(std::move(phase_finished)), - _task_finished(std::move(task_finished)) + _task_finished(std::move(task_finished)), + _pending_stages(Description::Implementation::get_stages(description)) { // Do nothing } Phase::ConstActivatorPtr _phase_activator; - Request::ConstTagPtr _tag; - std::list _remaining_stages; - std::function _update; - std::function _phase_finished; + ConstBookingPtr _booking; + std::function _update; + std::function _phase_finished; std::function _task_finished; + + std::list _pending_stages; + std::vector _pending_phases; + + ConstStagePtr _active_stage; + Phase::ActivePtr _active_phase; + std::list _completed_stages; + + std::optional _resume_interrupted_phase; + bool _cancelled = false; + bool _killed = false; }; +//============================================================================== +void Task::Active::generate_pending_phases() +{ + _pending_phases.reserve(_pending_stages.size()); + for (const auto& s : _pending_stages) + { + + } +} + +//============================================================================== +void Task::Active::begin_next_stage() +{ + +} + //============================================================================== auto Task::make_activator(Phase::ConstActivatorPtr phase_activator) --> execute::TaskActivator::Activate +-> rmf_task::Activator::Activate { } -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp index 74c3eb8b..c9d62922 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp @@ -15,10 +15,9 @@ * */ -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -27,21 +26,21 @@ class WaitFor::Model : public Phase::Model public: Model( - State invariant_initial_state, + rmf_task::State invariant_initial_state, rmf_traffic::Duration duration, - const Parameters& parameters); + const rmf_task::Parameters& parameters); - std::optional estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const final; + std::optional estimate_finish( + rmf_task::State initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const final; rmf_traffic::Duration invariant_duration() const final; - State invariant_finish_state() const final; + rmf_task::State invariant_finish_state() const final; private: - State _invariant_finish_state; + rmf_task::State _invariant_finish_state; double _invariant_battery_drain; rmf_traffic::Duration _duration; }; @@ -152,5 +151,4 @@ State WaitFor::Model::invariant_finish_state() const } } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence From 66777a8c4ad5b37e7621eee7d7ded20da2e298b8 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 11 Sep 2021 17:28:38 +0800 Subject: [PATCH 19/85] Continuing implementation of phase sequence task Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Activator.hpp | 30 ++++- rmf_task/include/rmf_task/Parameters.hpp | 4 +- rmf_task/include/rmf_task/Phase.hpp | 7 + rmf_task/src/rmf_task/Phase.cpp | 52 ++++++++ .../include/rmf_task_sequence/Phase.hpp | 69 ++++++---- .../include/rmf_task_sequence/Task.hpp | 25 +++- .../rmf_task_sequence/phases/DropOff.hpp | 15 ++- .../rmf_task_sequence/phases/GoToPlace.hpp | 21 +-- .../rmf_task_sequence/phases/PickUp.hpp | 15 ++- .../include/rmf_task_sequence/typedefs.hpp | 38 ++++++ .../src/rmf_task_sequence/Task.cpp | 121 ++++++++++++++++-- .../src/rmf_task_sequence/phases/DropOff.cpp | 12 +- .../rmf_task_sequence/phases/GoToPlace.cpp | 14 +- .../phases/PayloadTransfer.cpp | 12 +- .../src/rmf_task_sequence/phases/PickUp.cpp | 12 +- .../src/rmf_task_sequence/phases/WaitFor.cpp | 6 +- .../phases/internal_PayloadTransfer.hpp | 16 +-- 17 files changed, 363 insertions(+), 106 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp diff --git a/rmf_task/include/rmf_task/Activator.hpp b/rmf_task/include/rmf_task/Activator.hpp index 5847ee6e..b75bb4d4 100644 --- a/rmf_task/include/rmf_task/Activator.hpp +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -36,8 +36,14 @@ class Activator /// \tparam Description /// A class that implements the Request::Description interface /// - /// \param[in] request - /// An immutable reference to the relevant task request + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] booking + /// An immutable reference to the booking information for the task /// /// \param[in] description /// The down-casted description of the task @@ -59,7 +65,9 @@ class Activator using Activate = std::function< Task::ActivePtr( - const Task::ConstBookingPtr& request, + std::function get_state, + const ConstParametersPtr& parameters, + const Task::ConstBookingPtr& booking, const Description& description, std::optional backup_state, std::function update, @@ -79,6 +87,12 @@ class Activator /// Activate a Task object based on a Request::Description. /// + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// /// \param[in] request /// The task request /// @@ -93,6 +107,8 @@ class Activator /// /// \return an active, running instance of the requested task. std::shared_ptr activate( + std::function get_state, + const ConstParametersPtr& parameters, const Request& request, std::function update, std::function phase_finished, @@ -100,6 +116,12 @@ class Activator /// Restore a Task that crashed or disconnected. /// + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// /// \param[in] request /// The task request /// @@ -117,6 +139,8 @@ class Activator /// /// \return an active, running instance of the requested task. std::shared_ptr restore( + std::function get_state, + const ConstParametersPtr& parameters, const Request& request, std::string backup_state, std::function update, diff --git a/rmf_task/include/rmf_task/Parameters.hpp b/rmf_task/include/rmf_task/Parameters.hpp index 8a105a76..55b36b29 100644 --- a/rmf_task/include/rmf_task/Parameters.hpp +++ b/rmf_task/include/rmf_task/Parameters.hpp @@ -99,9 +99,11 @@ class Parameters private: rmf_utils::impl_ptr _pimpl; - }; +//============================================================================== +using ConstParametersPtr = std::shared_ptr; + } // namespace rmf_task #endif // RMF_TASK__AGV__PARAMETERS_HPP diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 1301d2ae..66569e31 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -101,6 +101,13 @@ class Phase::Completed { public: + /// Constructor + Completed( + ConstTagPtr tag_, + Log::View log_, + rmf_traffic::Time start_, + rmf_traffic::Time finish_); + /// Tag of the phase const ConstTagPtr& tag() const; diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 4613d138..10e17f10 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -72,6 +72,58 @@ rmf_traffic::Duration Phase::Tag::original_duration_estimate() const return _pimpl->duration; } +//============================================================================== +class Phase::Completed::Implementation +{ +public: + + ConstTagPtr tag; + Log::View log; + rmf_traffic::Time start; + rmf_traffic::Time finish; +}; + +//============================================================================== +Phase::Completed::Completed( + ConstTagPtr tag_, + Log::View log_, + rmf_traffic::Time start_, + rmf_traffic::Time finish_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(tag_), + std::move(log_), + start_, + finish_ + })) +{ + // Do nothing +} + +//============================================================================== +auto Phase::Completed::tag() const -> const ConstTagPtr& +{ + return _pimpl->tag; +} + +//============================================================================== +const Log::View& Phase::Completed::log() const +{ + return _pimpl->log; +} + +//============================================================================== +rmf_traffic::Time Phase::Completed::start_time() const +{ + return _pimpl->start; +} + +//============================================================================== +rmf_traffic::Time Phase::Completed::finish_time() const +{ + return _pimpl->finish; +} + //============================================================================== class Phase::Pending::Implementation { diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 697b5ed5..a63f8d9c 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -26,6 +26,8 @@ #include #include +#include + #include namespace rmf_task_sequence { @@ -115,6 +117,7 @@ class Phase::Active : public rmf_task::Phase::Active //============================================================================== class Phase::Activator { +public: /// Construct an empty Activator Activator(); @@ -123,6 +126,12 @@ class Phase::Activator /// \tparam Description /// A class that implements the sequence::PhaseDescription interface /// + /// \param[in] get_state + /// A callback for getting the current state of the robot + /// + /// \param[in] tag + /// The tag of this phase + /// /// \param[in] description /// An immutable reference to the relevant Description instance /// @@ -145,10 +154,12 @@ class Phase::Activator using Activate = std::function< ActivePtr( + std::function get_state, + ConstTagPtr tag, const Description& description, std::optional backup_state, std::function update, - std::function finished) + std::function finished) >; /// Add a callback to convert from a PhaseDescription into an active phase. @@ -160,6 +171,12 @@ class Phase::Activator /// Activate a phase based on a description of the phase. /// + /// \param[in] get_state + /// A callback for getting the current state of the robot + /// + /// \param[in] tag + /// The tag of this phase + /// /// \param[in] description /// The description of the phase /// @@ -168,17 +185,24 @@ class Phase::Activator /// The callback will be given a snapshot of the active phase. /// /// \param[in] finished - /// A callback that will be triggered when the task has finished. The - /// callback will be given a copy of the completed phase. + /// A callback that will be triggered when the phase has finished. /// /// \return an active, running instance of the described phase. ActivePtr activate( + std::function get_state, + ConstTagPtr tag, const Description& description, std::function update, - std::function finished) const; + std::function finished) const; /// Restore a phase based on a description of the phase and its backup state. /// + /// \param[in] get_state + /// A callback for getting the current state of the robot + /// + /// \param[in] tag + /// The tag of this phase + /// /// \param[in] description /// The description of the phase /// @@ -190,15 +214,16 @@ class Phase::Activator /// The callback will be given a snapshot of the active phase. /// /// \param[in] finished - /// A callback that will be triggered when the task has finished. The - /// callback will be given a copy of the completed phase. + /// A callback that will be triggered when the phase has finished. /// /// \return an active, running instance of the described phase. ActivePtr restore( + std::function get_state, + ConstTagPtr tag, const Description& description, const std::string& backup_state, std::function update, - std::function finished) const; + std::function finished) const; class Implementation; private: @@ -222,8 +247,8 @@ class Phase::Description /// /// \return a model based on the given start state and parameters. virtual ConstModelPtr make_model( - rmf_task::State invariant_initial_state, - const rmf_task::Parameters& parameters) const = 0; + State invariant_initial_state, + const Parameters& parameters) const = 0; /// Get the human-friendly information about this phase /// @@ -234,8 +259,8 @@ class Phase::Description /// Constraints on the robot during the phase virtual rmf_task::Phase::ConstTagPtr make_tag( rmf_task::Phase::Tag::Id id, - const rmf_task::State& initial_state, - const rmf_task::Parameters& parameters) const = 0; + const State& initial_state, + const Parameters& parameters) const = 0; /// Serialize this phase description into a string. virtual YAML::Node serialize() const = 0; @@ -260,17 +285,17 @@ class Phase::Model /// \param[in] /// /// \return an estimated state for the robot when the phase is finished. - virtual std::optional estimate_finish( - rmf_task::State initial_state, - const rmf_task::Constraints& constraints, - const rmf_task::TravelEstimator& travel_estimator) const = 0; + virtual std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const = 0; /// Estimate the invariant component of the request's duration. virtual rmf_traffic::Duration invariant_duration() const = 0; /// Get the components of the finish state that this phase is guaranteed to /// result in once the phase is finished. - virtual rmf_task::State invariant_finish_state() const = 0; + virtual State invariant_finish_state() const = 0; // Virtual destructor virtual ~Model() = default; @@ -302,20 +327,20 @@ class Phase::SequenceModel : public Phase::Model /// \return A Phase::Model implemented as a SequenceModel. static Phase::ConstModelPtr make( const std::vector& descriptions, - rmf_task::State invariant_initial_state, - const rmf_task::Parameters& parameters); + State invariant_initial_state, + const Parameters& parameters); // Documentation inherited std::optional estimate_finish( - rmf_task::State initial_state, - const rmf_task::Constraints& constraints, - const rmf_task::TravelEstimator& travel_estimator) const final; + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; // Documentation inherited - rmf_task::State invariant_finish_state() const final; + State invariant_finish_state() const final; class Implementation; private: diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index 4e2d05a8..c7959ef6 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -23,6 +23,8 @@ #include #include +#include + namespace rmf_task_sequence { //============================================================================== @@ -47,12 +49,21 @@ class Task : public rmf_task::Task using TaskFinished = std::function; /// Make an activator for a phase sequence task. This activator can be given - /// to + /// to the rmf_task::Activator class to activate phase sequence tasks from + /// phase sequence descriptions. /// /// \param[in] phase_activator - /// A phase activator + /// A phase activator. It is recommended to fully initialize this phase + /// activator (add all supported phases) before passing it to this function. + /// The task activator will keep a reference to this phase activator, so + /// modifying it while a task is activating a phase could cause data races + /// and therefore undefined behavior. + /// + /// \param[in] clock + /// A callback that gives the current time when called. static rmf_task::Activator::Activate make_activator( - Phase::ConstActivatorPtr phase_activator); + Phase::ConstActivatorPtr phase_activator, + std::function clock); }; @@ -101,7 +112,7 @@ class Task::Description : public rmf_task::Task::Description // Documentation inherited Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, - const rmf_task::Parameters& parameters) const final; + const Parameters& parameters) const final; class Implementation; private: @@ -115,9 +126,9 @@ class Task::Model : public rmf_task::Task::Model // Documentation inherited std::optional estimate_finish( - const rmf_task::State& initial_state, - const rmf_task::Constraints& task_planning_constraints, - const rmf_task::TravelEstimator& travel_estimator) const final; + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp index 051b3afc..a25439f8 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp @@ -21,10 +21,9 @@ #include #include -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -98,11 +97,14 @@ class DropOff::Description : public Phase::Description const Parameters& parameters) const final; // Documentation inherited - execute::Phase::ConstTagPtr make_tag( - execute::Phase::Tag::Id id, + Phase::ConstTagPtr make_tag( + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const final; + // Documentation inherited + YAML::Node serialize() const final; + class Implementation; private: Description(); @@ -110,7 +112,6 @@ class DropOff::Description : public Phase::Description }; } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence #endif // RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp index 64ea13e8..65fd0d13 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp @@ -46,6 +46,16 @@ class GoToPlace::Description : public Phase::Description /// Make a GoToPlace description using a goal. static DescriptionPtr make(Goal goal); + /// Get the current goal for this description. + const Goal& goal() const; + + /// Set the current goal for this description. + Description& goal(Goal new_goal); + + /// Get the name of the goal. If the goal does not have an explicit name, it + /// will be referred to as "#x" where "x" is the index number of the waypoint. + std::string goal_name(const rmf_task::Parameters& parameters) const; + // Documentation inherited Phase::ConstModelPtr make_model( rmf_task::State invariant_initial_state, @@ -57,15 +67,8 @@ class GoToPlace::Description : public Phase::Description const rmf_task::State& initial_state, const rmf_task::Parameters& parameters) const final; - /// Get the current goal for this description. - const Goal& goal() const; - - /// Set the current goal for this description. - Description& goal(Goal new_goal); - - /// Get the name of the goal. If the goal does not have an explicit name, it - /// will be referred to as "#x" where "x" is the index number of the waypoint. - std::string goal_name(const rmf_task::Parameters& parameters) const; + // Documentation inherited + YAML::Node serialize() const final; class Implementation; private: diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp index dd019700..0b3f194c 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp @@ -21,10 +21,9 @@ #include #include -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -98,11 +97,14 @@ class PickUp::Description : public Phase::Description const Parameters& parameters) const final; // Documentation inherited - execute::Phase::ConstTagPtr make_tag( - execute::Phase::Tag::Id id, + Phase::ConstTagPtr make_tag( + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const final; + // Documentation inherited + YAML::Node serialize() const final; + class Implementation; private: Description(); @@ -110,7 +112,6 @@ class PickUp::Description : public Phase::Description }; } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence #endif // RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp new file mode 100644 index 00000000..a2d4f2c2 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__TYPEDEFS_HPP +#define RMF_TASK_SEQUENCE__TYPEDEFS_HPP + +#include +#include +#include +#include +#include + +namespace rmf_task_sequence { + +using State = rmf_task::State; +using Parameters = rmf_task::Parameters; +using ConstParametersPtr = rmf_task::ConstParametersPtr; +using Constraints = rmf_task::Constraints; +using TravelEstimator = rmf_task::TravelEstimator; +using Payload = rmf_task::Payload; + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__TYPEDEFS_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index faea2085..958c2936 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -64,6 +64,9 @@ class Task::Active static Task::ActivePtr make( Phase::ConstActivatorPtr phase_activator, + std::function clock, + std::function get_state, + const ConstParametersPtr& parameters, const ConstBookingPtr& booking, const Description& description, std::optional backup_state, @@ -74,6 +77,9 @@ class Task::Active auto task = std::shared_ptr( new Active( std::move(phase_activator), + std::move(clock), + std::move(get_state), + parameters, booking, description, std::move(update), @@ -83,8 +89,8 @@ class Task::Active // TODO(MXG): Make use of backup_state to fast forward the task to the // relevant stage - task->generate_pending_phases(); - task->begin_next_stage(); + task->_generate_pending_phases(); + task->_begin_next_stage(); return task; } @@ -122,20 +128,31 @@ class Task::Active // Documentation inherited void rewind(uint64_t phase_id) final; + /// Get a weak reference to this object + std::weak_ptr weak_from_this() const; + private: - void generate_pending_phases(); + void _generate_pending_phases(); - void begin_next_stage(); + void _finish_phase(); + void _begin_next_stage(); + void _finish_task(); Active( Phase::ConstActivatorPtr phase_activator, + std::function clock, + std::function get_state, + const ConstParametersPtr& parameters, const ConstBookingPtr& booking, const Description& description, std::function update, std::function phase_finished, std::function task_finished) : _phase_activator(std::move(phase_activator)), + _clock(std::move(clock)), + _get_state(std::move(get_state)), + _parameters(parameters), _booking(std::move(booking)), _update(std::move(update)), _phase_finished(std::move(phase_finished)), @@ -146,6 +163,9 @@ class Task::Active } Phase::ConstActivatorPtr _phase_activator; + std::function _clock; + std::function _get_state; + ConstParametersPtr _parameters; ConstBookingPtr _booking; std::function _update; std::function _phase_finished; @@ -156,7 +176,10 @@ class Task::Active ConstStagePtr _active_stage; Phase::ActivePtr _active_phase; + std::optional _current_phase_start_time; + std::list _completed_stages; + std::vector _completed_phases; std::optional _resume_interrupted_phase; bool _cancelled = false; @@ -164,26 +187,106 @@ class Task::Active }; //============================================================================== -void Task::Active::generate_pending_phases() +void Task::Active::_generate_pending_phases() { + auto state = _get_state(); _pending_phases.reserve(_pending_stages.size()); for (const auto& s : _pending_stages) { - + _pending_phases.push_back( + std::make_shared( + s->description->make_tag(s->id, state, *_parameters))); } } //============================================================================== -void Task::Active::begin_next_stage() +void Task::Active::_finish_phase() { + _completed_stages.push_back(_active_stage); + _active_stage = nullptr; + const auto phase_finish_time = _clock(); + const auto completed_phase = + std::make_shared( + _active_phase->tag(), + _active_phase->finish_condition()->log(), + _current_phase_start_time.value(), + phase_finish_time); + + _completed_phases.push_back(completed_phase); + _phase_finished(completed_phase); + + _begin_next_stage(); } //============================================================================== -auto Task::make_activator(Phase::ConstActivatorPtr phase_activator) --> rmf_task::Activator::Activate +void Task::Active::_begin_next_stage() { + if (_pending_stages.empty()) + return _finish_task(); + + assert(!_pending_phases.empty()); + _active_stage = _pending_stages.front(); + assert(_active_stage->id == _pending_phases.front()->tag()->id()); + + _pending_stages.pop_front(); + auto tag = _pending_phases.front()->tag(); + _pending_phases.erase(_pending_phases.begin()); + + _current_phase_start_time = _clock(); + _active_phase = _phase_activator->activate( + _get_state, + std::move(tag), + *_active_stage->description, + [me = weak_from_this()](Phase::ConstSnapshotPtr snapshot) + { + if (const auto self = me.lock()) + self->_update(snapshot); + }, + [me = weak_from_this()]() + { + if (const auto self = me.lock()) + self->_finish_phase(); + }); +} + +//============================================================================== +void Task::Active::_finish_task() +{ + _task_finished(); +} +//============================================================================== +auto Task::make_activator( + Phase::ConstActivatorPtr phase_activator, + std::function clock) +-> rmf_task::Activator::Activate +{ + return [ + phase_activator = std::move(phase_activator), + clock = std::move(clock) + ]( + std::function get_state, + const ConstParametersPtr& parameters, + const ConstBookingPtr& booking, + const Description& description, + std::optional backup_state, + std::function update, + std::function phase_finished, + std::function task_finished) -> ActivePtr + { + return Active::make( + phase_activator, + clock, + std::move(get_state), + parameters, + booking, + description, + std::move(backup_state), + std::move(update), + std::move(phase_finished), + std::move(task_finished)); + }; } } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp index baaff516..28301f6e 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp @@ -17,10 +17,9 @@ #include "internal_PayloadTransfer.hpp" -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -115,8 +114,8 @@ Phase::ConstModelPtr DropOff::Description::make_model( } //============================================================================== -execute::Phase::ConstTagPtr DropOff::Description::make_tag( - execute::Phase::Tag::Id id, +Phase::ConstTagPtr DropOff::Description::make_tag( + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const { @@ -130,5 +129,4 @@ DropOff::Description::Description() } } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp index 8ab547ef..4d7aa24f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp @@ -15,10 +15,9 @@ * */ -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { namespace { @@ -212,8 +211,8 @@ Phase::ConstModelPtr GoToPlace::Description::make_model( } //============================================================================== -execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( - execute::Phase::Tag::Id id, +Phase::ConstTagPtr GoToPlace::Description::make_tag( + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const { @@ -240,7 +239,7 @@ execute::Phase::ConstTagPtr GoToPlace::Description::make_tag( if (!estimate.has_value()) return nullptr; - return std::make_shared( + return std::make_shared( id, "Go to [" + goal_name_ + "]", "Moving the robot from [" + start_name + "] to [" + goal_name_ + "]", @@ -274,5 +273,4 @@ GoToPlace::Description::Description() } } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp index c6f02e49..114d54b0 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp @@ -17,8 +17,7 @@ #include "internal_PayloadTransfer.hpp" -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -47,15 +46,15 @@ Phase::ConstModelPtr PayloadTransfer::make_model( } //============================================================================== -execute::Phase::ConstTagPtr PayloadTransfer::make_tag( +Phase::ConstTagPtr PayloadTransfer::make_tag( const std::string& type, - execute::Phase::Tag::Id id, + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const { const auto model = make_model(initial_state, parameters); - return std::make_shared( + return std::make_shared( id, type, type + " " + payload.brief("into") + " at " @@ -64,5 +63,4 @@ execute::Phase::ConstTagPtr PayloadTransfer::make_tag( } } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp index 728ef561..b0ba20da 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp @@ -17,10 +17,9 @@ #include "internal_PayloadTransfer.hpp" -#include +#include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -114,8 +113,8 @@ Phase::ConstModelPtr PickUp::Description::make_model( } //============================================================================== -execute::Phase::ConstTagPtr PickUp::Description::make_tag( - execute::Phase::Tag::Id id, +Phase::ConstTagPtr PickUp::Description::make_tag( + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const { @@ -129,5 +128,4 @@ PickUp::Description::Description() } } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp index c9d62922..31cbdc1d 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp @@ -89,13 +89,13 @@ Phase::ConstModelPtr WaitFor::Description::make_model( } //============================================================================== -execute::Phase::ConstTagPtr WaitFor::Description::make_tag( - execute::Phase::Tag::Id id, const State&, const Parameters&) const +Phase::ConstTagPtr WaitFor::Description::make_tag( + Phase::Tag::Id id, const State&, const Parameters&) const { const auto seconds = std::chrono::duration_cast( _pimpl->duration); - return std::make_shared( + return std::make_shared( id, "Waiting", "Waiting for [" + std::to_string(seconds.count()) + "] seconds to elapse", diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp index 446a25be..6a662636 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp @@ -19,14 +19,13 @@ #define SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP #include -#include -#include -#include +#include +#include +#include #include -namespace rmf_task { -namespace sequence { +namespace rmf_task_sequence { namespace phases { //============================================================================== @@ -52,15 +51,14 @@ class PayloadTransfer State invariant_initial_state, const Parameters& parameters) const; - execute::Phase::ConstTagPtr make_tag( + Phase::ConstTagPtr make_tag( const std::string& type, - execute::Phase::Tag::Id id, + Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const; }; } // namespace phases -} // namespace sequence -} // namespace rmf_task +} // namespace rmf_task_sequence #endif // SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP From 820cc333174a1db77c3e56e1717196d11896d48c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 13 Sep 2021 15:46:42 +0800 Subject: [PATCH 20/85] Thinking about how to implement backups for phase sequence tasks Signed-off-by: Michael X. Grey --- rmf_task_sequence/src/rmf_task_sequence/Task.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 958c2936..1d0eaa74 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -86,9 +86,7 @@ class Task::Active std::move(phase_finished), std::move(task_finished))); - // TODO(MXG): Make use of backup_state to fast forward the task to the - // relevant stage - + task->_load_backup(std::move(backup_state)); task->_generate_pending_phases(); task->_begin_next_stage(); @@ -133,6 +131,10 @@ class Task::Active private: + /// _load_backup should only be used in the make(~) function. It will + /// fast-forward the progress of the task to catch up to a backed up state, + /// since the task is being restored from a task that was already in progress. + void _load_backup(std::optional backup_state); void _generate_pending_phases(); void _finish_phase(); @@ -186,6 +188,12 @@ class Task::Active bool _killed = false; }; +//============================================================================== +void Task::Active::_load_backup(std::optional backup_state_opt) +{ + +} + //============================================================================== void Task::Active::_generate_pending_phases() { From 5e14e822bd77645879fa5e37fe7f2db6e8b98d56 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 13 Sep 2021 18:06:18 +0800 Subject: [PATCH 21/85] Work reactive backup callbacks into the system Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Activator.hpp | 28 ++++- rmf_task/include/rmf_task/detail/Backup.hpp | 4 +- .../rmf_task/detail/impl_Activator.hpp | 55 +++++++++ rmf_task/src/rmf_task/Activator.cpp | 104 ++++++++++++++++++ .../include/rmf_task_sequence/Phase.hpp | 33 +++++- .../src/rmf_task_sequence/Task.cpp | 20 ++++ 6 files changed, 237 insertions(+), 7 deletions(-) create mode 100644 rmf_task/include/rmf_task/detail/impl_Activator.hpp create mode 100644 rmf_task/src/rmf_task/Activator.cpp diff --git a/rmf_task/include/rmf_task/Activator.hpp b/rmf_task/include/rmf_task/Activator.hpp index b75bb4d4..41e5c0e5 100644 --- a/rmf_task/include/rmf_task/Activator.hpp +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -40,7 +40,7 @@ class Activator /// A callback for retrieving the current state of the robot /// /// \param[in] parameters - /// A reference to the parameters for the robot + /// A reference to the parameters for the roboth /// /// \param[in] booking /// An immutable reference to the booking information for the task @@ -57,6 +57,10 @@ class Activator /// A callback that will be triggered when the task has a significant /// update in its status. /// + /// \param[in] checkpoint + /// A callback that will be triggered when the task has reached a task + /// checkpoint whose state is worth backing up. + /// /// \param[in] finished /// A callback that will be triggered when the task has finished. /// @@ -71,6 +75,7 @@ class Activator const Description& description, std::optional backup_state, std::function update, + std::function checkpoint, std::function phase_finished, std::function task_finished) >; @@ -99,6 +104,10 @@ class Activator /// \param[in] update /// A callback that will be triggered when the task has a significant update /// + /// \param[in] checkpoint + /// A callback that will be triggered when the task has reached a task + /// checkpoint whose state is worth backing up. + /// /// \param[in] phase_finished /// A callback that will be triggered whenever a task phase is finished /// @@ -106,11 +115,12 @@ class Activator /// A callback that will be triggered when the task has finished /// /// \return an active, running instance of the requested task. - std::shared_ptr activate( + Task::ActivePtr activate( std::function get_state, const ConstParametersPtr& parameters, const Request& request, std::function update, + std::function checkpoint, std::function phase_finished, std::function task_finished); @@ -131,6 +141,10 @@ class Activator /// \param[in] update /// A callback that will be triggered when the task has a significant update /// + /// \param[in] checkpoint + /// A callback that will be triggered when the task has reached a task + /// checkpoint whose state is worth backing up. + /// /// \param[in] phase_finished /// A callback that will be triggered whenever a task phase is finished /// @@ -138,22 +152,30 @@ class Activator /// A callback that will be triggered when the task has finished /// /// \return an active, running instance of the requested task. - std::shared_ptr restore( + Task::ActivePtr restore( std::function get_state, const ConstParametersPtr& parameters, const Request& request, std::string backup_state, std::function update, + std::function checkpoint, std::function phase_finished, std::function task_finished); class Implementation; private: + + /// \private + void _add_activator( + std::type_index type, + Activate activator); + rmf_utils::impl_ptr _pimpl; }; } // namespace rmf_task +#include #endif // RMF_TASK__ACTIVATOR_HPP diff --git a/rmf_task/include/rmf_task/detail/Backup.hpp b/rmf_task/include/rmf_task/detail/Backup.hpp index 7dd0a1d8..5a9ad720 100644 --- a/rmf_task/include/rmf_task/detail/Backup.hpp +++ b/rmf_task/include/rmf_task/detail/Backup.hpp @@ -30,7 +30,7 @@ namespace detail { class Backup { public: - /// Back a Backup state + /// Make a Backup state /// /// \param[in] seq /// Sequence number. The Backup from this phase with the highest sequence @@ -38,7 +38,7 @@ class Backup /// issued. /// /// \param[in] state - /// A serialization of the phase's state. This will be used by TaskActivator + /// A serialization of the phase's state. This will be used by Activator /// when restoring a Task. static Backup make(uint64_t seq, std::string state); diff --git a/rmf_task/include/rmf_task/detail/impl_Activator.hpp b/rmf_task/include/rmf_task/detail/impl_Activator.hpp new file mode 100644 index 00000000..57a496b3 --- /dev/null +++ b/rmf_task/include/rmf_task/detail/impl_Activator.hpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__DETAIL__IMPL_ACTIVATOR_HPP +#define RMF_TASK__DETAIL__IMPL_ACTIVATOR_HPP + +#include + +namespace rmf_task { + +//============================================================================== +template +void Activator::add_activator(Activate activator) +{ + _add_activator( + typeid(Description), + [activator]( + std::function get_state, + const Task::ConstBookingPtr& booking, + const Task::Description& description, + std::optional backup_state, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) -> Task::ActivePtr + { + return activator( + std::move(get_state), + booking, + static_cast(description), + std::move(backup_state), + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished)); + }); +} + +} // namespace rmf_task + +#endif // RMF_TASK__DETAIL__IMPL_ACTIVATOR_HPP diff --git a/rmf_task/src/rmf_task/Activator.cpp b/rmf_task/src/rmf_task/Activator.cpp new file mode 100644 index 00000000..a2316f45 --- /dev/null +++ b/rmf_task/src/rmf_task/Activator.cpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { + +//============================================================================== +class Activator::Implementation +{ +public: + + std::unordered_map> activators; + +}; + +//============================================================================== +Activator::Activator() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +Task::ActivePtr Activator::activate( + std::function get_state, + const ConstParametersPtr& parameters, + const Request& request, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) +{ + // TODO(MXG): Should we issue some kind of error/warning to distinguish + // between a missing description versus a description that doesn't have a + // corresponding activator? Same for the restore(~) function. + if (!request.description()) + return nullptr; + + const auto& desc = *request.description(); + const auto& type = typeid(desc); + const auto it = _pimpl->activators.find(type); + if (it == _pimpl->activators.end()) + return nullptr; + + return it->second( + std::move(get_state), + parameters, + request.booking(), + *request.description(), + std::nullopt, + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished)); +} + +//============================================================================== +Task::ActivePtr Activator::restore( + std::function get_state, + const ConstParametersPtr& parameters, + const Request& request, + std::string backup_state, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) +{ + if (!request.description()) + return nullptr; + + const auto& desc = *request.description(); + const auto& type = typeid(desc); + const auto it = _pimpl->activators.find(type); + if (it == _pimpl->activators.end()) + return nullptr; + + return it->second( + std::move(get_state), + parameters, + request.booking(), + *request.description(), + std::move(backup_state), + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished)); +} + +} // namespace rmf_task diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index a63f8d9c..a9274749 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -62,13 +62,13 @@ class Phase::Active : public rmf_task::Phase::Active public: /// Backup data for the Phase. The state of the phase is represented by a - /// string. The meaning and format of the string is up to the phase + /// YAML::Node. The meaning and format of the Node is up to the phase /// implementation to decide. /// /// Each Backup is tagged with a sequence number. As the Phase makes progress, /// it can issue new Backups with higher sequence numbers. Only the Backup /// with the highest sequence number will be kept. - using Backup = rmf_task::detail::Backup; + class Backup; /// Get a backup for this Phase virtual Backup backup() const = 0; @@ -114,6 +114,25 @@ class Phase::Active : public rmf_task::Phase::Active virtual ~Active() = default; }; +class Phase::Active::Backup +{ +public: + + /// Make a backup of the phase + /// + /// \param[in] seq + /// Sequence number. The Backup from this phase with the highest sequence + /// number will be held onto until a Backup with a higher sequence number is + /// issued. + /// + /// \param[in] state + /// A serialization of the phase's state. This will be used by + /// Phase::Activator when restoring a Task. + static Backup make(uint64_t seq, YAML::Node state); + +private: +}; + //============================================================================== class Phase::Activator { @@ -184,6 +203,10 @@ class Phase::Activator /// A callback that will be triggered when the phase has a notable update. /// The callback will be given a snapshot of the active phase. /// + /// \param[in] checkpoint + /// A callback that will be triggered when the phase has reached a task + /// checkpoint whose state is worth backing up. + /// /// \param[in] finished /// A callback that will be triggered when the phase has finished. /// @@ -193,6 +216,7 @@ class Phase::Activator ConstTagPtr tag, const Description& description, std::function update, + std::function checkpoint, std::function finished) const; /// Restore a phase based on a description of the phase and its backup state. @@ -213,6 +237,10 @@ class Phase::Activator /// A callback that will triggered when the phase has a notable update. /// The callback will be given a snapshot of the active phase. /// + /// \param[in] checkpoint + /// A callback that will be triggered when the phase has reached a task + /// checkpoint whose state is worth backing up. + /// /// \param[in] finished /// A callback that will be triggered when the phase has finished. /// @@ -223,6 +251,7 @@ class Phase::Activator const Description& description, const std::string& backup_state, std::function update, + std::function checkpoint, std::function finished) const; class Implementation; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 1d0eaa74..be96bb28 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -71,6 +71,7 @@ class Task::Active const Description& description, std::optional backup_state, std::function update, + std::function checkpoint, std::function phase_finished, std::function task_finished) { @@ -83,6 +84,7 @@ class Task::Active booking, description, std::move(update), + std::move(checkpoint), std::move(phase_finished), std::move(task_finished))); @@ -141,6 +143,8 @@ class Task::Active void _begin_next_stage(); void _finish_task(); + void _issue_backup(Phase::Active::Backup backup); + Active( Phase::ConstActivatorPtr phase_activator, std::function clock, @@ -149,6 +153,7 @@ class Task::Active const ConstBookingPtr& booking, const Description& description, std::function update, + std::function checkpoint, std::function phase_finished, std::function task_finished) : _phase_activator(std::move(phase_activator)), @@ -157,6 +162,7 @@ class Task::Active _parameters(parameters), _booking(std::move(booking)), _update(std::move(update)), + _checkpoint(std::move(checkpoint)), _phase_finished(std::move(phase_finished)), _task_finished(std::move(task_finished)), _pending_stages(Description::Implementation::get_stages(description)) @@ -170,6 +176,7 @@ class Task::Active ConstParametersPtr _parameters; ConstBookingPtr _booking; std::function _update; + std::function _checkpoint; std::function _phase_finished; std::function _task_finished; @@ -251,6 +258,11 @@ void Task::Active::_begin_next_stage() if (const auto self = me.lock()) self->_update(snapshot); }, + [me = weak_from_this()](Phase::Active::Backup backup) + { + if (const auto self = me.lock()) + self->_issue_backup(std::move(backup)); + }, [me = weak_from_this()]() { if (const auto self = me.lock()) @@ -264,6 +276,12 @@ void Task::Active::_finish_task() _task_finished(); } +//============================================================================== +void Task::Active::_issue_backup(Phase::Active::Backup backup) +{ + +} + //============================================================================== auto Task::make_activator( Phase::ConstActivatorPtr phase_activator, @@ -280,6 +298,7 @@ auto Task::make_activator( const Description& description, std::optional backup_state, std::function update, + std::function checkpoint, std::function phase_finished, std::function task_finished) -> ActivePtr { @@ -292,6 +311,7 @@ auto Task::make_activator( description, std::move(backup_state), std::move(update), + std::move(checkpoint), std::move(phase_finished), std::move(task_finished)); }; From e330d362a81bd16f82a5ef0ef8c1c4d09b38b840 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Sep 2021 01:41:14 +0800 Subject: [PATCH 22/85] Creating minimal example of task activation Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Condition.hpp | 1 + rmf_task/include/rmf_task/Log.hpp | 3 +- rmf_task/include/rmf_task/Phase.hpp | 1 + .../rmf_task/detail/impl_Activator.hpp | 2 + rmf_task/src/rmf_task/Activator.cpp | 8 + rmf_task/src/rmf_task/Condition.cpp | 105 ++++++ rmf_task/src/rmf_task/Log.cpp | 357 ++++++++++++++++++ rmf_task/src/rmf_task/Phase.cpp | 47 +++ rmf_task/test/mock/MockCondition.cpp | 65 ++++ rmf_task/test/mock/MockCondition.hpp | 53 +++ rmf_task/test/mock/MockDelivery.cpp | 50 +++ rmf_task/test/mock/MockDelivery.hpp | 66 ++++ rmf_task/test/mock/MockPhase.cpp | 62 +++ rmf_task/test/mock/MockPhase.hpp | 60 +++ rmf_task/test/mock/MockTask.cpp | 150 ++++++++ rmf_task/test/mock/MockTask.hpp | 95 +++++ rmf_task/test/unit/test_Activator.cpp | 62 +++ 17 files changed, 1186 insertions(+), 1 deletion(-) create mode 100644 rmf_task/src/rmf_task/Condition.cpp create mode 100644 rmf_task/src/rmf_task/Log.cpp create mode 100644 rmf_task/test/mock/MockCondition.cpp create mode 100644 rmf_task/test/mock/MockCondition.hpp create mode 100644 rmf_task/test/mock/MockDelivery.cpp create mode 100644 rmf_task/test/mock/MockDelivery.hpp create mode 100644 rmf_task/test/mock/MockPhase.cpp create mode 100644 rmf_task/test/mock/MockPhase.hpp create mode 100644 rmf_task/test/mock/MockTask.cpp create mode 100644 rmf_task/test/mock/MockTask.hpp create mode 100644 rmf_task/test/unit/test_Activator.cpp diff --git a/rmf_task/include/rmf_task/Condition.hpp b/rmf_task/include/rmf_task/Condition.hpp index 921793e2..19ba623b 100644 --- a/rmf_task/include/rmf_task/Condition.hpp +++ b/rmf_task/include/rmf_task/Condition.hpp @@ -137,6 +137,7 @@ class Condition::Snapshot : public Condition class Implementation; private: + Snapshot(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task/include/rmf_task/Log.hpp b/rmf_task/include/rmf_task/Log.hpp index ce110a30..842a20c7 100644 --- a/rmf_task/include/rmf_task/Log.hpp +++ b/rmf_task/include/rmf_task/Log.hpp @@ -196,13 +196,14 @@ class Log::Reader::Iterable::iterator /// that is equal to Log::Reader::Iterable::end(). /// /// \return a copy of the iterator before it was incremented. - iterator& operator++(int); + iterator operator++(int); /// Equality comparison operator bool operator==(const iterator& other) const; class Implementation; private: + iterator(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 66569e31..c45ea309 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -162,6 +162,7 @@ class Phase::Snapshot : public Phase::Active class Implementation; private: + Snapshot(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task/include/rmf_task/detail/impl_Activator.hpp b/rmf_task/include/rmf_task/detail/impl_Activator.hpp index 57a496b3..dd96a41a 100644 --- a/rmf_task/include/rmf_task/detail/impl_Activator.hpp +++ b/rmf_task/include/rmf_task/detail/impl_Activator.hpp @@ -30,6 +30,7 @@ void Activator::add_activator(Activate activator) typeid(Description), [activator]( std::function get_state, + const ConstParametersPtr& parameters, const Task::ConstBookingPtr& booking, const Task::Description& description, std::optional backup_state, @@ -40,6 +41,7 @@ void Activator::add_activator(Activate activator) { return activator( std::move(get_state), + parameters, booking, static_cast(description), std::move(backup_state), diff --git a/rmf_task/src/rmf_task/Activator.cpp b/rmf_task/src/rmf_task/Activator.cpp index a2316f45..ac428265 100644 --- a/rmf_task/src/rmf_task/Activator.cpp +++ b/rmf_task/src/rmf_task/Activator.cpp @@ -101,4 +101,12 @@ Task::ActivePtr Activator::restore( std::move(task_finished)); } +//============================================================================== +void Activator::_add_activator( + std::type_index type, + Activate activator) +{ + _pimpl->activators.insert({type, std::move(activator)}); +} + } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Condition.cpp b/rmf_task/src/rmf_task/Condition.cpp new file mode 100644 index 00000000..6f9ee122 --- /dev/null +++ b/rmf_task/src/rmf_task/Condition.cpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { + +namespace { +//============================================================================== +std::vector snapshot_conditions( + const std::vector& queue) +{ + // NOTE(MXG): This implementation is using recursion. That should be fine + // since I don't expect much depth in the trees of subconditions, but we may + // want to revisit this and implement it as a queue instead if we ever find + // a use-case with deep recursion. + std::vector output; + output.reserve(queue.size()); + for (const auto& c : queue) + output.push_back(Condition::Snapshot::make(*c)); + + return output; +} +} // anonymous namespace + +//============================================================================== +class Condition::Snapshot::Implementation +{ +public: + + Status status; + std::string name; + std::string detail; + Log::View log; + std::vector subconditions; + +}; + +//============================================================================== +ConstConditionPtr Condition::Snapshot::make(const Condition& other) +{ + Snapshot output; + output._pimpl = rmf_utils::make_impl( + Implementation{ + other.status(), + other.name(), + other.detail(), + other.log(), + snapshot_conditions(other.subconditions()) + }); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +auto Condition::Snapshot::status() const -> Status +{ + return _pimpl->status; +} + +//============================================================================== +std::string Condition::Snapshot::name() const +{ + return _pimpl->name; +} + +//============================================================================== +std::string Condition::Snapshot::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +Log::View Condition::Snapshot::log() const +{ + return _pimpl->log; +} + +//============================================================================== +std::vector Condition::Snapshot::subconditions() const +{ + return _pimpl->subconditions; +} + +//============================================================================== +Condition::Snapshot::Snapshot() +{ + // Do nothing +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Log.cpp b/rmf_task/src/rmf_task/Log.cpp new file mode 100644 index 00000000..eab73595 --- /dev/null +++ b/rmf_task/src/rmf_task/Log.cpp @@ -0,0 +1,357 @@ +/* + * Copyright (C) 2021 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 + +#include + +namespace rmf_task { + +//============================================================================== +class Log::Implementation +{ +public: + std::function clock; + std::shared_ptr> entries; + + Implementation(std::function clock_) + : clock(std::move(clock_)), + entries(std::make_shared>()) + { + if (!clock) + { + clock = [](){ return std::chrono::system_clock::now(); }; + } + } + +}; + +//============================================================================== +class Log::Entry::Implementation +{ +public: + + static Entry make( + Tier tier, + std::chrono::system_clock::time_point time, + std::string text) + { + Log::Entry output; + output._pimpl = rmf_utils::make_impl( + Implementation{ + tier, + time, + std::move(text) + }); + + return output; + } + + Tier tier; + std::chrono::system_clock::time_point time; + std::string text; + +}; + +//============================================================================== +class Log::View::Implementation +{ +public: + + static View make(const Log& log) + { + View output; + + if (log._pimpl->entries->empty()) + { + output._pimpl = rmf_utils::make_impl( + Implementation{log._pimpl->entries, std::nullopt, std::nullopt}); + } + else + { + output._pimpl = rmf_utils::make_impl( + Implementation{ + log._pimpl->entries, + log._pimpl->entries->cbegin(), + --log._pimpl->entries->cend() + }); + } + + return output; + } + + static const Implementation& get(const View& view) + { + return *view._pimpl; + } + + std::shared_ptr> shared; + + /// begin is the iterator for the first entry in the entire log + std::optional::const_iterator> begin; + + /// last is the iterator for the last entry that will be provided by this + /// view. This is NOT the usual end() iterator, but instead it is one-before + /// the usual end() iterator. + std::optional::const_iterator> last; +}; + +//============================================================================== +class Log::Reader::Implementation +{ +public: + + struct Memory + { + std::weak_ptr> weak; + std::optional::const_iterator> last; + + Memory() + { + // Do nothing + } + }; + + std::unordered_map memories; + + Iterable iterate(const View& view); +}; + +//============================================================================== +class Log::Reader::Iterable::Implementation +{ +public: + std::shared_ptr> shared; + std::optional begin; + + static Log::Reader::Iterable make( + std::shared_ptr> shared, + std::optional::const_iterator> begin, + std::optional::const_iterator> last); +}; + +//============================================================================== +class Log::Reader::Iterable::iterator::Implementation +{ +public: + std::list::const_iterator it; + std::list::const_iterator last; + + static iterator make( + std::list::const_iterator it, + std::list::const_iterator last) + { + iterator output; + output._pimpl = rmf_utils::make_impl( + Implementation{std::move(it), std::move(last)}); + + return output; + } + + static iterator end() + { + return iterator(); + } +}; + +//============================================================================== +Log::Reader::Iterable Log::Reader::Iterable::Implementation::make( + std::shared_ptr> shared, + std::optional::const_iterator> begin, + std::optional::const_iterator> last) +{ + Iterable iterable; + iterable._pimpl = rmf_utils::make_impl(); + iterable._pimpl->shared = std::move(shared); + if (begin.has_value()) + { + iterable._pimpl->begin = + iterator::Implementation::make(*begin, last.value()); + } + else + { + iterable._pimpl->begin = iterator::Implementation::end(); + } + + return iterable; +} + +//============================================================================== +auto Log::Reader::Implementation::iterate(const View& view) -> Iterable +{ + const auto& v = View::Implementation::get(view); + const auto it = memories.insert({v.shared.get(), Memory()}).first; + auto& memory = it->second; + if (memory.weak.lock()) + { + if (!memory.last.has_value()) + memory.last = v.begin; + } + else + { + memory.weak = v.shared; + memory.last = v.begin; + } + + auto iterable = Iterable::Implementation::make( + v.shared, memory.last, v.last); + + memory.last = v.last; + + return iterable; +} + +//============================================================================== +Log::Log( + std::function clock) +: _pimpl(rmf_utils::make_impl(std::move(clock))) +{ + // Do nothing +} + +//============================================================================== +void Log::info(std::string text) +{ + _pimpl->entries->emplace_back( + Entry::Implementation::make( + Entry::Tier::Info, _pimpl->clock(), std::move(text))); +} + +//============================================================================== +void Log::warn(std::string text) +{ + _pimpl->entries->emplace_back( + Entry::Implementation::make( + Entry::Tier::Warning, _pimpl->clock(), std::move(text))); +} + +//============================================================================== +void Log::error(std::string text) +{ + _pimpl->entries->emplace_back( + Entry::Implementation::make( + Entry::Tier::Error, _pimpl->clock(), std::move(text))); +} + +//============================================================================== +void Log::insert(Log::Entry entry) +{ + _pimpl->entries->emplace_back(std::move(entry)); +} + +//============================================================================== +Log::View Log::view() const +{ + return View::Implementation::make(*this); +} + +//============================================================================== +auto Log::Entry::tier() const -> Tier +{ + return _pimpl->tier; +} + +//============================================================================== +std::chrono::system_clock::time_point Log::Entry::time() const +{ + return _pimpl->time; +} + +//============================================================================== +const std::string& Log::Entry::text() const +{ + return _pimpl->text; +} + +//============================================================================== +Log::Entry::Entry() +{ + // Do nothing +} + +//============================================================================== +Log::View::View() +{ + // Do nothing +} + +//============================================================================== +Log::Reader::Reader() +: _pimpl(rmf_utils::make_unique_impl()) +{ + // Do nothing +} + +//============================================================================== +auto Log::Reader::Iterable::begin() const -> iterator +{ + return _pimpl->begin.value_or(iterator::Implementation::end()); +} + +//============================================================================== +auto Log::Reader::Iterable::end() const -> iterator +{ + return iterator::Implementation::end(); +} + +//============================================================================== +auto Log::Reader::Iterable::iterator::operator*() const -> const Entry& +{ + return *_pimpl->it; +} + +//============================================================================== +auto Log::Reader::Iterable::iterator::operator->() const -> const Entry* +{ + return &(*_pimpl->it); +} + +//============================================================================== +auto Log::Reader::Iterable::iterator::operator++() -> iterator& +{ + if (_pimpl->it == _pimpl->last) + _pimpl = nullptr; + else + ++_pimpl->it; + + return *this; +} + +//============================================================================== +auto Log::Reader::Iterable::iterator::operator++(int) -> iterator +{ + auto original = *this; + ++(*this); + return original; +} + +//============================================================================== +bool Log::Reader::Iterable::iterator::operator==(const iterator& other) const +{ + if (!_pimpl || !other._pimpl) + return _pimpl == other._pimpl; + + return _pimpl->it == other._pimpl->it; +} + +//============================================================================== +Log::Reader::Iterable::iterator::iterator() +{ + // Do nothing +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 10e17f10..85f7a6f1 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -124,6 +124,53 @@ rmf_traffic::Time Phase::Completed::finish_time() const return _pimpl->finish; } +//============================================================================== +class Phase::Snapshot::Implementation +{ +public: + ConstTagPtr tag; + ConstConditionPtr finish_condition; + rmf_traffic::Time estimated_finish_time; +}; + +//============================================================================== +Phase::ConstSnapshotPtr Phase::Snapshot::make(const Active& active) +{ + Snapshot output; + output._pimpl = rmf_utils::make_impl( + Implementation{ + active.tag(), + Condition::Snapshot::make(*active.finish_condition()), + active.estimate_finish_time() + }); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +Phase::ConstTagPtr Phase::Snapshot::tag() const +{ + return _pimpl->tag; +} + +//============================================================================== +ConstConditionPtr Phase::Snapshot::finish_condition() const +{ + return _pimpl->finish_condition; +} + +//============================================================================== +rmf_traffic::Time Phase::Snapshot::estimate_finish_time() const +{ + return _pimpl->estimated_finish_time; +} + +//============================================================================== +Phase::Snapshot::Snapshot() +{ + // Do nothing +} + //============================================================================== class Phase::Pending::Implementation { diff --git a/rmf_task/test/mock/MockCondition.cpp b/rmf_task/test/mock/MockCondition.cpp new file mode 100644 index 00000000..85166d76 --- /dev/null +++ b/rmf_task/test/mock/MockCondition.cpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2021 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 "MockCondition.hpp" + +namespace test_rmf_task { + +//============================================================================== +MockCondition::MockCondition( + std::string name_, + std::string detail_, + Status initial_status) + : _status(initial_status), + _name(std::move(name_)), + _detail(std::move(detail_)) +{ + // Do nothing +} + +//============================================================================== +auto MockCondition::status() const -> Status +{ + return _status; +} + +//============================================================================== +std::string MockCondition::name() const +{ + return _name; +} + +//============================================================================== +std::string MockCondition::detail() const +{ + return _detail; +} + +//============================================================================== +rmf_task::Log::View MockCondition::log() const +{ + return _log.view(); +} + +//============================================================================== +std::vector MockCondition::subconditions() const +{ + return std::vector( + _subconditions.begin(), _subconditions.end()); +} + +} // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockCondition.hpp b/rmf_task/test/mock/MockCondition.hpp new file mode 100644 index 00000000..b3f8ec7e --- /dev/null +++ b/rmf_task/test/mock/MockCondition.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2021 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 TEST__MOCK__MOCKCONDITION_HPP +#define TEST__MOCK__MOCKCONDITION_HPP + +#include + +namespace test_rmf_task { + +//============================================================================== +class MockCondition : public rmf_task::Condition +{ +public: + + MockCondition( + std::string name_, + std::string detail_, + Status initial_status = Status::Standby); + + // Interface + Status status() const final; + std::string name() const final; + std::string detail() const final; + rmf_task::Log::View log() const final; + std::vector subconditions() const final; + + // Fields + Status _status; + std::string _name; + std::string _detail; + rmf_task::Log _log; + std::vector> _subconditions; + +}; + +} // namespace test_rmf_task + +#endif // TEST__MOCK__MOCKCONDITION_HPP diff --git a/rmf_task/test/mock/MockDelivery.cpp b/rmf_task/test/mock/MockDelivery.cpp new file mode 100644 index 00000000..dbdb0d69 --- /dev/null +++ b/rmf_task/test/mock/MockDelivery.cpp @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2021 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 "MockDelivery.hpp" + +namespace test_rmf_task { + +//============================================================================== +auto MockDelivery::make_activator() -> Activator +{ + return []( + std::function get_state, + const rmf_task::ConstParametersPtr& parameters, + const Task::ConstBookingPtr& booking, + const Description& description, + std::optional backup_state, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) -> Task::ActivePtr + { + return std::shared_ptr( + new Active( + description, + std::move(get_state), + parameters, + booking, + std::move(backup_state), + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished))); + }; +} + +} // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockDelivery.hpp b/rmf_task/test/mock/MockDelivery.hpp new file mode 100644 index 00000000..4e7f9ca0 --- /dev/null +++ b/rmf_task/test/mock/MockDelivery.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 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 TEST__MOCK__MOCKDELIVERY_HPP +#define TEST__MOCK__MOCKDELIVERY_HPP + +#include +#include + +#include "MockTask.hpp" + +namespace test_rmf_task { + +//============================================================================== +/// A class that provides a mock implementation of an Active Delivery task +class MockDelivery : public rmf_task::Task +{ +public: + + using Phase = rmf_task::Phase; + using Description = rmf_task::requests::Delivery::Description; + using Activator = rmf_task::Activator::Activate; + + static Activator make_activator(); + + class Active : public MockTask::Active + { + public: + + template + Active( + const Description& desc, Args&&... args) + : MockTask::Active(std::forward(args)...), + _description(desc) + { + // TODO(MXG): We could use the description, state, and parameters to + // get the actual estimate for the pending phases + using namespace std::chrono_literals; + add_pending_phase("Go to pick up", "Pretending to go to a pick up point", 1min); + add_pending_phase("Pick up", "Pretending to pick something up", 30s); + add_pending_phase("Go to drop off", "Pretending to go to a drop off point", 1min); + add_pending_phase("Drop off", "Pretending to drop something off", 30s); + } + + Description _description; + }; + +}; + +} // namespace test_rmf_task + +#endif // TEST__MOCK__MOCKDELIVERY_HPP diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp new file mode 100644 index 00000000..8b443a69 --- /dev/null +++ b/rmf_task/test/mock/MockPhase.cpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2021 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 "MockPhase.hpp" + +namespace test_rmf_task { + +//============================================================================== +MockPhase::Active::Active( + rmf_traffic::Time start_time_, + ConstTagPtr tag_, + std::function update_, + std::function phase_finished_) + : _tag(std::move(tag_)), + _condition(std::make_shared( + "Mock condition", "This is a mocked up condition")), + _start_time(start_time_), + _update(std::move(update_)), + _phase_finished(std::move(phase_finished_)) +{ + // Do nothing +} + +//============================================================================== +rmf_task::Phase::ConstTagPtr MockPhase::Active::tag() const +{ + return _tag; +} + +//============================================================================== +rmf_task::ConstConditionPtr MockPhase::Active::finish_condition() const +{ + return _condition; +} + +//============================================================================== +rmf_traffic::Time MockPhase::Active::estimate_finish_time() const +{ + return _start_time + _tag->original_duration_estimate(); +} + +//============================================================================== +void MockPhase::Active::send_update() const +{ + _update(Phase::Snapshot::make(*this)); +} + +} // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockPhase.hpp b/rmf_task/test/mock/MockPhase.hpp new file mode 100644 index 00000000..9e6051e6 --- /dev/null +++ b/rmf_task/test/mock/MockPhase.hpp @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2021 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 TEST__MOCK__MOCKPHASE_HPP +#define TEST__MOCK__MOCKPHASE_HPP + +#include + +#include "MockCondition.hpp" + +namespace test_rmf_task { + +//============================================================================== +/// A class that provides a mock implementation of a generic phase +class MockPhase : public rmf_task::Phase +{ +public: + + class Active : public rmf_task::Phase::Active + { + public: + + Active( + rmf_traffic::Time start_time_, + ConstTagPtr tag_, + std::function update_, + std::function phase_finished_); + + ConstTagPtr tag() const final; + rmf_task::ConstConditionPtr finish_condition() const final; + rmf_traffic::Time estimate_finish_time() const final; + + void send_update() const; + + ConstTagPtr _tag; + std::shared_ptr _condition; + rmf_traffic::Time _start_time; + std::function _update; + std::function _phase_finished; + }; + +}; + +} // namespace test_rmf_task + +#endif // TEST__MOCK__MOCKPHASE_HPP diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp new file mode 100644 index 00000000..e669d7e6 --- /dev/null +++ b/rmf_task/test/mock/MockTask.cpp @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2021 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 "MockTask.hpp" + +namespace test_rmf_task { + +//============================================================================== +auto MockTask::Active::completed_phases() const +-> const std::vector& +{ + return _completed_phases; +} + +//============================================================================== +auto MockTask::Active::active_phase() const -> Phase::ConstActivePtr +{ + return _active_phase; +} + +//============================================================================== +auto MockTask::Active::pending_phases() const -> std::vector +{ + return _pending_phases; +} + +//============================================================================== +auto MockTask::Active::tag() const -> const ConstTagPtr& +{ + return _ctag; +} + +//============================================================================== +rmf_traffic::Time MockTask::Active::estimate_finish_time() const +{ + return _tag->original_finish_estimate(); +} + +//============================================================================== +auto MockTask::Active::backup() const -> Backup +{ + throw std::runtime_error("MockTask::Active::backup() not implemented"); +} + +//============================================================================== +auto MockTask::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + task_is_interrupted(); + return make_resumer([](){}); +} + +//============================================================================== +void MockTask::Active::cancel() +{ + _pending_phases.clear(); + _task_finished(); +} + +//============================================================================== +void MockTask::Active::kill() +{ + _pending_phases.clear(); + _task_finished(); +} + +//============================================================================== +void MockTask::Active::skip(uint64_t, bool) +{ + throw std::runtime_error("MockTask::Active::skip(~) not implemented"); +} + +//============================================================================== +void MockTask::Active::rewind(uint64_t) +{ + throw std::runtime_error("MockTask::Active::rewind(~) not implemented"); +} + +//============================================================================== +MockTask::Active::Active( + std::function, + const rmf_task::ConstParametersPtr&, + const Task::ConstBookingPtr& booking, + std::optional, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) +: _tag(std::make_shared( + booking, "Mock Task", "Mocked up task", rmf_traffic::Time())), + _ctag(_tag), + _update(std::move(update)), + _checkpoint(std::move(checkpoint)), + _phase_finished(std::move(phase_finished)), + _task_finished(std::move(task_finished)) +{ + // Do nothing +} + +//============================================================================== +void MockTask::Active::add_pending_phase( + std::string name, + std::string detail, + rmf_traffic::Duration estimate) +{ + _pending_phases.emplace_back( + std::make_shared( + _next_phase_id++, std::move(name), std::move(detail), estimate)); +} + +//============================================================================== +void MockTask::Active::start_next_phase(rmf_traffic::Time current_time) +{ + if (_active_phase) + { + _phase_finished( + std::make_shared( + _active_phase->tag(), + _active_phase->finish_condition()->log(), + _active_phase->_start_time, + current_time)); + } + + if (_pending_phases.empty()) + return _task_finished(); + + const auto next_phase = _pending_phases.front(); + _pending_phases.erase(_pending_phases.begin()); + _active_phase = std::make_shared( + current_time, + next_phase.tag(), + [update = _update](Phase::ConstSnapshotPtr u){ update(std::move(u)); }, + [](){}); +} + +} // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp new file mode 100644 index 00000000..442560b9 --- /dev/null +++ b/rmf_task/test/mock/MockTask.hpp @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2021 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 TEST__MOCK__MOCKTASK_HPP +#define TEST__MOCK__MOCKTASK_HPP + +#include + +#include "MockPhase.hpp" + +namespace test_rmf_task { + +//============================================================================== +class MockTask : public rmf_task::Task +{ +public: + + using Phase = rmf_task::Phase; + + class Active : public rmf_task::Task::Active + { + public: + + const std::vector& + completed_phases() const override; + + Phase::ConstActivePtr active_phase() const override; + + std::vector pending_phases() const override; + + const ConstTagPtr& tag() const override; + + rmf_traffic::Time estimate_finish_time() const override; + + Backup backup() const override; + + Resume interrupt(std::function task_is_interrupted) override; + + void cancel() override; + + void kill() override; + + void skip(uint64_t phase_id, bool value = true) override; + + void rewind(uint64_t phase_id) override; + + Active( + std::function get_state, + const rmf_task::ConstParametersPtr& parameters, + const Task::ConstBookingPtr& booking, + std::optional backup_state, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished); + + void add_pending_phase( + std::string name, + std::string detail, + rmf_traffic::Duration estimate); + + void start_next_phase(rmf_traffic::Time current_time); + + std::shared_ptr _tag; + ConstTagPtr _ctag; + std::vector _completed_phases; + std::shared_ptr _active_phase; + std::vector _pending_phases; + std::size_t _next_phase_id = 0; + + std::function _update; + std::function _checkpoint; + std::function _phase_finished; + std::function _task_finished; + }; + +}; + +} // namespace test_rmf_task + +#endif // TEST__MOCK__MOCKTASK_HPP diff --git a/rmf_task/test/unit/test_Activator.cpp b/rmf_task/test/unit/test_Activator.cpp new file mode 100644 index 00000000..871156de --- /dev/null +++ b/rmf_task/test/unit/test_Activator.cpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2021 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 + +#include "../mock/MockDelivery.hpp" + +SCENARIO("Activator") +{ + rmf_task::Activator activator; + activator.add_activator(test_rmf_task::MockDelivery::make_activator()); + + using namespace std::chrono_literals; + auto request = rmf_task::requests::Delivery::make( + 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + + rmf_task::Phase::ConstSnapshotPtr phase_snapshot; + auto active = activator.activate( + nullptr, + nullptr, + *request, + [&phase_snapshot](auto s){ phase_snapshot = s; }, + [](auto) {}, + [](auto) {}, + []() {}); + + REQUIRE(active); + + auto mock_active = + std::dynamic_pointer_cast(active); + + REQUIRE(mock_active); + + CHECK(phase_snapshot == nullptr); + mock_active->start_next_phase(rmf_traffic::Time()); + CHECK(phase_snapshot == nullptr); + + mock_active->_active_phase->send_update(); + REQUIRE(phase_snapshot != nullptr); + CHECK(phase_snapshot->tag()->id() == 0); + + mock_active->start_next_phase(rmf_traffic::Time()); + REQUIRE(phase_snapshot != nullptr); + CHECK(phase_snapshot->tag()->id() == 0); + mock_active->_active_phase->send_update(); + REQUIRE(phase_snapshot != nullptr); + CHECK(phase_snapshot->tag()->id() == 1); +} From d2911aacc5e5ec528e8cb2bf742a1ad53b370b7c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Sep 2021 12:05:21 +0800 Subject: [PATCH 23/85] Create minimal example of task restoring Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Task.hpp | 2 +- rmf_task/src/rmf_task/detail/Backup.cpp | 74 +++++++++++++++++++++++++ rmf_task/test/mock/MockDelivery.cpp | 7 ++- rmf_task/test/mock/MockDelivery.hpp | 13 ++++- rmf_task/test/mock/MockTask.cpp | 1 - rmf_task/test/mock/MockTask.hpp | 1 - rmf_task/test/unit/test_Activator.cpp | 29 +++++++++- 7 files changed, 119 insertions(+), 8 deletions(-) create mode 100644 rmf_task/src/rmf_task/detail/Backup.cpp diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 7b48dbbf..1052bbb1 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -185,7 +185,7 @@ class Task::Active /// Each Backup is tagged with a sequence number. As the Task makes progress, /// it can issue new Backups with higher sequence numbers. Only the Backup /// with the highest sequence number will be kept. - class Backup : public detail::Backup {}; + using Backup = detail::Backup; /// Descriptions of the phases that have been completed virtual const std::vector& diff --git a/rmf_task/src/rmf_task/detail/Backup.cpp b/rmf_task/src/rmf_task/detail/Backup.cpp new file mode 100644 index 00000000..f3531570 --- /dev/null +++ b/rmf_task/src/rmf_task/detail/Backup.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { +namespace detail { + +//============================================================================== +class Backup::Implementation +{ +public: + uint64_t sequence; + std::string state; +}; + +//============================================================================== +Backup Backup::make(uint64_t seq, std::string state) +{ + Backup output; + output._pimpl = rmf_utils::make_impl( + Implementation{seq, std::move(state)}); + + return output; +} + +//============================================================================== +uint64_t Backup::sequence() const +{ + return _pimpl->sequence; +} + +//============================================================================== +Backup& Backup::sequence(uint64_t seq) +{ + _pimpl->sequence = seq; + return *this; +} + +//============================================================================== +const std::string& Backup::state() const +{ + return _pimpl->state; +} + +//============================================================================== +Backup& Backup::state(std::string new_state) +{ + _pimpl->state = std::move(new_state); + return *this; +} + +//============================================================================== +Backup::Backup() +{ + // Do nothing +} + +} // namespace detail +} // namespace rmf_task diff --git a/rmf_task/test/mock/MockDelivery.cpp b/rmf_task/test/mock/MockDelivery.cpp index dbdb0d69..3c5806ca 100644 --- a/rmf_task/test/mock/MockDelivery.cpp +++ b/rmf_task/test/mock/MockDelivery.cpp @@ -36,10 +36,10 @@ auto MockDelivery::make_activator() -> Activator return std::shared_ptr( new Active( description, + std::move(backup_state), std::move(get_state), parameters, booking, - std::move(backup_state), std::move(update), std::move(checkpoint), std::move(phase_finished), @@ -47,4 +47,9 @@ auto MockDelivery::make_activator() -> Activator }; } +auto MockDelivery::Active::backup() const -> Backup +{ + return Backup::make(_backup_seq++, "Hello, I am a backup"); +} + } // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockDelivery.hpp b/rmf_task/test/mock/MockDelivery.hpp index 4e7f9ca0..254cc7bb 100644 --- a/rmf_task/test/mock/MockDelivery.hpp +++ b/rmf_task/test/mock/MockDelivery.hpp @@ -43,9 +43,12 @@ class MockDelivery : public rmf_task::Task template Active( - const Description& desc, Args&&... args) + const Description& desc, + std::optional backup, + Args&&... args) : MockTask::Active(std::forward(args)...), - _description(desc) + _description(desc), + _restored_state(std::move(backup)) { // TODO(MXG): We could use the description, state, and parameters to // get the actual estimate for the pending phases @@ -54,9 +57,15 @@ class MockDelivery : public rmf_task::Task add_pending_phase("Pick up", "Pretending to pick something up", 30s); add_pending_phase("Go to drop off", "Pretending to go to a drop off point", 1min); add_pending_phase("Drop off", "Pretending to drop something off", 30s); + + // TODO(MXG): We could use the _restored_state here to } + Backup backup() const override; + Description _description; + std::optional _restored_state; + mutable uint64_t _backup_seq = 0; }; }; diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index e669d7e6..1d22425c 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -95,7 +95,6 @@ MockTask::Active::Active( std::function, const rmf_task::ConstParametersPtr&, const Task::ConstBookingPtr& booking, - std::optional, std::function update, std::function checkpoint, std::function phase_finished, diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp index 442560b9..88112a1c 100644 --- a/rmf_task/test/mock/MockTask.hpp +++ b/rmf_task/test/mock/MockTask.hpp @@ -62,7 +62,6 @@ class MockTask : public rmf_task::Task std::function get_state, const rmf_task::ConstParametersPtr& parameters, const Task::ConstBookingPtr& booking, - std::optional backup_state, std::function update, std::function checkpoint, std::function phase_finished, diff --git a/rmf_task/test/unit/test_Activator.cpp b/rmf_task/test/unit/test_Activator.cpp index 871156de..5b8951c6 100644 --- a/rmf_task/test/unit/test_Activator.cpp +++ b/rmf_task/test/unit/test_Activator.cpp @@ -19,7 +19,7 @@ #include "../mock/MockDelivery.hpp" -SCENARIO("Activator") +SCENARIO("Activate fresh task") { rmf_task::Activator activator; activator.add_activator(test_rmf_task::MockDelivery::make_activator()); @@ -29,12 +29,13 @@ SCENARIO("Activator") 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); rmf_task::Phase::ConstSnapshotPtr phase_snapshot; + std::optional backup; auto active = activator.activate( nullptr, nullptr, *request, [&phase_snapshot](auto s){ phase_snapshot = s; }, - [](auto) {}, + [&backup](auto b){ backup = b; }, [](auto) {}, []() {}); @@ -59,4 +60,28 @@ SCENARIO("Activator") mock_active->_active_phase->send_update(); REQUIRE(phase_snapshot != nullptr); CHECK(phase_snapshot->tag()->id() == 1); + + // ====== Restoring a task ======== + auto checkpoint = rmf_task::Task::Active::Backup::make(10, "Hello, backup"); + mock_active->_checkpoint(checkpoint); + REQUIRE(backup.has_value()); + CHECK(backup->sequence() == checkpoint.sequence()); + CHECK(backup->state() == checkpoint.state()); + + auto restored = activator.restore( + nullptr, + nullptr, + *request, + backup->state(), + [&phase_snapshot](auto s){ phase_snapshot = s; }, + [&backup](auto b){ backup = b; }, + [](auto){}, + [](){}); + + auto mock_restored = + std::dynamic_pointer_cast(restored); + + REQUIRE(mock_restored); + REQUIRE(mock_restored->_restored_state.has_value()); + CHECK(mock_restored->_restored_state == checkpoint.state()); } From 3505ac070d1df5cc2c68ea8310d262d1b5c11d4b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Sep 2021 12:11:00 +0800 Subject: [PATCH 24/85] Use derived class implementation of backup Signed-off-by: Michael X. Grey --- rmf_task/test/mock/MockTask.cpp | 6 ++++++ rmf_task/test/mock/MockTask.hpp | 2 ++ rmf_task/test/unit/test_Activator.cpp | 10 +++++----- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index 1d22425c..2d748663 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -146,4 +146,10 @@ void MockTask::Active::start_next_phase(rmf_traffic::Time current_time) [](){}); } +//============================================================================== +void MockTask::Active::issue_backup() +{ + _checkpoint(backup()); +} + } // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp index 88112a1c..88a5f66d 100644 --- a/rmf_task/test/mock/MockTask.hpp +++ b/rmf_task/test/mock/MockTask.hpp @@ -74,6 +74,8 @@ class MockTask : public rmf_task::Task void start_next_phase(rmf_traffic::Time current_time); + void issue_backup(); + std::shared_ptr _tag; ConstTagPtr _ctag; std::vector _completed_phases; diff --git a/rmf_task/test/unit/test_Activator.cpp b/rmf_task/test/unit/test_Activator.cpp index 5b8951c6..f0e17d86 100644 --- a/rmf_task/test/unit/test_Activator.cpp +++ b/rmf_task/test/unit/test_Activator.cpp @@ -62,11 +62,11 @@ SCENARIO("Activate fresh task") CHECK(phase_snapshot->tag()->id() == 1); // ====== Restoring a task ======== - auto checkpoint = rmf_task::Task::Active::Backup::make(10, "Hello, backup"); - mock_active->_checkpoint(checkpoint); + CHECK_FALSE(backup.has_value()); + mock_active->issue_backup(); REQUIRE(backup.has_value()); - CHECK(backup->sequence() == checkpoint.sequence()); - CHECK(backup->state() == checkpoint.state()); + CHECK(backup->sequence() == 0); + CHECK(backup->state() == "Hello, I am a backup"); auto restored = activator.restore( nullptr, @@ -83,5 +83,5 @@ SCENARIO("Activate fresh task") REQUIRE(mock_restored); REQUIRE(mock_restored->_restored_state.has_value()); - CHECK(mock_restored->_restored_state == checkpoint.state()); + CHECK(mock_restored->_restored_state == backup->state()); } From 724eb0a3bdd60b9e83800c25d8f598e49396be9f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Sep 2021 12:13:36 +0800 Subject: [PATCH 25/85] Finish comment Signed-off-by: Michael X. Grey --- rmf_task/test/mock/MockDelivery.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_task/test/mock/MockDelivery.hpp b/rmf_task/test/mock/MockDelivery.hpp index 254cc7bb..2e4512eb 100644 --- a/rmf_task/test/mock/MockDelivery.hpp +++ b/rmf_task/test/mock/MockDelivery.hpp @@ -58,7 +58,8 @@ class MockDelivery : public rmf_task::Task add_pending_phase("Go to drop off", "Pretending to go to a drop off point", 1min); add_pending_phase("Drop off", "Pretending to drop something off", 30s); - // TODO(MXG): We could use the _restored_state here to + // TODO(MXG): We could use the _restored_state here to fast-forward this + // mock task to the phase that the previous Task::Active left off from. } Backup backup() const override; From ecbe2582b1401205767dbc6b8d9ebfeebf0687b8 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Sep 2021 17:48:41 +0800 Subject: [PATCH 26/85] Creating an integration test to guide the implementation of backups Signed-off-by: Michael X. Grey --- .../include/rmf_task/BackupFileManager.hpp | 107 ++++++++ rmf_task/src/rmf_task/BackupFileManager.cpp | 233 ++++++++++++++++++ rmf_task/test/unit/test_backups.cpp | 108 ++++++++ 3 files changed, 448 insertions(+) create mode 100644 rmf_task/include/rmf_task/BackupFileManager.hpp create mode 100644 rmf_task/src/rmf_task/BackupFileManager.cpp create mode 100644 rmf_task/test/unit/test_backups.cpp diff --git a/rmf_task/include/rmf_task/BackupFileManager.hpp b/rmf_task/include/rmf_task/BackupFileManager.hpp new file mode 100644 index 00000000..0a54220c --- /dev/null +++ b/rmf_task/include/rmf_task/BackupFileManager.hpp @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__BACKUPFILEMANAGER_HPP +#define RMF_TASK__BACKUPFILEMANAGER_HPP + +#include + +#include + +namespace rmf_task { + +//============================================================================== +class BackupFileManager +{ +public: + + class Group; + class Robot; + + /// Construct a BackupFileManager + /// + /// \param[in] root_directory + /// Specify the root directory that the backup files should live in + BackupFileManager(std::filesystem::path root_directory); + + /// Set whether any previously existing backups should be cleared out on + /// startup. By default this behavior is turned OFF. + /// + /// \param[in] value + /// True if the behavior should be turned on; false if it should be turned + /// off. + BackupFileManager& clear_on_startup(bool value = true); + + /// Set whether any currently existing backups should be cleared out on + /// shutdown. By default this behavior is turned ON. + /// + /// \param[in] value + /// True if the behavior should be turned on; false if it should be turned + /// off. + BackupFileManager& clear_on_shutdown(bool value = true); + + /// Make a group (a.k.a. fleet) to back up. + std::shared_ptr make_group(std::string name); + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +//============================================================================== +class BackupFileManager::Group +{ +public: + + /// Make a handle to backup a robot for this group + /// + /// \param[in] name + /// The unique name of the robot that's being backed up + std::shared_ptr make_robot(std::string name); + + // TODO(MXG): Add an API for saving the task assignments of the Group. When + // the Group is constructed/destructed, it should clear out those task + // assignments, according to the RAII settings of its parent BackupFileManager + // instance. + + class Implementation; +private: + Group(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +//============================================================================== +class BackupFileManager::Robot +{ +public: + + /// Read a backup state from file if a backup file exists for this robot. + /// If a backup does not exist, return a nullopt. + std::optional read() const; + + /// Write a backup to file + void write(const Task::Active::Backup& backup); + + class Implementation; +private: + Robot(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__BACKUPFILEMANAGER_HPP diff --git a/rmf_task/src/rmf_task/BackupFileManager.cpp b/rmf_task/src/rmf_task/BackupFileManager.cpp new file mode 100644 index 00000000..8aca9608 --- /dev/null +++ b/rmf_task/src/rmf_task/BackupFileManager.cpp @@ -0,0 +1,233 @@ +/* + * Copyright (C) 2021 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 + +#include + +namespace rmf_task { + +//============================================================================== +class BackupFileManager::Implementation +{ +public: + + struct Settings + { + bool clear_on_startup = false; + bool clear_on_shutdown = true; + }; + using ConstSettingsPtr = std::shared_ptr; + + Implementation(std::filesystem::path directory) + : root_directory(std::move(directory)), + settings(std::make_shared()) + { + // Do nothing + } + + std::filesystem::path root_directory; + std::shared_ptr settings; + + std::unordered_map> groups; +}; + +//============================================================================== +class BackupFileManager::Group::Implementation +{ +public: + + using ConstSettingsPtr = BackupFileManager::Implementation::ConstSettingsPtr; + + Implementation( + std::filesystem::path directory, + ConstSettingsPtr settings) + : group_directory(std::move(directory)), + settings(std::move(settings)) + { + // Do nothing + } + + template + static std::shared_ptr make(Args&&... args) + { + Group output; + output._pimpl = rmf_utils::make_unique_impl( + std::forward(args)...); + + return std::make_shared(std::move(output)); + } + + std::filesystem::path group_directory; + ConstSettingsPtr settings; + + std::unordered_map> robots; +}; + +//============================================================================== +class BackupFileManager::Robot::Implementation +{ +public: + + using ConstSettingsPtr = BackupFileManager::Implementation::ConstSettingsPtr; + + Implementation( + std::filesystem::path directory, + ConstSettingsPtr settings) + : robot_directory(std::move(directory)), + settings(std::move(settings)) + { + if (settings->clear_on_startup) + clear_backup(); + } + + template + static std::shared_ptr make(Args&&... args) + { + Robot output; + output._pimpl = rmf_utils::make_unique_impl( + std::forward(args)...); + + return std::make_shared(std::move(output)); + } + + ~Implementation() + { + if (settings->clear_on_shutdown) + clear_backup(); + } + + std::filesystem::path robot_directory; + ConstSettingsPtr settings; + std::optional last_seq; + + void write_if_new(const Task::Active::Backup& backup) + { + if (last_seq.has_value()) + { + if (rmf_utils::modular(backup.sequence()).less_than_or_equal(*last_seq)) + return; + } + + last_seq = backup.sequence(); + write(backup.state()); + } + +private: + void write(const std::string& state) + { + throw std::runtime_error( + "[BackupFileManager::Robot::Implementation::write] not implemented yet"); + } + + void clear_backup() + { + throw std::runtime_error( + "[BackupFileManager::Robot::Implementation::clear_backup] " + "not implemented yet"); + } +}; + +//============================================================================== +BackupFileManager::BackupFileManager(std::filesystem::path root_directory) +: _pimpl(rmf_utils::make_unique_impl(std::move(root_directory))) +{ + // Do nothing +} + +//============================================================================== +BackupFileManager& BackupFileManager::clear_on_startup(bool value) +{ + _pimpl->settings->clear_on_startup = value; + return *this; +} + +//============================================================================== +BackupFileManager& BackupFileManager::clear_on_shutdown(bool value) +{ + _pimpl->settings->clear_on_shutdown = value; + return *this; +} + +//============================================================================== +auto BackupFileManager::make_group(std::string name) -> std::shared_ptr +{ + // TODO(MXG): Consider sanitizing the incoming name + const auto insertion = _pimpl->groups.insert({name, std::weak_ptr()}); + const auto& it = insertion.first; + if (!insertion.second) + { + auto group = it->second.lock(); + if (group) + return group; + } + + auto group = Group::Implementation::make( + _pimpl->root_directory / std::filesystem::path(std::move(name)), + _pimpl->settings); + + it->second = group; + return group; +} + +//============================================================================== +auto BackupFileManager::Group::make_robot(std::string name) +-> std::shared_ptr +{ + const auto insertion = _pimpl->robots.insert({name, std::weak_ptr()}); + const auto& it = insertion.first; + if (!insertion.second) + { + auto robot = it->second.lock(); + if (robot) + return robot; + } + + auto robot = Robot::Implementation::make( + _pimpl->group_directory / std::filesystem::path(std::move(name)), + _pimpl->settings); + + it->second = robot; + return robot; +} + +//============================================================================== +BackupFileManager::Group::Group() +{ + // Do nothing +} + +//============================================================================== +std::optional BackupFileManager::Robot::read() const +{ + throw std::runtime_error( + "[BackupFileManager::Robot::read] not implemented yet"); +} + +//============================================================================== +void BackupFileManager::Robot::write(const Task::Active::Backup& backup) +{ + _pimpl->write_if_new(backup); +} + +//============================================================================== +BackupFileManager::Robot::Robot() +{ + // Do nothing +} + +} // namespace rmf_task diff --git a/rmf_task/test/unit/test_backups.cpp b/rmf_task/test/unit/test_backups.cpp new file mode 100644 index 00000000..c72a9c5b --- /dev/null +++ b/rmf_task/test/unit/test_backups.cpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2021 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 + +#include "../mock/MockDelivery.hpp" + +#include + +SCENARIO("Back up to file") +{ + rmf_task::Activator activator; + activator.add_activator(test_rmf_task::MockDelivery::make_activator()); + + const std::string backup_root_dir = "/tmp/rmf_task/test_backups"; + rmf_task::BackupFileManager backup(backup_root_dir); + backup.clear_on_startup(); + + auto robot_backup = backup.make_group("group")->make_robot("robot"); + CHECK_FALSE(robot_backup->read().has_value()); + + using namespace std::chrono_literals; + auto request = rmf_task::requests::Delivery::make( + 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + + rmf_task::Phase::ConstSnapshotPtr phase_snapshot; + auto active_task = activator.activate( + nullptr, + nullptr, + *request, + [&phase_snapshot](auto s){ phase_snapshot = s; }, + [robot_backup](auto b){ robot_backup->write(b); }, + [](auto) {}, + []() {}); + + REQUIRE(active_task); + + auto mock_active_task = + std::dynamic_pointer_cast(active_task); + + REQUIRE(mock_active_task); + + // ====== Advance the robot forward through some phases ====== + CHECK(phase_snapshot == nullptr); + mock_active_task->start_next_phase(rmf_traffic::Time()); + CHECK(phase_snapshot == nullptr); + + mock_active_task->_active_phase->send_update(); + REQUIRE(phase_snapshot != nullptr); + CHECK(phase_snapshot->tag()->id() == 0); + + mock_active_task->start_next_phase(rmf_traffic::Time()); + REQUIRE(phase_snapshot != nullptr); + CHECK(phase_snapshot->tag()->id() == 0); + mock_active_task->_active_phase->send_update(); + REQUIRE(phase_snapshot != nullptr); + CHECK(phase_snapshot->tag()->id() == 1); + + // ====== Backup the task ====== + mock_active_task->issue_backup(); + + // ====== Restore the task ====== + rmf_task::BackupFileManager restore(backup_root_dir); + + auto robot_restore = restore.make_group("group")->make_robot("robot"); + const auto restored_state = robot_restore->read(); + REQUIRE(restored_state.has_value()); + + rmf_task::Phase::ConstSnapshotPtr restored_snapshot; + auto restored_task = activator.restore( + nullptr, + nullptr, + *request, + restored_state.value(), + [&restored_snapshot](auto s){ restored_snapshot = s; }, + [robot_restore](auto b){ robot_restore->write(b); }, + [](auto){}, + [](){}); + + auto mock_restored_task = + std::dynamic_pointer_cast( + restored_task); + + REQUIRE(mock_restored_task); + REQUIRE(mock_restored_task->_restored_state.has_value()); + CHECK(mock_restored_task->_restored_state == restored_state.value()); + + // When the task is restored, it should resume where mock_active_task left off + // and it should issue a phase snapshot to reflect that + REQUIRE(restored_snapshot); + CHECK(restored_snapshot->tag()->id() == phase_snapshot->tag()->id()); + CHECK(restored_snapshot->tag()->name() == phase_snapshot->tag()->name()); + CHECK(restored_snapshot->tag()->detail() == phase_snapshot->tag()->detail()); +} From 37830d526473e8bf9a19fe4658520133baf038c6 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Sep 2021 17:50:19 +0800 Subject: [PATCH 27/85] Move backups test into integration folder Signed-off-by: Michael X. Grey --- rmf_task/test/{unit => integration}/test_backups.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename rmf_task/test/{unit => integration}/test_backups.cpp (100%) diff --git a/rmf_task/test/unit/test_backups.cpp b/rmf_task/test/integration/test_backups.cpp similarity index 100% rename from rmf_task/test/unit/test_backups.cpp rename to rmf_task/test/integration/test_backups.cpp From 56c1f400627c2c4e3f51847636f4205a15317d1f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 23 Sep 2021 14:19:53 +0800 Subject: [PATCH 28/85] Continuing to implement backup and restore for phase sequence tasks Signed-off-by: Michael X. Grey --- .../include/rmf_task_sequence/Phase.hpp | 9 + .../src/rmf_task_sequence/Task.cpp | 190 +++++++++++++++++- 2 files changed, 190 insertions(+), 9 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index a9274749..a7020877 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -114,6 +114,7 @@ class Phase::Active : public rmf_task::Phase::Active virtual ~Active() = default; }; +//============================================================================== class Phase::Active::Backup { public: @@ -130,7 +131,15 @@ class Phase::Active::Backup /// Phase::Activator when restoring a Task. static Backup make(uint64_t seq, YAML::Node state); + /// Get the sequence number + uint64_t sequence() const; + + /// Get the YAML representation of the backed up state + const YAML::Node& state() const; + + class Implementation; private: + rmf_utils::impl_ptr _pimpl; }; //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index be96bb28..940be808 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -19,9 +19,12 @@ #include +#include + namespace rmf_task_sequence { namespace { + //============================================================================== struct Stage { @@ -88,7 +91,9 @@ class Task::Active std::move(phase_finished), std::move(task_finished))); - task->_load_backup(std::move(backup_state)); + if (!task->_load_backup(std::move(backup_state))) + return task; + task->_generate_pending_phases(); task->_begin_next_stage(); @@ -136,14 +141,23 @@ class Task::Active /// _load_backup should only be used in the make(~) function. It will /// fast-forward the progress of the task to catch up to a backed up state, /// since the task is being restored from a task that was already in progress. - void _load_backup(std::optional backup_state); + /// + /// \return false if the task needs to be aborted due to a bad backup_state, + /// otherwise return true. + bool _load_backup(std::optional backup_state); void _generate_pending_phases(); void _finish_phase(); void _begin_next_stage(); void _finish_task(); - void _issue_backup(Phase::Active::Backup backup); + void _issue_backup( + Phase::Tag::Id source_phase_id, + Phase::Active::Backup phase_backup) const; + + Backup _generate_backup( + Phase::Tag::Id current_phase_id, + Phase::Active::Backup phase_backup) const; Active( Phase::ConstActivatorPtr phase_activator, @@ -165,7 +179,8 @@ class Task::Active _checkpoint(std::move(checkpoint)), _phase_finished(std::move(phase_finished)), _task_finished(std::move(task_finished)), - _pending_stages(Description::Implementation::get_stages(description)) + _pending_stages(Description::Implementation::get_stages(description)), + _cancel_sequence_initial_id(_pending_stages.size()+1) { // Do nothing } @@ -191,13 +206,119 @@ class Task::Active std::vector _completed_phases; std::optional _resume_interrupted_phase; - bool _cancelled = false; + std::optional _cancelled_on_phase = std::nullopt; bool _killed = false; + + mutable std::optional _last_phase_backup_sequence_number; + mutable uint64_t _next_task_backup_sequence_number = 0; + + const uint64_t _cancel_sequence_initial_id; }; //============================================================================== -void Task::Active::_load_backup(std::optional backup_state_opt) +auto Task::Builder::add_phase( + Phase::ConstDescriptionPtr description, + std::vector cancellation_sequence) -> Builder& { + // NOTE(MXG): We give each phase the ID of _pimpl->stages.size()+1 because + // the ID 0 is reserved for the RestoreFromBackup phase, in case that's ever + // needed. + _pimpl->stages.emplace_back( + std::make_shared( + Stage{ + _pimpl->stages.size()+1, + std::move(description), + std::move(cancellation_sequence) + })); + + return *this; +} + +//============================================================================== +auto Task::Active::backup() const -> Backup +{ + return _generate_backup( + _active_phase->tag()->id(), + _active_phase->backup()); +} + +//============================================================================== +bool Task::Active::_load_backup(std::optional backup_state_opt) +{ + if (!backup_state_opt.has_value()) + return true; + + auto restore_phase_tag = std::make_shared( + 0, + "Restore from backup", + "The task progress is being restored from a backed up state", + rmf_traffic::Duration(0)); + + // TODO(MXG): Allow users to specify a custom clock for the log + const auto start_time = std::chrono::steady_clock::now(); + auto restore_phase_log = rmf_task::Log(); + + auto failed_to_restore = [&]() -> bool + { + _pending_stages.clear(); + _phase_finished( + std::make_shared( + std::move(restore_phase_tag), + std::move(restore_phase_log), + start_time, + std::chrono::steady_clock::now())); + + _finish_task(); + return false; + }; + + // TODO(MXG): Use a formal schema to validate the input instead of validating + // it manually. + const auto state = YAML::Load(*backup_state_opt); + const auto& schema = state["schema_version"]; + if (!schema) + { + restore_phase_log.error( + "The field [schema_version] is missing from the root directory of the " + "backup state:\n```\n" + *backup_state_opt + "\n```"); + + return failed_to_restore(); + } + + if (!schema.IsScalar()) + { + restore_phase_log.error( + "The field [schema_version] contains an unsupported value type [" + + std::to_string(schema.Type()) + "] expected scalar type [" + + std::to_string(YAML::NodeType::Scalar) + "]\n```\n" + + *backup_state_opt + "\n```"); + + return failed_to_restore(); + } + + try + { + const auto schema_version = schema.as(); + if (schema_version != 1) + { + restore_phase_log.error( + "Unrecognized value for [schema_version]: " + + std::to_string(schema_version) +". Backup state might have been " + "produced by a newer or unsupported implementation.\n```" + + *backup_state_opt + "\n```"); + + return failed_to_restore(); + } + } + catch(const YAML::BadConversion& err) + { + restore_phase_log.error( + std::string("Invalid data type for [schema_version]: ") + err.what() + + "\n```\n" + *backup_state_opt + "\n```"); + + return failed_to_restore(); + } + } @@ -248,6 +369,9 @@ void Task::Active::_begin_next_stage() auto tag = _pending_phases.front()->tag(); _pending_phases.erase(_pending_phases.begin()); + // Reset our memory of phase backup sequence number + _last_phase_backup_sequence_number = std::nullopt; + _current_phase_start_time = _clock(); _active_phase = _phase_activator->activate( _get_state, @@ -258,10 +382,11 @@ void Task::Active::_begin_next_stage() if (const auto self = me.lock()) self->_update(snapshot); }, - [me = weak_from_this()](Phase::Active::Backup backup) + [me = weak_from_this(), id = _active_phase->tag()->id()]( + Phase::Active::Backup backup) { if (const auto self = me.lock()) - self->_issue_backup(std::move(backup)); + self->_issue_backup(id, std::move(backup)); }, [me = weak_from_this()]() { @@ -277,9 +402,56 @@ void Task::Active::_finish_task() } //============================================================================== -void Task::Active::_issue_backup(Phase::Active::Backup backup) +void Task::Active::_issue_backup( + Phase::Tag::Id source_phase_id, + Phase::Active::Backup phase_backup) const { + if (source_phase_id != _active_phase->tag()->id()) + { + // If this backup is for something other than the current phase, ignore it + return; + } + + if (_last_phase_backup_sequence_number.has_value()) + { + const auto cutoff = *_last_phase_backup_sequence_number; + if (rmf_utils::modular(phase_backup.sequence()).less_than_or_equal(cutoff)) + { + // The current backup sequence number has already passed this one + return; + } + } + + _last_phase_backup_sequence_number = phase_backup.sequence(); + _checkpoint(_generate_backup(source_phase_id, std::move(phase_backup))); +} + +//============================================================================== +auto Task::Active::_generate_backup( + Phase::Tag::Id current_phase_id, + Phase::Active::Backup phase_backup) const -> Backup +{ + YAML::Node current_phase; + current_phase["id"] = current_phase_id; + if (_cancelled_on_phase.has_value()) + current_phase["cancelled_from"] = *_cancelled_on_phase; + + current_phase["backup"] = phase_backup.state(); + + std::vector skipping_phases; + for (const auto& p : _pending_phases) + { + if (p->will_be_skipped()) + skipping_phases.push_back(p->tag()->id()); + } + + YAML::Node root; + root["schema_version"] = 1; + root["current_phase"] = std::move(current_phase); + root["skip_phases"] = std::move(skipping_phases); + // TODO(MXG): Is there anything else we need to consider as part of the state? + return Backup::make(_next_task_backup_sequence_number++, YAML::Dump(root)); } //============================================================================== From ed651ae62e6117d5ad97ee7eca31246e975569c5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 24 Sep 2021 17:28:54 +0800 Subject: [PATCH 29/85] Finished backup/restore implementation -- needs testing Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Phase.hpp | 4 + rmf_task/src/rmf_task/Phase.cpp | 7 + .../include/rmf_task_sequence/Phase.hpp | 40 +-- .../src/rmf_task_sequence/Task.cpp | 232 +++++++++++++++--- 4 files changed, 217 insertions(+), 66 deletions(-) diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index c45ea309..4ad1e4c8 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -46,6 +46,7 @@ class Phase using ConstSnapshotPtr = std::shared_ptr; class Pending; + using PendingPtr = std::shared_ptr; using ConstPendingPtr = std::shared_ptr; }; @@ -177,6 +178,9 @@ class Phase::Pending /// Tag of the phase const ConstTagPtr& tag() const; + /// Change whether this pending phase will be skipped + Pending& will_be_skipped(bool value); + /// Check if this phase is set to be skipped bool will_be_skipped() const; diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 85f7a6f1..179685f8 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -194,6 +194,13 @@ auto Phase::Pending::tag() const -> const ConstTagPtr& return _pimpl->tag; } +//============================================================================== +auto Phase::Pending::will_be_skipped(bool value) -> Pending& +{ + _pimpl->will_be_skipped = value; + return *this; +} + //============================================================================== bool Phase::Pending::will_be_skipped() const { diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index a7020877..65f45845 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -208,42 +208,12 @@ class Phase::Activator /// \param[in] description /// The description of the phase /// - /// \param[in] update - /// A callback that will be triggered when the phase has a notable update. - /// The callback will be given a snapshot of the active phase. - /// - /// \param[in] checkpoint - /// A callback that will be triggered when the phase has reached a task - /// checkpoint whose state is worth backing up. - /// - /// \param[in] finished - /// A callback that will be triggered when the phase has finished. - /// - /// \return an active, running instance of the described phase. - ActivePtr activate( - std::function get_state, - ConstTagPtr tag, - const Description& description, - std::function update, - std::function checkpoint, - std::function finished) const; - - /// Restore a phase based on a description of the phase and its backup state. - /// - /// \param[in] get_state - /// A callback for getting the current state of the robot - /// - /// \param[in] tag - /// The tag of this phase - /// - /// \param[in] description - /// The description of the phase - /// /// \param[in] backup_state - /// The serialized backup state of the phase + /// If the phase is being restored, pass its backup state in here. Otherwise + /// if the phase is being freshly activated, pass a nullopt. /// /// \param[in] update - /// A callback that will triggered when the phase has a notable update. + /// A callback that will be triggered when the phase has a notable update. /// The callback will be given a snapshot of the active phase. /// /// \param[in] checkpoint @@ -254,11 +224,11 @@ class Phase::Activator /// A callback that will be triggered when the phase has finished. /// /// \return an active, running instance of the described phase. - ActivePtr restore( + ActivePtr activate( std::function get_state, ConstTagPtr tag, const Description& description, - const std::string& backup_state, + std::optional backup_state, std::function update, std::function checkpoint, std::function finished) const; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 940be808..995e1704 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -91,8 +91,11 @@ class Task::Active std::move(phase_finished), std::move(task_finished))); - if (!task->_load_backup(std::move(backup_state))) + if (backup_state.has_value()) + { + task->_load_backup(std::move(*backup_state)); return task; + } task->_generate_pending_phases(); task->_begin_next_stage(); @@ -144,13 +147,16 @@ class Task::Active /// /// \return false if the task needs to be aborted due to a bad backup_state, /// otherwise return true. - bool _load_backup(std::optional backup_state); + void _load_backup(std::string backup_state); void _generate_pending_phases(); void _finish_phase(); - void _begin_next_stage(); + void _begin_next_stage(std::optional restore = std::nullopt); void _finish_task(); + void _prepare_cancellation_sequence( + std::vector sequence); + void _issue_backup( Phase::Tag::Id source_phase_id, Phase::Active::Backup phase_backup) const; @@ -196,7 +202,7 @@ class Task::Active std::function _task_finished; std::list _pending_stages; - std::vector _pending_phases; + std::vector _pending_phases; ConstStagePtr _active_stage; Phase::ActivePtr _active_phase; @@ -243,11 +249,10 @@ auto Task::Active::backup() const -> Backup } //============================================================================== -bool Task::Active::_load_backup(std::optional backup_state_opt) +void Task::Active::_load_backup(std::string backup_state_str) { - if (!backup_state_opt.has_value()) - return true; - + // TODO(MXG): This function (and really the entire class) could be refactored + // into a smarter component-based design instead of this wall-of-text. auto restore_phase_tag = std::make_shared( 0, "Restore from backup", @@ -258,29 +263,33 @@ bool Task::Active::_load_backup(std::optional backup_state_opt) const auto start_time = std::chrono::steady_clock::now(); auto restore_phase_log = rmf_task::Log(); - auto failed_to_restore = [&]() -> bool - { - _pending_stages.clear(); - _phase_finished( - std::make_shared( - std::move(restore_phase_tag), - std::move(restore_phase_log), - start_time, - std::chrono::steady_clock::now())); - - _finish_task(); - return false; - }; + const auto failed_to_restore = [&]() -> void + { + _pending_stages.clear(); + _phase_finished( + std::make_shared( + std::move(restore_phase_tag), + restore_phase_log.view(), + start_time, + std::chrono::steady_clock::now())); + + _finish_task(); + }; + + const auto get_state_text = [&]() -> std::string + { + return "\n```\n" + backup_state_str + "\n```\n"; + }; // TODO(MXG): Use a formal schema to validate the input instead of validating // it manually. - const auto state = YAML::Load(*backup_state_opt); + const auto state = YAML::Load(backup_state_str); const auto& schema = state["schema_version"]; if (!schema) { restore_phase_log.error( "The field [schema_version] is missing from the root directory of the " - "backup state:\n```\n" + *backup_state_opt + "\n```"); + "backup state:" + get_state_text()); return failed_to_restore(); } @@ -290,8 +299,7 @@ bool Task::Active::_load_backup(std::optional backup_state_opt) restore_phase_log.error( "The field [schema_version] contains an unsupported value type [" + std::to_string(schema.Type()) + "] expected scalar type [" - + std::to_string(YAML::NodeType::Scalar) + "]\n```\n" - + *backup_state_opt + "\n```"); + + std::to_string(YAML::NodeType::Scalar) + "]" + get_state_text()); return failed_to_restore(); } @@ -304,22 +312,161 @@ bool Task::Active::_load_backup(std::optional backup_state_opt) restore_phase_log.error( "Unrecognized value for [schema_version]: " + std::to_string(schema_version) +". Backup state might have been " - "produced by a newer or unsupported implementation.\n```" - + *backup_state_opt + "\n```"); + "produced by a newer or unsupported implementation." + + get_state_text()); return failed_to_restore(); } } - catch(const YAML::BadConversion& err) + catch (const YAML::BadConversion& err) { restore_phase_log.error( std::string("Invalid data type for [schema_version]: ") + err.what() - + "\n```\n" + *backup_state_opt + "\n```"); + + get_state_text()); + + return failed_to_restore(); + } + + const auto current_phase_yaml = state["current_phase"]; + if (!current_phase_yaml) + { + restore_phase_log.error( + "[current_phase] element missing from backup" + get_state_text()); + + return failed_to_restore(); + } + + const auto current_phase_id_yaml = current_phase_yaml["id"]; + if (!current_phase_id_yaml) + { + restore_phase_log.error( + "[id] element missing from [current_phase]" + get_state_text()); + + return failed_to_restore(); + } + + const auto cancelled_from_yaml = current_phase_yaml["cancelled_from"]; + if (cancelled_from_yaml) + { + std::optional cancelled_from_phase_opt; + try + { + cancelled_from_phase_opt = cancelled_from_yaml.as(); + } + catch (const YAML::BadConversion& err) + { + restore_phase_log.error( + "[cancelled_from] element in [current_phase] has an invalid type" + + get_state_text()); + + return failed_to_restore(); + } + + const auto cancelled_from = cancelled_from_phase_opt.value(); + if (cancelled_from >= _cancel_sequence_initial_id) + { + restore_phase_log.error( + "Invalid value [" + std::to_string(cancelled_from) + + "] for [cancelled_from]. Value must be less than [" + + std::to_string(_cancel_sequence_initial_id) + "]" + get_state_text()); + + return failed_to_restore(); + } + + for (const auto& stage : _pending_stages) + { + if (stage->id == cancelled_from) + { + _prepare_cancellation_sequence(stage->cancellation_sequence); + break; + } + } + } + + std::optional current_phase_id_opt; + try + { + current_phase_id_opt = current_phase_id_yaml.as(); + } + catch (const YAML::BadConversion& err) + { + restore_phase_log.error( + "[id] element in [current_phase] has an invalid type" + get_state_text()); return failed_to_restore(); } + const auto current_phase_id = current_phase_id_opt.value(); + bool found_phase = false; + while (!found_phase) + { + const auto stage = _pending_stages.front(); + if (stage->id != current_phase_id) + { + _pending_stages.pop_front(); + continue; + } + + found_phase = true; + } + + const auto skip_phases_yaml = state["skip_phases"]; + if (skip_phases_yaml) + { + std::vector skip_phases; + try + { + skip_phases = skip_phases_yaml.as>(); + } + catch (const YAML::BadConversion& err) + { + restore_phase_log.error( + std::string("Invalid data type for [skip_phases]: ") + err.what() + + get_state_text()); + return failed_to_restore(); + } + + auto pending_it = _pending_phases.begin(); + for (const auto& id : skip_phases) + { + if (id == 0) + { + // This should probably issue a warning, because this would be kind of + // weird, but not really a problem + continue; + } + + if (pending_it == _pending_phases.end() + || id < (*pending_it)->tag()->id()) + { + // This shouldn't happen, but it's not a critical error. In the worst + // case, the operator needs to resend a skip command. + restore_phase_log.warn( + "Unexpected ordering of phase skip IDs" + get_state_text()); + continue; + } + + while (pending_it != _pending_phases.end() + && (*pending_it)->tag()->id() < id) + { + ++pending_it; + } + + (*pending_it)->will_be_skipped(true); + } + } + + const auto phase_state_yaml = current_phase_yaml["state"]; + if (!phase_state_yaml) + { + restore_phase_log.error( + "Missing [state] field for [current_phase]" + get_state_text()); + + return failed_to_restore(); + } + + _begin_next_stage(phase_state_yaml.as()); } //============================================================================== @@ -356,7 +503,7 @@ void Task::Active::_finish_phase() } //============================================================================== -void Task::Active::_begin_next_stage() +void Task::Active::_begin_next_stage(std::optional restore) { if (_pending_stages.empty()) return _finish_task(); @@ -377,6 +524,7 @@ void Task::Active::_begin_next_stage() _get_state, std::move(tag), *_active_stage->description, + std::move(restore), [me = weak_from_this()](Phase::ConstSnapshotPtr snapshot) { if (const auto self = me.lock()) @@ -426,6 +574,28 @@ void Task::Active::_issue_backup( _checkpoint(_generate_backup(source_phase_id, std::move(phase_backup))); } +//============================================================================== +void Task::Active::_prepare_cancellation_sequence( + std::vector sequence) +{ + _pending_phases.clear(); + _pending_stages.clear(); + + uint64_t next_stage_id = _cancel_sequence_initial_id; + for (auto&& phase : sequence) + { + _pending_stages.emplace_back( + std::make_shared( + Stage{ + next_stage_id++, + std::move(phase), + {} + })); + } + + _generate_pending_phases(); +} + //============================================================================== auto Task::Active::_generate_backup( Phase::Tag::Id current_phase_id, @@ -436,7 +606,7 @@ auto Task::Active::_generate_backup( if (_cancelled_on_phase.has_value()) current_phase["cancelled_from"] = *_cancelled_on_phase; - current_phase["backup"] = phase_backup.state(); + current_phase["state"] = phase_backup.state(); std::vector skipping_phases; for (const auto& p : _pending_phases) From e1c2d51f7f32daf36e5298247656ed1e84b667bd Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 24 Sep 2021 18:23:17 +0800 Subject: [PATCH 30/85] Implementing phase activator Signed-off-by: Michael X. Grey --- rmf_task/src/rmf_task/Activator.cpp | 2 +- .../include/rmf_task_sequence/Phase.hpp | 13 +++++ .../rmf_task_sequence/detail/impl_Phase.hpp | 53 +++++++++++++++++++ .../src/rmf_task_sequence/Phase.cpp | 41 ++++++++++++++ 4 files changed, 108 insertions(+), 1 deletion(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp diff --git a/rmf_task/src/rmf_task/Activator.cpp b/rmf_task/src/rmf_task/Activator.cpp index ac428265..1a36d8fe 100644 --- a/rmf_task/src/rmf_task/Activator.cpp +++ b/rmf_task/src/rmf_task/Activator.cpp @@ -106,7 +106,7 @@ void Activator::_add_activator( std::type_index type, Activate activator) { - _pimpl->activators.insert({type, std::move(activator)}); + _pimpl->activators.insert_or_assign(type, std::move(activator)); } } // namespace rmf_task diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 65f45845..956afab4 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -174,6 +174,10 @@ class Phase::Activator /// active phase. This snapshot can be safely read in parallel to the phase /// execution. /// + /// \param[in] checkpoint + /// A callback that will be triggered when the phase has reached a task + /// checkpoint whose state is worth backing up. + /// /// \param[in] finished /// A callback that will be triggered when the phase has finished. /// @@ -187,6 +191,7 @@ class Phase::Activator const Description& description, std::optional backup_state, std::function update, + std::function checkpoint, std::function finished) >; @@ -235,6 +240,12 @@ class Phase::Activator class Implementation; private: + + /// \private + void _add_activator( + std::type_index type, + Activate activator); + rmf_utils::impl_ptr _pimpl; }; @@ -358,4 +369,6 @@ class Phase::SequenceModel : public Phase::Model } // namespace rmf_task_sequence +#include + #endif // RMF_TASK_SEQUENCE__PHASE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp new file mode 100644 index 00000000..4880334b --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__DETAIL__IMPL_PHASE_HPP +#define RMF_TASK_SEQUENCE__DETAIL__IMPL_PHASE_HPP + +#include + +namespace rmf_task_sequence { + +//============================================================================== +template +void Phase::Activator::add_activator(Activate activator) +{ + _add_activator( + typeid(Desc), + [activator]( + std::function get_state, + ConstTagPtr tag, + const Phase::Description& description, + std::optional backup_state, + std::function phase_update, + std::function phase_checkpoint, + std::function phase_finished) + { + return activator( + std::move(get_state), + std::move(tag), + static_cast(description), + std::move(backup_state), + std::move(phase_update), + std::move(phase_checkpoint), + std::move(phase_finished)); + }); +} + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__IMPL_PHASE_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index 00ef3a6a..6d7496e4 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -19,6 +19,47 @@ namespace rmf_task_sequence { +//============================================================================== +class Phase::Activator::Implementation +{ +public: + + std::unordered_map> activators; + +}; + +//============================================================================== +Phase::ActivePtr Phase::Activator::activate( + std::function get_state, + ConstTagPtr tag, + const Description& description, + std::optional backup_state, + std::function update, + std::function checkpoint, + std::function finished) const +{ + const auto& type = typeid(description); + const auto it = _pimpl->activators.find(type); + if (it == _pimpl->activators.end()) + return nullptr; + + return it->second( + std::move(get_state), + std::move(tag), + description, + std::move(backup_state), + std::move(update), + std::move(checkpoint), + std::move(finished)); +} + +//============================================================================== +void Phase::Activator::_add_activator( + std::type_index type, Activate activator) +{ + _pimpl->activators.insert_or_assign(type, std::move(activator)); +} + //============================================================================== class Phase::SequenceModel::Implementation { From 399528164db20c6fadb0a1f5aece14bacc287483 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 6 Oct 2021 18:41:49 +0800 Subject: [PATCH 31/85] Migrating to JSON and json-schema Signed-off-by: Michael X. Grey --- rmf_task_sequence/CMakeLists.txt | 2 +- .../include/rmf_task_sequence/Phase.hpp | 8 ++-- rmf_task_sequence/package.xml | 2 +- .../backup_PhaseSequenceTask-0_1.schema.json | 40 +++++++++++++++++++ 4 files changed, 46 insertions(+), 6 deletions(-) create mode 100644 rmf_task_sequence/schemas/backup_PhaseSequenceTask-0_1.schema.json diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 208e7c1f..c5397c08 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -21,7 +21,7 @@ endif() include(GNUInstallDirs) find_package(rmf_task REQUIRED) -find_package(yaml-cpp REQUIRED) +find_package(nlohmann_json REQUIRED) find_package(ament_cmake_catch2 QUIET) find_package(rmf_cmake_uncrustify QUIET) diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 956afab4..8251eaac 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -28,7 +28,7 @@ #include -#include +#include namespace rmf_task_sequence { @@ -129,13 +129,13 @@ class Phase::Active::Backup /// \param[in] state /// A serialization of the phase's state. This will be used by /// Phase::Activator when restoring a Task. - static Backup make(uint64_t seq, YAML::Node state); + static Backup make(uint64_t seq, nlohmann::json state); /// Get the sequence number uint64_t sequence() const; /// Get the YAML representation of the backed up state - const YAML::Node& state() const; + const nlohmann::json& state() const; class Implementation; private: @@ -282,7 +282,7 @@ class Phase::Description const Parameters& parameters) const = 0; /// Serialize this phase description into a string. - virtual YAML::Node serialize() const = 0; + virtual nlohmann::json serialize() const = 0; // Virtual destructor virtual ~Description() = default; diff --git a/rmf_task_sequence/package.xml b/rmf_task_sequence/package.xml index e1a1c29c..6888a0a7 100644 --- a/rmf_task_sequence/package.xml +++ b/rmf_task_sequence/package.xml @@ -10,7 +10,7 @@ Grey rmf_task - yaml-cpp + nlohmann-json3-dev ament_cmake_catch2 rmf_cmake_uncrustify diff --git a/rmf_task_sequence/schemas/backup_PhaseSequenceTask-0_1.schema.json b/rmf_task_sequence/schemas/backup_PhaseSequenceTask-0_1.schema.json new file mode 100644 index 00000000..d3641177 --- /dev/null +++ b/rmf_task_sequence/schemas/backup_PhaseSequenceTask-0_1.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://open-rmf.org/rmf_task_sequence/backup_PhaseSequenceTask/0.1", + "title": "Phase Sequence Task", + "description": "A task which is defined by a sequence of specified phases", + "properties": { + "schema_version": { + "description": "The version of the PhaseSequenceTask schema being used", + "const": "0.1" + }, + "current_phase": { + "description": "The current phase of the task when the backup occurred", + "properties": { + "id": { + "description": "The integer ID of the phase", + "type": "integer", + "minimum": 0 + }, + "cancelled_from": { + "description": "The integer ID of the phase that was cancelled to reach the current phase", + "type": "integer", + "minimum": 0 + }, + "state": { + "description": "The serialized state of the backed up current phase" + } + }, + "required": [ "id", "state" ] + }, + "skip_phases": { + "description": "A list of pending phases that are supposed to be skipped", + "type": "array", + "items": { + "type": "integer", + "minimum": 0 + } + } + }, + "required": [ "schema_version", "current_phase" ] +} From e2116b0a58722c1748fba69e30f717ccaf0349a7 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Oct 2021 00:26:21 +0800 Subject: [PATCH 32/85] Fix header guards and begin schema header template Signed-off-by: Michael X. Grey --- .../cmake/generate_schema_header.cmake | 0 .../include/rmf_task_sequence/Task.hpp | 6 +++--- .../rmf_task_sequence/phases/DropOff.hpp | 6 +++--- .../rmf_task_sequence/phases/GoToPlace.hpp | 6 +++--- .../include/rmf_task_sequence/phases/PickUp.hpp | 6 +++--- .../phases/internal_PayloadTransfer.hpp | 6 +++--- .../templates/schemas_template.hpp.in | 17 +++++++++++++++++ 7 files changed, 32 insertions(+), 15 deletions(-) create mode 100644 rmf_task_sequence/cmake/generate_schema_header.cmake create mode 100644 rmf_task_sequence/templates/schemas_template.hpp.in diff --git a/rmf_task_sequence/cmake/generate_schema_header.cmake b/rmf_task_sequence/cmake/generate_schema_header.cmake new file mode 100644 index 00000000..e69de29b diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index c7959ef6..1c3e0722 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__SEQUENCE__TASK_HPP -#define RMF_TASK__SEQUENCE__TASK_HPP +#ifndef RMF_TASK_SEQUENCE__TASK_HPP +#define RMF_TASK_SEQUENCE__TASK_HPP #include #include @@ -140,4 +140,4 @@ class Task::Model : public rmf_task::Task::Model } // namespace rmf_task_sequence -#endif // RMF_TASK__SEQUENCE__TASK_HPP +#endif // RMF_TASK_SEQUENCE__TASK_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp index a25439f8..fc274b09 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP -#define RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP +#ifndef RMF_TASK_SEQUENCE__PHASES__DROPOFF_HPP +#define RMF_TASK_SEQUENCE__PHASES__DROPOFF_HPP #include @@ -114,4 +114,4 @@ class DropOff::Description : public Phase::Description } // namespace phases } // namespace rmf_task_sequence -#endif // RMF_TASK__SEQUENCE__PHASES__DROPOFF_HPP +#endif // RMF_TASK_SEQUENCE__PHASES__DROPOFF_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp index 65fd0d13..a08667ed 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP -#define RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP +#ifndef RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP +#define RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP #include @@ -79,4 +79,4 @@ class GoToPlace::Description : public Phase::Description } // namespace phases } // namespace rmf_task_sequence -#endif // RMF_TASK__SEQUENCE__PHASES__GOTOPLACE_HPP +#endif // RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp index 0b3f194c..8a41d675 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP -#define RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP +#ifndef RMF_TASK_SEQUENCE__PHASES__PICKUP_HPP +#define RMF_TASK_SEQUENCE__PHASES__PICKUP_HPP #include @@ -114,4 +114,4 @@ class PickUp::Description : public Phase::Description } // namespace phases } // namespace rmf_task_sequence -#endif // RMF_TASK__SEQUENCE__PHASES__PICKUP_HPP +#endif // RMF_TASK_SEQUENCE__PHASES__PICKUP_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp index 6a662636..b0ac7ba6 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp @@ -15,8 +15,8 @@ * */ -#ifndef SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP -#define SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP +#ifndef SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP +#define SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP #include #include @@ -61,4 +61,4 @@ class PayloadTransfer } // namespace phases } // namespace rmf_task_sequence -#endif // SRC__RMF_TASK__SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP +#endif // SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP diff --git a/rmf_task_sequence/templates/schemas_template.hpp.in b/rmf_task_sequence/templates/schemas_template.hpp.in new file mode 100644 index 00000000..21208f9f --- /dev/null +++ b/rmf_task_sequence/templates/schemas_template.hpp.in @@ -0,0 +1,17 @@ +/* + * This file is automatically generated by the build system of rmf_task_sequence + * + * Automatically generated files do not have a copyright + */ + +#ifndef RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ + +namespace rmf_task_sequence { +namespace schemas { + +const std::string @lower_schema_name@ = +R"raw_schema_definition( +@lower_schema_text@)raw_schema_definition"R + +} // namespace schemas +} // namespace rmf_task_sequence From 8c2844f39cc61a3071240086046e3cdd59db2d55 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Oct 2021 12:46:56 +0800 Subject: [PATCH 33/85] Working on the generation of schema headers Signed-off-by: Michael X. Grey --- rmf_task_sequence/cmake/generate_schema_header.cmake | 12 ++++++++++++ ...son => backup_PhaseSequenceTask_v0_1.schema.json} | 0 rmf_task_sequence/templates/schemas_template.hpp.in | 2 +- 3 files changed, 13 insertions(+), 1 deletion(-) rename rmf_task_sequence/schemas/{backup_PhaseSequenceTask-0_1.schema.json => backup_PhaseSequenceTask_v0_1.schema.json} (100%) diff --git a/rmf_task_sequence/cmake/generate_schema_header.cmake b/rmf_task_sequence/cmake/generate_schema_header.cmake index e69de29b..d59655b3 100644 --- a/rmf_task_sequence/cmake/generate_schema_header.cmake +++ b/rmf_task_sequence/cmake/generate_schema_header.cmake @@ -0,0 +1,12 @@ +################################################# +# generate_schema_header() +# +# This function takes a schema file and generates a C++ header +# file that hardcodes the schema into it as a const string. +function(generate_schema_header file_name) + get_filename_component(schema_name ${file_name} NAME_WE) + string(TOUPPER ${schema_name} upper_schema_name) + configure_file( + ../templates/schemas_template.hpp.in + ) +endfunction() diff --git a/rmf_task_sequence/schemas/backup_PhaseSequenceTask-0_1.schema.json b/rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json similarity index 100% rename from rmf_task_sequence/schemas/backup_PhaseSequenceTask-0_1.schema.json rename to rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json diff --git a/rmf_task_sequence/templates/schemas_template.hpp.in b/rmf_task_sequence/templates/schemas_template.hpp.in index 21208f9f..e6f7b768 100644 --- a/rmf_task_sequence/templates/schemas_template.hpp.in +++ b/rmf_task_sequence/templates/schemas_template.hpp.in @@ -9,7 +9,7 @@ namespace rmf_task_sequence { namespace schemas { -const std::string @lower_schema_name@ = +const std::string @schema_name@ = R"raw_schema_definition( @lower_schema_text@)raw_schema_definition"R From 0615abf29bc8003fdf73de55fbac2981d0f8a9f0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Oct 2021 13:57:46 +0800 Subject: [PATCH 34/85] Update transitive dependency Signed-off-by: Michael X. Grey --- rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in index 07e63da9..4b671c2f 100644 --- a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in +++ b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in @@ -5,7 +5,7 @@ get_filename_component(rmf_task_sequence_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" include(CMakeFindDependencyMacro) find_dependency(rmf_task REQUIRED) -find_dependency(yaml-cpp REQUIRED) +find_dependency(nlohmann-json-dev REQUIRED) if(NOT TARGET rmf_task_sequence::rmf_task_sequence) include("${rmf_task_sequence_CMAKE_DIR}/rmf_task_sequence-targets.cmake") From 05b4754733288f238798c34e57c639edb520e3b5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Oct 2021 15:26:43 +0800 Subject: [PATCH 35/85] Able to generate schemas Signed-off-by: Michael X. Grey --- rmf_task_sequence/CMakeLists.txt | 3 + .../cmake/generate_schema_header.cmake | 6 +- .../rmf_task_sequence/phases/DropOff.hpp | 2 +- .../rmf_task_sequence/phases/GoToPlace.hpp | 2 +- .../rmf_task_sequence/phases/PickUp.hpp | 2 +- .../rmf_task_sequence/phases/WaitFor.hpp | 2 +- rmf_task_sequence/schemas/CMakeLists.txt | 12 + .../src/rmf_task_sequence/Task.cpp | 434 +++++++++--------- .../templates/schemas_template.hpp.in | 9 +- 9 files changed, 256 insertions(+), 216 deletions(-) create mode 100644 rmf_task_sequence/schemas/CMakeLists.txt diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index c5397c08..c5045685 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -46,6 +46,7 @@ target_include_directories(rmf_task_sequence PUBLIC $ $ + $ # for auto-generated headers ) if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) @@ -74,6 +75,8 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) ) endif() +# Generate the schema headers +add_subdirectory(schemas) # Create cmake config files include(CMakePackageConfigHelpers) diff --git a/rmf_task_sequence/cmake/generate_schema_header.cmake b/rmf_task_sequence/cmake/generate_schema_header.cmake index d59655b3..2cac214b 100644 --- a/rmf_task_sequence/cmake/generate_schema_header.cmake +++ b/rmf_task_sequence/cmake/generate_schema_header.cmake @@ -6,7 +6,11 @@ function(generate_schema_header file_name) get_filename_component(schema_name ${file_name} NAME_WE) string(TOUPPER ${schema_name} upper_schema_name) + file(READ ${file_name} schema_text) + configure_file( ../templates/schemas_template.hpp.in - ) + ${CMAKE_BINARY_DIR}/include/rmf_task_sequence/schemas/${schema_name}.hpp + @ONLY + ) endfunction() diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp index fc274b09..f4d57e13 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp @@ -103,7 +103,7 @@ class DropOff::Description : public Phase::Description const Parameters& parameters) const final; // Documentation inherited - YAML::Node serialize() const final; + nlohmann::json serialize() const final; class Implementation; private: diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp index a08667ed..6d889998 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp @@ -68,7 +68,7 @@ class GoToPlace::Description : public Phase::Description const rmf_task::Parameters& parameters) const final; // Documentation inherited - YAML::Node serialize() const final; + nlohmann::json serialize() const final; class Implementation; private: diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp index 8a41d675..b59a5a6a 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp @@ -103,7 +103,7 @@ class PickUp::Description : public Phase::Description const Parameters& parameters) const final; // Documentation inherited - YAML::Node serialize() const final; + nlohmann::json serialize() const final; class Implementation; private: diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp index f960327d..faa4a41e 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp @@ -74,7 +74,7 @@ class WaitFor::Description : public Phase::Description const rmf_task::Parameters& parameters) const final; // Documentation inherited - YAML::Node serialize() const final; + nlohmann::json serialize() const final; class Implementation; private: diff --git a/rmf_task_sequence/schemas/CMakeLists.txt b/rmf_task_sequence/schemas/CMakeLists.txt new file mode 100644 index 00000000..ec81d1d1 --- /dev/null +++ b/rmf_task_sequence/schemas/CMakeLists.txt @@ -0,0 +1,12 @@ +include(../cmake/generate_schema_header.cmake) + +file(GLOB schema_files "*.schema.json") + +foreach(schema_file ${schema_files}) + generate_schema_header(${schema_file}) +endforeach() + +install( + DIRECTORY ${CMAKE_BINARY_DIR}/include/ + DESTINATION include +) diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 995e1704..ab19923e 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -21,6 +21,8 @@ #include +#include + namespace rmf_task_sequence { namespace { @@ -251,8 +253,6 @@ auto Task::Active::backup() const -> Backup //============================================================================== void Task::Active::_load_backup(std::string backup_state_str) { - // TODO(MXG): This function (and really the entire class) could be refactored - // into a smarter component-based design instead of this wall-of-text. auto restore_phase_tag = std::make_shared( 0, "Restore from backup", @@ -263,212 +263,230 @@ void Task::Active::_load_backup(std::string backup_state_str) const auto start_time = std::chrono::steady_clock::now(); auto restore_phase_log = rmf_task::Log(); - const auto failed_to_restore = [&]() -> void - { - _pending_stages.clear(); - _phase_finished( - std::make_shared( - std::move(restore_phase_tag), - restore_phase_log.view(), - start_time, - std::chrono::steady_clock::now())); - - _finish_task(); - }; - - const auto get_state_text = [&]() -> std::string - { - return "\n```\n" + backup_state_str + "\n```\n"; - }; - - // TODO(MXG): Use a formal schema to validate the input instead of validating - // it manually. - const auto state = YAML::Load(backup_state_str); - const auto& schema = state["schema_version"]; - if (!schema) - { - restore_phase_log.error( - "The field [schema_version] is missing from the root directory of the " - "backup state:" + get_state_text()); - - return failed_to_restore(); - } - - if (!schema.IsScalar()) - { - restore_phase_log.error( - "The field [schema_version] contains an unsupported value type [" - + std::to_string(schema.Type()) + "] expected scalar type [" - + std::to_string(YAML::NodeType::Scalar) + "]" + get_state_text()); - - return failed_to_restore(); - } - - try - { - const auto schema_version = schema.as(); - if (schema_version != 1) - { - restore_phase_log.error( - "Unrecognized value for [schema_version]: " - + std::to_string(schema_version) +". Backup state might have been " - "produced by a newer or unsupported implementation." - + get_state_text()); - - return failed_to_restore(); - } - } - catch (const YAML::BadConversion& err) - { - restore_phase_log.error( - std::string("Invalid data type for [schema_version]: ") + err.what() - + get_state_text()); - - return failed_to_restore(); - } - - const auto current_phase_yaml = state["current_phase"]; - if (!current_phase_yaml) - { - restore_phase_log.error( - "[current_phase] element missing from backup" + get_state_text()); - - return failed_to_restore(); - } - - const auto current_phase_id_yaml = current_phase_yaml["id"]; - if (!current_phase_id_yaml) - { - restore_phase_log.error( - "[id] element missing from [current_phase]" + get_state_text()); - - return failed_to_restore(); - } - - const auto cancelled_from_yaml = current_phase_yaml["cancelled_from"]; - if (cancelled_from_yaml) - { - std::optional cancelled_from_phase_opt; - try - { - cancelled_from_phase_opt = cancelled_from_yaml.as(); - } - catch (const YAML::BadConversion& err) - { - restore_phase_log.error( - "[cancelled_from] element in [current_phase] has an invalid type" - + get_state_text()); - - return failed_to_restore(); - } - - const auto cancelled_from = cancelled_from_phase_opt.value(); - if (cancelled_from >= _cancel_sequence_initial_id) - { - restore_phase_log.error( - "Invalid value [" + std::to_string(cancelled_from) - + "] for [cancelled_from]. Value must be less than [" - + std::to_string(_cancel_sequence_initial_id) + "]" + get_state_text()); - - return failed_to_restore(); - } - - for (const auto& stage : _pending_stages) - { - if (stage->id == cancelled_from) - { - _prepare_cancellation_sequence(stage->cancellation_sequence); - break; - } - } - } - - std::optional current_phase_id_opt; - try - { - current_phase_id_opt = current_phase_id_yaml.as(); - } - catch (const YAML::BadConversion& err) - { - restore_phase_log.error( - "[id] element in [current_phase] has an invalid type" + get_state_text()); - - return failed_to_restore(); - } - - const auto current_phase_id = current_phase_id_opt.value(); - bool found_phase = false; - while (!found_phase) - { - const auto stage = _pending_stages.front(); - if (stage->id != current_phase_id) - { - _pending_stages.pop_front(); - continue; - } - found_phase = true; - } - - const auto skip_phases_yaml = state["skip_phases"]; - if (skip_phases_yaml) - { - std::vector skip_phases; - try - { - skip_phases = skip_phases_yaml.as>(); - } - catch (const YAML::BadConversion& err) - { - restore_phase_log.error( - std::string("Invalid data type for [skip_phases]: ") + err.what() - + get_state_text()); - - return failed_to_restore(); - } - - auto pending_it = _pending_phases.begin(); - for (const auto& id : skip_phases) - { - if (id == 0) - { - // This should probably issue a warning, because this would be kind of - // weird, but not really a problem - continue; - } - - if (pending_it == _pending_phases.end() - || id < (*pending_it)->tag()->id()) - { - // This shouldn't happen, but it's not a critical error. In the worst - // case, the operator needs to resend a skip command. - restore_phase_log.warn( - "Unexpected ordering of phase skip IDs" + get_state_text()); - continue; - } - - while (pending_it != _pending_phases.end() - && (*pending_it)->tag()->id() < id) - { - ++pending_it; - } - - (*pending_it)->will_be_skipped(true); - } - } - - const auto phase_state_yaml = current_phase_yaml["state"]; - if (!phase_state_yaml) - { - restore_phase_log.error( - "Missing [state] field for [current_phase]" + get_state_text()); - - return failed_to_restore(); - } - - _begin_next_stage(phase_state_yaml.as()); } +////============================================================================== +//void Task::Active::_load_backup(std::string backup_state_str) +//{ +// // TODO(MXG): This function (and really the entire class) could be refactored +// // into a smarter component-based design instead of this wall-of-text. +// auto restore_phase_tag = std::make_shared( +// 0, +// "Restore from backup", +// "The task progress is being restored from a backed up state", +// rmf_traffic::Duration(0)); + +// // TODO(MXG): Allow users to specify a custom clock for the log +// const auto start_time = std::chrono::steady_clock::now(); +// auto restore_phase_log = rmf_task::Log(); + +// const auto failed_to_restore = [&]() -> void +// { +// _pending_stages.clear(); +// _phase_finished( +// std::make_shared( +// std::move(restore_phase_tag), +// restore_phase_log.view(), +// start_time, +// std::chrono::steady_clock::now())); + +// _finish_task(); +// }; + +// const auto get_state_text = [&]() -> std::string +// { +// return "\n```\n" + backup_state_str + "\n```\n"; +// }; + +// // TODO(MXG): Use a formal schema to validate the input instead of validating +// // it manually. +// const auto state = YAML::Load(backup_state_str); +// const auto& schema = state["schema_version"]; +// if (!schema) +// { +// restore_phase_log.error( +// "The field [schema_version] is missing from the root directory of the " +// "backup state:" + get_state_text()); + +// return failed_to_restore(); +// } + +// if (!schema.IsScalar()) +// { +// restore_phase_log.error( +// "The field [schema_version] contains an unsupported value type [" +// + std::to_string(schema.Type()) + "] expected scalar type [" +// + std::to_string(YAML::NodeType::Scalar) + "]" + get_state_text()); + +// return failed_to_restore(); +// } + +// try +// { +// const auto schema_version = schema.as(); +// if (schema_version != 1) +// { +// restore_phase_log.error( +// "Unrecognized value for [schema_version]: " +// + std::to_string(schema_version) +". Backup state might have been " +// "produced by a newer or unsupported implementation." +// + get_state_text()); + +// return failed_to_restore(); +// } +// } +// catch (const YAML::BadConversion& err) +// { +// restore_phase_log.error( +// std::string("Invalid data type for [schema_version]: ") + err.what() +// + get_state_text()); + +// return failed_to_restore(); +// } + +// const auto current_phase_yaml = state["current_phase"]; +// if (!current_phase_yaml) +// { +// restore_phase_log.error( +// "[current_phase] element missing from backup" + get_state_text()); + +// return failed_to_restore(); +// } + +// const auto current_phase_id_yaml = current_phase_yaml["id"]; +// if (!current_phase_id_yaml) +// { +// restore_phase_log.error( +// "[id] element missing from [current_phase]" + get_state_text()); + +// return failed_to_restore(); +// } + +// const auto cancelled_from_yaml = current_phase_yaml["cancelled_from"]; +// if (cancelled_from_yaml) +// { +// std::optional cancelled_from_phase_opt; +// try +// { +// cancelled_from_phase_opt = cancelled_from_yaml.as(); +// } +// catch (const YAML::BadConversion& err) +// { +// restore_phase_log.error( +// "[cancelled_from] element in [current_phase] has an invalid type" +// + get_state_text()); + +// return failed_to_restore(); +// } + +// const auto cancelled_from = cancelled_from_phase_opt.value(); +// if (cancelled_from >= _cancel_sequence_initial_id) +// { +// restore_phase_log.error( +// "Invalid value [" + std::to_string(cancelled_from) +// + "] for [cancelled_from]. Value must be less than [" +// + std::to_string(_cancel_sequence_initial_id) + "]" + get_state_text()); + +// return failed_to_restore(); +// } + +// for (const auto& stage : _pending_stages) +// { +// if (stage->id == cancelled_from) +// { +// _prepare_cancellation_sequence(stage->cancellation_sequence); +// break; +// } +// } +// } + +// std::optional current_phase_id_opt; +// try +// { +// current_phase_id_opt = current_phase_id_yaml.as(); +// } +// catch (const YAML::BadConversion& err) +// { +// restore_phase_log.error( +// "[id] element in [current_phase] has an invalid type" + get_state_text()); + +// return failed_to_restore(); +// } + +// const auto current_phase_id = current_phase_id_opt.value(); +// bool found_phase = false; +// while (!found_phase) +// { +// const auto stage = _pending_stages.front(); +// if (stage->id != current_phase_id) +// { +// _pending_stages.pop_front(); +// continue; +// } + +// found_phase = true; +// } + +// const auto skip_phases_yaml = state["skip_phases"]; +// if (skip_phases_yaml) +// { +// std::vector skip_phases; +// try +// { +// skip_phases = skip_phases_yaml.as>(); +// } +// catch (const YAML::BadConversion& err) +// { +// restore_phase_log.error( +// std::string("Invalid data type for [skip_phases]: ") + err.what() +// + get_state_text()); + +// return failed_to_restore(); +// } + +// auto pending_it = _pending_phases.begin(); +// for (const auto& id : skip_phases) +// { +// if (id == 0) +// { +// // This should probably issue a warning, because this would be kind of +// // weird, but not really a problem +// continue; +// } + +// if (pending_it == _pending_phases.end() +// || id < (*pending_it)->tag()->id()) +// { +// // This shouldn't happen, but it's not a critical error. In the worst +// // case, the operator needs to resend a skip command. +// restore_phase_log.warn( +// "Unexpected ordering of phase skip IDs" + get_state_text()); +// continue; +// } + +// while (pending_it != _pending_phases.end() +// && (*pending_it)->tag()->id() < id) +// { +// ++pending_it; +// } + +// (*pending_it)->will_be_skipped(true); +// } +// } + +// const auto phase_state_yaml = current_phase_yaml["state"]; +// if (!phase_state_yaml) +// { +// restore_phase_log.error( +// "Missing [state] field for [current_phase]" + get_state_text()); + +// return failed_to_restore(); +// } + +// _begin_next_stage(phase_state_yaml.as()); +//} + //============================================================================== void Task::Active::_generate_pending_phases() { @@ -601,7 +619,7 @@ auto Task::Active::_generate_backup( Phase::Tag::Id current_phase_id, Phase::Active::Backup phase_backup) const -> Backup { - YAML::Node current_phase; + nlohmann::json current_phase; current_phase["id"] = current_phase_id; if (_cancelled_on_phase.has_value()) current_phase["cancelled_from"] = *_cancelled_on_phase; @@ -615,13 +633,13 @@ auto Task::Active::_generate_backup( skipping_phases.push_back(p->tag()->id()); } - YAML::Node root; + nlohmann::json root; root["schema_version"] = 1; root["current_phase"] = std::move(current_phase); root["skip_phases"] = std::move(skipping_phases); // TODO(MXG): Is there anything else we need to consider as part of the state? - return Backup::make(_next_task_backup_sequence_number++, YAML::Dump(root)); + return Backup::make(_next_task_backup_sequence_number++, root.dump()); } //============================================================================== diff --git a/rmf_task_sequence/templates/schemas_template.hpp.in b/rmf_task_sequence/templates/schemas_template.hpp.in index e6f7b768..c10e79e7 100644 --- a/rmf_task_sequence/templates/schemas_template.hpp.in +++ b/rmf_task_sequence/templates/schemas_template.hpp.in @@ -5,13 +5,16 @@ */ #ifndef RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ +#define RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ namespace rmf_task_sequence { namespace schemas { -const std::string @schema_name@ = -R"raw_schema_definition( -@lower_schema_text@)raw_schema_definition"R +constexpr const char* const @schema_name@ = +R"raw_schema( +@schema_text@)raw_schema"; } // namespace schemas } // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ From d730e0273188cc03e6e983ba55adb4a8267d6b86 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Oct 2021 22:48:03 +0800 Subject: [PATCH 36/85] Reimplement backups using json Signed-off-by: Michael X. Grey --- rmf_task_sequence/CMakeLists.txt | 4 +- .../include/rmf_task_sequence/Phase.hpp | 4 +- .../schemas/ErrorHandler.hpp | 55 +++ rmf_task_sequence/package.xml | 1 + .../src/rmf_task_sequence/Phase.cpp | 5 +- .../src/rmf_task_sequence/Task.cpp | 350 +++++++----------- .../schemas/ErrorHandler.cpp | 43 +++ .../templates/schemas_template.hpp.in | 6 +- 8 files changed, 237 insertions(+), 231 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/schemas/ErrorHandler.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/schemas/ErrorHandler.cpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index c5045685..15388570 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -22,6 +22,7 @@ include(GNUInstallDirs) find_package(rmf_task REQUIRED) find_package(nlohmann_json REQUIRED) +find_package(nlohmann_json_schema_validator_vendor REQUIRED) find_package(ament_cmake_catch2 QUIET) find_package(rmf_cmake_uncrustify QUIET) @@ -39,7 +40,8 @@ add_library(rmf_task_sequence SHARED target_link_libraries(rmf_task_sequence PUBLIC rmf_task::rmf_task - yaml-cpp + nlohmann_json::nlohmann_json + nlohmann_json_schema_validator ) target_include_directories(rmf_task_sequence diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 8251eaac..78695fdb 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -189,7 +189,7 @@ class Phase::Activator std::function get_state, ConstTagPtr tag, const Description& description, - std::optional backup_state, + std::optional backup_state, std::function update, std::function checkpoint, std::function finished) @@ -233,7 +233,7 @@ class Phase::Activator std::function get_state, ConstTagPtr tag, const Description& description, - std::optional backup_state, + std::optional backup_state, std::function update, std::function checkpoint, std::function finished) const; diff --git a/rmf_task_sequence/include/rmf_task_sequence/schemas/ErrorHandler.hpp b/rmf_task_sequence/include/rmf_task_sequence/schemas/ErrorHandler.hpp new file mode 100644 index 00000000..fe8e5f84 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/schemas/ErrorHandler.hpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__SCHEMAS__ERRORHANDLER_HPP +#define RMF_TASK_SEQUENCE__SCHEMAS__ERRORHANDLER_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace schemas { + +//============================================================================== +class ErrorHandler : public nlohmann::json_schema::error_handler +{ +public: + + void error( + const nlohmann::json::json_pointer& ptr, + const nlohmann::json& instance, + const std::string& message) final; + + struct Info + { + nlohmann::json::json_pointer ptr; + nlohmann::json instance; + std::string message; + }; + + std::optional failure; + + static std::optional has_error( + const nlohmann::json_schema::json_validator& validator, + const nlohmann::json json); +}; + +} // namespace schemas +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__SCHEMAS__ERRORHANDLER_HPP diff --git a/rmf_task_sequence/package.xml b/rmf_task_sequence/package.xml index 6888a0a7..f8c0889f 100644 --- a/rmf_task_sequence/package.xml +++ b/rmf_task_sequence/package.xml @@ -11,6 +11,7 @@ rmf_task nlohmann-json3-dev + nlohmann_json_schema_validator_vendor ament_cmake_catch2 rmf_cmake_uncrustify diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index 6d7496e4..a0863381 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -29,11 +29,10 @@ class Phase::Activator::Implementation }; //============================================================================== -Phase::ActivePtr Phase::Activator::activate( - std::function get_state, +Phase::ActivePtr Phase::Activator::activate(std::function get_state, ConstTagPtr tag, const Description& description, - std::optional backup_state, + std::optional backup_state, std::function update, std::function checkpoint, std::function finished) const diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index ab19923e..82e017b2 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -21,8 +21,13 @@ #include +#include #include +#include + +#include + namespace rmf_task_sequence { namespace { @@ -141,6 +146,8 @@ class Task::Active /// Get a weak reference to this object std::weak_ptr weak_from_this() const; + static const nlohmann::json_schema::json_validator backup_schema_validator; + private: /// _load_backup should only be used in the make(~) function. It will @@ -153,7 +160,7 @@ class Task::Active void _generate_pending_phases(); void _finish_phase(); - void _begin_next_stage(std::optional restore = std::nullopt); + void _begin_next_stage(std::optional restore = std::nullopt); void _finish_task(); void _prepare_cancellation_sequence( @@ -223,6 +230,12 @@ class Task::Active const uint64_t _cancel_sequence_initial_id; }; +//============================================================================== +const nlohmann::json_schema::json_validator +Task::Active::backup_schema_validator = + nlohmann::json_schema::json_validator( + schemas::backup_PhaseSequenceTask_v0_1); + //============================================================================== auto Task::Builder::add_phase( Phase::ConstDescriptionPtr description, @@ -263,229 +276,120 @@ void Task::Active::_load_backup(std::string backup_state_str) const auto start_time = std::chrono::steady_clock::now(); auto restore_phase_log = rmf_task::Log(); + const auto get_state_text = [&]() -> std::string + { + return "\n```\n" + backup_state_str + "\n```\n"; + }; -} + const auto failed_to_restore = [&]() -> void + { + _pending_stages.clear(); + _phase_finished( + std::make_shared( + std::move(restore_phase_tag), + restore_phase_log.view(), + start_time, + std::chrono::steady_clock::now())); + + _finish_task(); + }; + + const auto backup_state = nlohmann::json::parse(backup_state_str); + if (const auto result = + schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) + { + restore_phase_log.error( + "Error in state data while trying to restore task from backup: " + + result->message); + + return failed_to_restore(); + } + + const auto& current_phase_json = backup_state["current_phase"]; + const auto& cancelled_from_json = current_phase_json["cancelled_from"]; + if (cancelled_from_json) + { + const auto cancelled_from = cancelled_from_json.get(); + if (cancelled_from >= _cancel_sequence_initial_id) + { + restore_phase_log.error( + "Invalid value [" + std::to_string(cancelled_from) + + "] for [cancelled_from]. Value must be less than [" + + std::to_string(_cancel_sequence_initial_id) + "]" + get_state_text()); + + return failed_to_restore(); + } + + for (const auto& stage : _pending_stages) + { + if (stage->id == cancelled_from) + { + _prepare_cancellation_sequence(stage->cancellation_sequence); + break; + } + } + } + + const auto& current_phase_id_json = current_phase_json["id"]; + const auto current_phase_id = current_phase_id_json.get(); + bool found_phase = false; + while (!found_phase && !_pending_stages.empty()) + { + const auto stage = _pending_stages.front(); + if (stage->id != current_phase_id) + { + _pending_stages.pop_front(); + continue; + } + + found_phase = true; + } + + if (_pending_stages.empty()) + { + restore_phase_log.error( + "Invalid value [" + std::to_string(current_phase_id) + + "] for [current_phase/id]. " + "Value is higher than all available phase IDs."); + + return failed_to_restore(); + } + + const auto& skip_phases_json = backup_state["skip_phases"]; + if (skip_phases_json) + { + const auto skip_phases = skip_phases_json.get>(); + auto pending_it = _pending_phases.begin(); + const auto pending_end = _pending_phases.end(); + for (const auto& id : skip_phases) + { + if (id == 0) + { + // This should probably issue a warning, because this would be kind of + // weird, but it's not really a problem + continue; + } + + while (pending_it != pending_end && (*pending_it)->tag()->id() < id) + { + ++pending_it; + } + + if (pending_it == pending_end || id < (*pending_it)->tag()->id()) + { + // This shouldn't happen, but it's not a critical error. In the worst + // case, the operator needs to resend a skip command. + restore_phase_log.warn( + "Unexpected ordering of phase skip IDs" + get_state_text()); + continue; + } + + (*pending_it)->will_be_skipped(true); + } + } -////============================================================================== -//void Task::Active::_load_backup(std::string backup_state_str) -//{ -// // TODO(MXG): This function (and really the entire class) could be refactored -// // into a smarter component-based design instead of this wall-of-text. -// auto restore_phase_tag = std::make_shared( -// 0, -// "Restore from backup", -// "The task progress is being restored from a backed up state", -// rmf_traffic::Duration(0)); - -// // TODO(MXG): Allow users to specify a custom clock for the log -// const auto start_time = std::chrono::steady_clock::now(); -// auto restore_phase_log = rmf_task::Log(); - -// const auto failed_to_restore = [&]() -> void -// { -// _pending_stages.clear(); -// _phase_finished( -// std::make_shared( -// std::move(restore_phase_tag), -// restore_phase_log.view(), -// start_time, -// std::chrono::steady_clock::now())); - -// _finish_task(); -// }; - -// const auto get_state_text = [&]() -> std::string -// { -// return "\n```\n" + backup_state_str + "\n```\n"; -// }; - -// // TODO(MXG): Use a formal schema to validate the input instead of validating -// // it manually. -// const auto state = YAML::Load(backup_state_str); -// const auto& schema = state["schema_version"]; -// if (!schema) -// { -// restore_phase_log.error( -// "The field [schema_version] is missing from the root directory of the " -// "backup state:" + get_state_text()); - -// return failed_to_restore(); -// } - -// if (!schema.IsScalar()) -// { -// restore_phase_log.error( -// "The field [schema_version] contains an unsupported value type [" -// + std::to_string(schema.Type()) + "] expected scalar type [" -// + std::to_string(YAML::NodeType::Scalar) + "]" + get_state_text()); - -// return failed_to_restore(); -// } - -// try -// { -// const auto schema_version = schema.as(); -// if (schema_version != 1) -// { -// restore_phase_log.error( -// "Unrecognized value for [schema_version]: " -// + std::to_string(schema_version) +". Backup state might have been " -// "produced by a newer or unsupported implementation." -// + get_state_text()); - -// return failed_to_restore(); -// } -// } -// catch (const YAML::BadConversion& err) -// { -// restore_phase_log.error( -// std::string("Invalid data type for [schema_version]: ") + err.what() -// + get_state_text()); - -// return failed_to_restore(); -// } - -// const auto current_phase_yaml = state["current_phase"]; -// if (!current_phase_yaml) -// { -// restore_phase_log.error( -// "[current_phase] element missing from backup" + get_state_text()); - -// return failed_to_restore(); -// } - -// const auto current_phase_id_yaml = current_phase_yaml["id"]; -// if (!current_phase_id_yaml) -// { -// restore_phase_log.error( -// "[id] element missing from [current_phase]" + get_state_text()); - -// return failed_to_restore(); -// } - -// const auto cancelled_from_yaml = current_phase_yaml["cancelled_from"]; -// if (cancelled_from_yaml) -// { -// std::optional cancelled_from_phase_opt; -// try -// { -// cancelled_from_phase_opt = cancelled_from_yaml.as(); -// } -// catch (const YAML::BadConversion& err) -// { -// restore_phase_log.error( -// "[cancelled_from] element in [current_phase] has an invalid type" -// + get_state_text()); - -// return failed_to_restore(); -// } - -// const auto cancelled_from = cancelled_from_phase_opt.value(); -// if (cancelled_from >= _cancel_sequence_initial_id) -// { -// restore_phase_log.error( -// "Invalid value [" + std::to_string(cancelled_from) -// + "] for [cancelled_from]. Value must be less than [" -// + std::to_string(_cancel_sequence_initial_id) + "]" + get_state_text()); - -// return failed_to_restore(); -// } - -// for (const auto& stage : _pending_stages) -// { -// if (stage->id == cancelled_from) -// { -// _prepare_cancellation_sequence(stage->cancellation_sequence); -// break; -// } -// } -// } - -// std::optional current_phase_id_opt; -// try -// { -// current_phase_id_opt = current_phase_id_yaml.as(); -// } -// catch (const YAML::BadConversion& err) -// { -// restore_phase_log.error( -// "[id] element in [current_phase] has an invalid type" + get_state_text()); - -// return failed_to_restore(); -// } - -// const auto current_phase_id = current_phase_id_opt.value(); -// bool found_phase = false; -// while (!found_phase) -// { -// const auto stage = _pending_stages.front(); -// if (stage->id != current_phase_id) -// { -// _pending_stages.pop_front(); -// continue; -// } - -// found_phase = true; -// } - -// const auto skip_phases_yaml = state["skip_phases"]; -// if (skip_phases_yaml) -// { -// std::vector skip_phases; -// try -// { -// skip_phases = skip_phases_yaml.as>(); -// } -// catch (const YAML::BadConversion& err) -// { -// restore_phase_log.error( -// std::string("Invalid data type for [skip_phases]: ") + err.what() -// + get_state_text()); - -// return failed_to_restore(); -// } - -// auto pending_it = _pending_phases.begin(); -// for (const auto& id : skip_phases) -// { -// if (id == 0) -// { -// // This should probably issue a warning, because this would be kind of -// // weird, but not really a problem -// continue; -// } - -// if (pending_it == _pending_phases.end() -// || id < (*pending_it)->tag()->id()) -// { -// // This shouldn't happen, but it's not a critical error. In the worst -// // case, the operator needs to resend a skip command. -// restore_phase_log.warn( -// "Unexpected ordering of phase skip IDs" + get_state_text()); -// continue; -// } - -// while (pending_it != _pending_phases.end() -// && (*pending_it)->tag()->id() < id) -// { -// ++pending_it; -// } - -// (*pending_it)->will_be_skipped(true); -// } -// } - -// const auto phase_state_yaml = current_phase_yaml["state"]; -// if (!phase_state_yaml) -// { -// restore_phase_log.error( -// "Missing [state] field for [current_phase]" + get_state_text()); - -// return failed_to_restore(); -// } - -// _begin_next_stage(phase_state_yaml.as()); -//} + _begin_next_stage(current_phase_json["state"]); +} //============================================================================== void Task::Active::_generate_pending_phases() @@ -521,7 +425,7 @@ void Task::Active::_finish_phase() } //============================================================================== -void Task::Active::_begin_next_stage(std::optional restore) +void Task::Active::_begin_next_stage(std::optional restore) { if (_pending_stages.empty()) return _finish_task(); diff --git a/rmf_task_sequence/src/rmf_task_sequence/schemas/ErrorHandler.cpp b/rmf_task_sequence/src/rmf_task_sequence/schemas/ErrorHandler.cpp new file mode 100644 index 00000000..21f617bc --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/schemas/ErrorHandler.cpp @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { +namespace schemas { + +//============================================================================== +void ErrorHandler::error( + const nlohmann::json::json_pointer& ptr, + const nlohmann::json& instance, + const std::string& message) +{ + failure = Info{ptr, instance, message}; +} + +//============================================================================== +auto ErrorHandler::has_error( + const nlohmann::json_schema::json_validator& validator, + const nlohmann::json json) -> std::optional +{ + ErrorHandler handler; + validator.validate(json, handler); + return handler.failure; +} + +} // namespace schemas +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/templates/schemas_template.hpp.in b/rmf_task_sequence/templates/schemas_template.hpp.in index c10e79e7..80d3b154 100644 --- a/rmf_task_sequence/templates/schemas_template.hpp.in +++ b/rmf_task_sequence/templates/schemas_template.hpp.in @@ -7,12 +7,14 @@ #ifndef RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ #define RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ +#include + namespace rmf_task_sequence { namespace schemas { -constexpr const char* const @schema_name@ = +static const nlohmann::json @schema_name@ = R"raw_schema( -@schema_text@)raw_schema"; +@schema_text@)raw_schema"_json; } // namespace schemas } // namespace rmf_task_sequence From ae6c0c7196a9b800634be66b0bd643966143b7b2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 11 Oct 2021 14:40:47 +0800 Subject: [PATCH 37/85] Include original message in error log Signed-off-by: Michael X. Grey --- rmf_task_sequence/src/rmf_task_sequence/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 82e017b2..fbd9a467 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -300,7 +300,7 @@ void Task::Active::_load_backup(std::string backup_state_str) { restore_phase_log.error( "Error in state data while trying to restore task from backup: " + - result->message); + result->message + "\n - Original message:" + get_state_text()); return failed_to_restore(); } From d744b630cc05e6724d3611145fb506dbf76395c8 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 12 Oct 2021 16:35:03 +0800 Subject: [PATCH 38/85] Put active phase snapshots into completed phases Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Phase.hpp | 4 +- .../include/rmf_task/phases/RestoreBackup.hpp | 56 +++++++++++++++++++ rmf_task/src/rmf_task/Phase.cpp | 13 ++--- rmf_task/test/mock/MockTask.cpp | 2 +- 4 files changed, 65 insertions(+), 10 deletions(-) create mode 100644 rmf_task/include/rmf_task/phases/RestoreBackup.hpp diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 4ad1e4c8..6693e6dd 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -105,7 +105,7 @@ class Phase::Completed /// Constructor Completed( ConstTagPtr tag_, - Log::View log_, + ConstSnapshotPtr snapshot_, rmf_traffic::Time start_, rmf_traffic::Time finish_); @@ -113,7 +113,7 @@ class Phase::Completed const ConstTagPtr& tag() const; /// The final log of the phase - const Log::View& log() const; + const ConstSnapshotPtr& snapshot() const; /// The actual time that the phase started rmf_traffic::Time start_time() const; diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp new file mode 100644 index 00000000..1c8ccbaf --- /dev/null +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__PHASES__RESTOREBACKUP_HPP +#define RMF_TASK__PHASES__RESTOREBACKUP_HPP + +#include + +namespace rmf_task_sequence { +namespace phases { + +//============================================================================== +class RestoreBackup +{ +public: + + class Active; + using ActivePtr = std::shared_ptr; + + class Condition; + using ConditionPtr = std::shared_ptr; +}; + +//============================================================================== +class RestoreBackup::Active +{ +public: + + static ActivePtr make(std::string backup_state_str); + + + + class Implementation; +private: + Active(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace phases +} // namespace rmf_task_sequence + +#endif // RMF_TASK__PHASES__RESTOREBACKUP_HPP diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 179685f8..5b461142 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -78,21 +78,20 @@ class Phase::Completed::Implementation public: ConstTagPtr tag; - Log::View log; + ConstSnapshotPtr snapshot; rmf_traffic::Time start; rmf_traffic::Time finish; }; //============================================================================== -Phase::Completed::Completed( - ConstTagPtr tag_, - Log::View log_, +Phase::Completed::Completed(ConstTagPtr tag_, + ConstSnapshotPtr snapshot_, rmf_traffic::Time start_, rmf_traffic::Time finish_) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(tag_), - std::move(log_), + std::move(snapshot_), start_, finish_ })) @@ -107,9 +106,9 @@ auto Phase::Completed::tag() const -> const ConstTagPtr& } //============================================================================== -const Log::View& Phase::Completed::log() const +auto Phase::Completed::snapshot() const -> const ConstSnapshotPtr& { - return _pimpl->log; + return _pimpl->snapshot; } //============================================================================== diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index 2d748663..6e025ee1 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -129,7 +129,7 @@ void MockTask::Active::start_next_phase(rmf_traffic::Time current_time) _phase_finished( std::make_shared( _active_phase->tag(), - _active_phase->finish_condition()->log(), + rmf_task::Phase::Snapshot::make(*_active_phase), _active_phase->_start_time, current_time)); } From eab243b4842444c05bf79c77b2564f5489e74a76 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 13 Oct 2021 17:41:24 +0800 Subject: [PATCH 39/85] Iterating on RestoreBackup phase Signed-off-by: Michael X. Grey --- .../include/rmf_task/phases/RestoreBackup.hpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index 1c8ccbaf..f9f85605 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -20,11 +20,11 @@ #include -namespace rmf_task_sequence { +namespace rmf_task { namespace phases { //============================================================================== -class RestoreBackup +class RestoreBackup : public Phase { public: @@ -36,12 +36,20 @@ class RestoreBackup }; //============================================================================== -class RestoreBackup::Active +class RestoreBackup::Active : public Phase::Active { public: + /// Make an active RestoreBackup phase static ActivePtr make(std::string backup_state_str); + // Documentation inherited + ConstTagPtr tag() const final; + + // Documentation inherited + ConstConditionPtr finish_condition() const final; + + class Implementation; @@ -51,6 +59,6 @@ class RestoreBackup::Active }; } // namespace phases -} // namespace rmf_task_sequence +} // namespace rmf_task #endif // RMF_TASK__PHASES__RESTOREBACKUP_HPP From cc329cf254c814b2fab0e6e6e7c41ec8d899d9b4 Mon Sep 17 00:00:00 2001 From: Charayaphan Nakorn Boon Han Date: Thu, 14 Oct 2021 21:56:18 +0800 Subject: [PATCH 40/85] Implement backup file manager (#41) Signed-off-by: Michael X. Grey Signed-off-by: Charayaphan Nakorn Boon Han --- .../include/rmf_task/BackupFileManager.hpp | 4 +- rmf_task/include/rmf_task/Phase.hpp | 4 +- rmf_task/include/rmf_task/Task.hpp | 2 +- .../include/rmf_task/phases/RestoreBackup.hpp | 61 +++++++ rmf_task/src/rmf_task/BackupFileManager.cpp | 150 +++++++++++++++--- rmf_task/src/rmf_task/Log.cpp | 4 +- rmf_task/src/rmf_task/Payload.cpp | 12 +- rmf_task/src/rmf_task/Phase.cpp | 34 ++-- rmf_task/src/rmf_task/Task.cpp | 12 +- rmf_task/test/integration/test_backups.cpp | 136 ++++++++++++++-- rmf_task/test/mock/MockCondition.cpp | 6 +- rmf_task/test/mock/MockDelivery.cpp | 5 +- rmf_task/test/mock/MockDelivery.hpp | 25 ++- rmf_task/test/mock/MockPhase.cpp | 10 +- rmf_task/test/mock/MockTask.cpp | 10 +- rmf_task/test/unit/test_Activator.cpp | 16 +- .../src/rmf_task_sequence/Phase.cpp | 3 +- .../src/rmf_task_sequence/Task.cpp | 2 +- 18 files changed, 397 insertions(+), 99 deletions(-) create mode 100644 rmf_task/include/rmf_task/phases/RestoreBackup.hpp diff --git a/rmf_task/include/rmf_task/BackupFileManager.hpp b/rmf_task/include/rmf_task/BackupFileManager.hpp index 0a54220c..53ce0597 100644 --- a/rmf_task/include/rmf_task/BackupFileManager.hpp +++ b/rmf_task/include/rmf_task/BackupFileManager.hpp @@ -36,7 +36,9 @@ class BackupFileManager /// /// \param[in] root_directory /// Specify the root directory that the backup files should live in - BackupFileManager(std::filesystem::path root_directory); + BackupFileManager(std::filesystem::path root_directory, + std::function info_logger = nullptr, + std::function debug_logger = nullptr); /// Set whether any previously existing backups should be cleared out on /// startup. By default this behavior is turned OFF. diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 4ad1e4c8..6693e6dd 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -105,7 +105,7 @@ class Phase::Completed /// Constructor Completed( ConstTagPtr tag_, - Log::View log_, + ConstSnapshotPtr snapshot_, rmf_traffic::Time start_, rmf_traffic::Time finish_); @@ -113,7 +113,7 @@ class Phase::Completed const ConstTagPtr& tag() const; /// The final log of the phase - const Log::View& log() const; + const ConstSnapshotPtr& snapshot() const; /// The actual time that the phase started rmf_traffic::Time start_time() const; diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 1052bbb1..95623e9a 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -267,7 +267,7 @@ class Task::Active /// /// \param[in] value /// True if the phase should be skipped, false otherwise. - virtual void skip(uint64_t phase_id, bool value=true) = 0; + virtual void skip(uint64_t phase_id, bool value = true) = 0; /// Rewind the Task to a specific phase. This can be issued by operators if /// a phase did not actually go as intended and needs to be repeated. diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp new file mode 100644 index 00000000..5494c86e --- /dev/null +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__PHASES__RESTOREBACKUP_HPP +#define RMF_TASK__PHASES__RESTOREBACKUP_HPP + +#include + +namespace rmf_task { +namespace phases { + +//============================================================================== +class RestoreBackup : public Phase +{ +public: + + class Active; + using ActivePtr = std::shared_ptr; + + class Condition; + using ConditionPtr = std::shared_ptr; +}; + +//============================================================================== +class RestoreBackup::Active : public Phase::Active +{ +public: + + /// Make an active RestoreBackup phase + static ActivePtr make(std::string backup_state_str); + + // Documentation inherited + ConstTagPtr tag() const final; + + // Documentation inherited + ConstConditionPtr finish_condition() const final; + + class Implementation; +private: + Active(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace phases +} // namespace rmf_task + +#endif // RMF_TASK__PHASES__RESTOREBACKUP_HPP diff --git a/rmf_task/src/rmf_task/BackupFileManager.cpp b/rmf_task/src/rmf_task/BackupFileManager.cpp index 8aca9608..9ea310e3 100644 --- a/rmf_task/src/rmf_task/BackupFileManager.cpp +++ b/rmf_task/src/rmf_task/BackupFileManager.cpp @@ -15,6 +15,9 @@ * */ +#include +#include +#include #include #include @@ -30,17 +33,23 @@ class BackupFileManager::Implementation { bool clear_on_startup = false; bool clear_on_shutdown = true; + std::function info_logger = nullptr; + std::function debug_logger = nullptr; }; using ConstSettingsPtr = std::shared_ptr; - Implementation(std::filesystem::path directory) + Implementation( + std::filesystem::path directory, + std::function info, + std::function debug) : root_directory(std::move(directory)), settings(std::make_shared()) { - // Do nothing + settings->info_logger = std::move(info); + settings->debug_logger = std::move(debug); } - std::filesystem::path root_directory; + const std::filesystem::path root_directory; std::shared_ptr settings; std::unordered_map> groups; @@ -59,11 +68,11 @@ class BackupFileManager::Group::Implementation : group_directory(std::move(directory)), settings(std::move(settings)) { - // Do nothing + std::filesystem::create_directories(this->group_directory); } template - static std::shared_ptr make(Args&&... args) + static std::shared_ptr make(Args&& ... args) { Group output; output._pimpl = rmf_utils::make_unique_impl( @@ -72,7 +81,7 @@ class BackupFileManager::Group::Implementation return std::make_shared(std::move(output)); } - std::filesystem::path group_directory; + const std::filesystem::path group_directory; ConstSettingsPtr settings; std::unordered_map> robots; @@ -91,12 +100,14 @@ class BackupFileManager::Robot::Implementation : robot_directory(std::move(directory)), settings(std::move(settings)) { - if (settings->clear_on_startup) - clear_backup(); + if (this->settings->clear_on_startup) + this->clear_backup(); + + std::filesystem::create_directories(this->robot_directory); } template - static std::shared_ptr make(Args&&... args) + static std::shared_ptr make(Args&& ... args) { Robot output; output._pimpl = rmf_utils::make_unique_impl( @@ -111,9 +122,14 @@ class BackupFileManager::Robot::Implementation clear_backup(); } - std::filesystem::path robot_directory; + const std::filesystem::path robot_directory; ConstSettingsPtr settings; std::optional last_seq; + const std::string backup_file_name = "backup"; + const std::string pre_backup_file_name = ".backup"; + const std::string pre_backup_file_path = robot_directory / + pre_backup_file_name; + const std::string backup_file_path = robot_directory / backup_file_name; void write_if_new(const Task::Active::Backup& backup) { @@ -127,24 +143,63 @@ class BackupFileManager::Robot::Implementation write(backup.state()); } + void log_debug(const std::string msg) const + { + if (settings->debug_logger) + { + settings->debug_logger(msg); + } + else + { + std::cout << msg << std::endl; + } + } + + void log_info(const std::string msg) const + { + if (settings->info_logger) + { + settings->info_logger(msg); + } + else + { + std::cout << msg << std::endl; + } + } + private: void write(const std::string& state) { - throw std::runtime_error( - "[BackupFileManager::Robot::Implementation::write] not implemented yet"); + std::ofstream pre_backup(pre_backup_file_path, std::ios::out); + if (!pre_backup) + throw std::runtime_error( + "Could not open file " + pre_backup_file_path + + " for pre_backup."); + else + { + pre_backup << state; + pre_backup.close(); + std::filesystem::rename(pre_backup_file_path, backup_file_path); + } } void clear_backup() { - throw std::runtime_error( - "[BackupFileManager::Robot::Implementation::clear_backup] " - "not implemented yet"); + if (std::filesystem::exists(robot_directory)) + std::filesystem::remove(pre_backup_file_path); + std::filesystem::remove(backup_file_path); } + }; //============================================================================== -BackupFileManager::BackupFileManager(std::filesystem::path root_directory) -: _pimpl(rmf_utils::make_unique_impl(std::move(root_directory))) +BackupFileManager::BackupFileManager(std::filesystem::path root_directory, + std::function info, + std::function debug) +: _pimpl(rmf_utils::make_unique_impl( + std::move(root_directory), + std::move(info), + std::move(debug))) { // Do nothing } @@ -214,8 +269,64 @@ BackupFileManager::Group::Group() //============================================================================== std::optional BackupFileManager::Robot::read() const { - throw std::runtime_error( - "[BackupFileManager::Robot::read] not implemented yet"); + if (!std::filesystem::exists(_pimpl->robot_directory)) + { + throw std::runtime_error("[BackupFileManager::Robot::read] Directory " + + _pimpl->robot_directory.string() + + " missing. This should not happen."); + } + + if (std::filesystem::is_empty(_pimpl->robot_directory)) + return std::nullopt; + + // Check for foreign files + auto directory_it = std::filesystem::directory_iterator( + _pimpl->robot_directory); + for (auto& p: directory_it) + { + auto filename = p.path().filename().string(); + if (filename.compare(_pimpl->backup_file_name) != 0 && + filename.compare(_pimpl->pre_backup_file_name) != 0) + { + throw std::runtime_error("[BackupFileManager::Robot::read] Foreign file " + + filename + " found. This should be removed."); + } + } + + // At this point, file is either backup_file_name, or .backup_file_name, or both + if (std::filesystem::exists(_pimpl->backup_file_path)) + { + if (std::filesystem::exists(_pimpl->pre_backup_file_path)) + { + //suspicious to have both backup files, something definitely broke in the previous run. + _pimpl->log_debug( + "[BackupFileManager::Robot::read] Multiple backup files found. This suggests an error with the previous backup run. Using the older edited backup file.."); + std::filesystem::remove(_pimpl->pre_backup_file_path); + } + + std::ifstream backup(_pimpl->backup_file_path, std::ios::in); + if (!backup) + throw std::runtime_error( + "Could not open file " + _pimpl->backup_file_path + + " for backup."); + else + { + std::stringstream buffer; + buffer << backup.rdbuf(); + return std::optional(buffer.str()); + } + } + else + { + // At this point, we either have exactly .backup, or no files at all + if (std::filesystem::exists(_pimpl->pre_backup_file_path)) + { + std::filesystem::remove(_pimpl->pre_backup_file_path); + } + + return std::nullopt; + } + } //============================================================================== @@ -231,3 +342,4 @@ BackupFileManager::Robot::Robot() } } // namespace rmf_task + diff --git a/rmf_task/src/rmf_task/Log.cpp b/rmf_task/src/rmf_task/Log.cpp index eab73595..a6d69619 100644 --- a/rmf_task/src/rmf_task/Log.cpp +++ b/rmf_task/src/rmf_task/Log.cpp @@ -34,7 +34,7 @@ class Log::Implementation { if (!clock) { - clock = [](){ return std::chrono::system_clock::now(); }; + clock = []() { return std::chrono::system_clock::now(); }; } } @@ -79,7 +79,7 @@ class Log::View::Implementation if (log._pimpl->entries->empty()) { output._pimpl = rmf_utils::make_impl( - Implementation{log._pimpl->entries, std::nullopt, std::nullopt}); + Implementation{log._pimpl->entries, std::nullopt, std::nullopt}); } else { diff --git a/rmf_task/src/rmf_task/Payload.cpp b/rmf_task/src/rmf_task/Payload.cpp index 8a346adc..8d031cea 100644 --- a/rmf_task/src/rmf_task/Payload.cpp +++ b/rmf_task/src/rmf_task/Payload.cpp @@ -35,7 +35,7 @@ class Payload::Implementation //============================================================================== Payload::Payload(std::vector components) : _pimpl(rmf_utils::make_impl( - Implementation{std::move(components)})) + Implementation{std::move(components)})) { // Do nothing } @@ -112,11 +112,11 @@ Payload::Component::Component( uint32_t quantity, std::string compartment) : _pimpl(rmf_utils::make_impl( - Implementation{ - std::move(sku), - quantity, - std::move(compartment) - })) + Implementation{ + std::move(sku), + quantity, + std::move(compartment) + })) { // Do nothing } diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 179685f8..66ddc20f 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -38,12 +38,12 @@ Phase::Tag::Tag( std::string detail_, rmf_traffic::Duration estimate_) : _pimpl(rmf_utils::make_impl( - Implementation{ - id_, - std::move(name_), - std::move(detail_), - estimate_ - })) + Implementation{ + id_, + std::move(name_), + std::move(detail_), + estimate_ + })) { // Do nothing } @@ -78,7 +78,7 @@ class Phase::Completed::Implementation public: ConstTagPtr tag; - Log::View log; + ConstSnapshotPtr snapshot; rmf_traffic::Time start; rmf_traffic::Time finish; }; @@ -86,16 +86,16 @@ class Phase::Completed::Implementation //============================================================================== Phase::Completed::Completed( ConstTagPtr tag_, - Log::View log_, + ConstSnapshotPtr snapshot_, rmf_traffic::Time start_, rmf_traffic::Time finish_) : _pimpl(rmf_utils::make_impl( - Implementation{ - std::move(tag_), - std::move(log_), - start_, - finish_ - })) + Implementation{ + std::move(tag_), + std::move(snapshot_), + start_, + finish_ + })) { // Do nothing } @@ -107,9 +107,9 @@ auto Phase::Completed::tag() const -> const ConstTagPtr& } //============================================================================== -const Log::View& Phase::Completed::log() const +auto Phase::Completed::snapshot() const -> const ConstSnapshotPtr& { - return _pimpl->log; + return _pimpl->snapshot; } //============================================================================== @@ -156,7 +156,7 @@ Phase::ConstTagPtr Phase::Snapshot::tag() const //============================================================================== ConstConditionPtr Phase::Snapshot::finish_condition() const { - return _pimpl->finish_condition; + return _pimpl->finish_condition; } //============================================================================== diff --git a/rmf_task/src/rmf_task/Task.cpp b/rmf_task/src/rmf_task/Task.cpp index 80d68d0a..aebdff98 100644 --- a/rmf_task/src/rmf_task/Task.cpp +++ b/rmf_task/src/rmf_task/Task.cpp @@ -89,12 +89,12 @@ Task::Tag::Tag( std::string detail_, rmf_traffic::Time original_finish_estimate_) : _pimpl(rmf_utils::make_impl( - Implementation{ - std::move(booking_), - std::move(category_), - std::move(detail_), - original_finish_estimate_ - })) + Implementation{ + std::move(booking_), + std::move(category_), + std::move(detail_), + original_finish_estimate_ + })) { // Do nothing } diff --git a/rmf_task/test/integration/test_backups.cpp b/rmf_task/test/integration/test_backups.cpp index c72a9c5b..233a762e 100644 --- a/rmf_task/test/integration/test_backups.cpp +++ b/rmf_task/test/integration/test_backups.cpp @@ -15,24 +15,132 @@ * */ +#include +#include #include #include "../mock/MockDelivery.hpp" #include -SCENARIO("Back up to file") +const std::filesystem::path backup_root_dir = "/tmp/rmf_task/test_backups"; +void cleanup() { + std::filesystem::remove_all(std::filesystem::weakly_canonical(backup_root_dir)); +} + +SCENARIO("Backup file creation and clearing tests") +{ + cleanup(); + rmf_task::Activator activator; activator.add_activator(test_rmf_task::MockDelivery::make_activator()); + rmf_task::BackupFileManager backup(backup_root_dir); + + auto group_backup = backup.make_group("group"); + CHECK(std::filesystem::exists(backup_root_dir / "group")); + + auto robot_backup = group_backup->make_robot("robot"); + CHECK(std::filesystem::exists(backup_root_dir / "group" / "robot")); + + using namespace std::chrono_literals; + auto request = rmf_task::requests::Delivery::make( + 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + rmf_task::Phase::ConstSnapshotPtr phase_snapshot; + auto active_task = activator.activate( + nullptr, + nullptr, + *request, + [&phase_snapshot](auto s) { phase_snapshot = s; }, + [robot_backup](auto b) { robot_backup->write(b); }, + [](auto) {}, + []() {}); + REQUIRE(active_task); + + auto mock_active_task = + std::dynamic_pointer_cast(active_task); + REQUIRE(mock_active_task); + + auto mock_active_task_backup = mock_active_task->backup(); + + robot_backup->write(mock_active_task_backup); + CHECK(std::filesystem::exists(backup_root_dir / "group" / "robot" / + "backup")); + CHECK_FALSE(std::filesystem::exists(backup_root_dir / "group" / "robot" / + ".backup")); + + robot_backup.reset(); + active_task.reset(); + mock_active_task.reset(); + CHECK(std::filesystem::exists(backup_root_dir / "group" / "robot")); + CHECK_FALSE(std::filesystem::exists(backup_root_dir / "group" / "robot" / + "backup")); + CHECK_FALSE(std::filesystem::exists(backup_root_dir / "group" / "robot" / + ".backup")); + + group_backup.reset(); // Should not delete group or robot folder + CHECK(std::filesystem::exists(backup_root_dir / "group")); + CHECK(std::filesystem::exists(backup_root_dir / "group" / "robot")); +} + +SCENARIO("RAII tests for temporary BackupFileManager::Robot instances") +{ + cleanup(); + + rmf_task::Activator activator; + activator.add_activator(test_rmf_task::MockDelivery::make_activator()); + rmf_task::BackupFileManager backup(backup_root_dir); + + auto group_backup = backup.make_group("group"); - const std::string backup_root_dir = "/tmp/rmf_task/test_backups"; + using namespace std::chrono_literals; + auto request = rmf_task::requests::Delivery::make( + 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + rmf_task::Phase::ConstSnapshotPtr phase_snapshot; + auto active_task = activator.activate( + nullptr, + nullptr, + *request, + [&phase_snapshot](auto s) { phase_snapshot = s; }, + [](auto) {}, + [](auto) {}, + []() {}); + REQUIRE(active_task); + + auto mock_active_task = + std::dynamic_pointer_cast(active_task); + REQUIRE(mock_active_task); + + auto mock_active_task_backup = mock_active_task->backup(); + + CHECK_FALSE(std::filesystem::exists(backup_root_dir / "group" / "robot" / + "backup")); + group_backup->make_robot("robot")->write(mock_active_task_backup); + // Due to RAII and clear_on_shutdown the destructor is called immediately + CHECK_FALSE(std::filesystem::exists(backup_root_dir / "group" / "robot" / + "backup")); +} + + +SCENARIO("Back up to file") +{ + cleanup(); + + rmf_task::Activator activator; + activator.add_activator(test_rmf_task::MockDelivery::make_activator()); rmf_task::BackupFileManager backup(backup_root_dir); - backup.clear_on_startup(); auto robot_backup = backup.make_group("group")->make_robot("robot"); CHECK_FALSE(robot_backup->read().has_value()); + //// ====== We should get a nullopt on restoring if no backup files ====== + rmf_task::BackupFileManager null_restore(backup_root_dir); + + auto null_robot_restore = + null_restore.make_group("group")->make_robot("robot"); + const auto null_restored_state = null_robot_restore->read(); + REQUIRE(!null_restored_state.has_value()); + using namespace std::chrono_literals; auto request = rmf_task::requests::Delivery::make( 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); @@ -42,8 +150,8 @@ SCENARIO("Back up to file") nullptr, nullptr, *request, - [&phase_snapshot](auto s){ phase_snapshot = s; }, - [robot_backup](auto b){ robot_backup->write(b); }, + [&phase_snapshot](auto s) { phase_snapshot = s; }, + [robot_backup](auto b) { robot_backup->write(b); }, [](auto) {}, []() {}); @@ -51,12 +159,9 @@ SCENARIO("Back up to file") auto mock_active_task = std::dynamic_pointer_cast(active_task); - REQUIRE(mock_active_task); - // ====== Advance the robot forward through some phases ====== - CHECK(phase_snapshot == nullptr); - mock_active_task->start_next_phase(rmf_traffic::Time()); + //// ====== Advance the robot forward through some phases ====== CHECK(phase_snapshot == nullptr); mock_active_task->_active_phase->send_update(); @@ -73,7 +178,7 @@ SCENARIO("Back up to file") // ====== Backup the task ====== mock_active_task->issue_backup(); - // ====== Restore the task ====== + //// ====== Restore the task ====== rmf_task::BackupFileManager restore(backup_root_dir); auto robot_restore = restore.make_group("group")->make_robot("robot"); @@ -86,14 +191,14 @@ SCENARIO("Back up to file") nullptr, *request, restored_state.value(), - [&restored_snapshot](auto s){ restored_snapshot = s; }, - [robot_restore](auto b){ robot_restore->write(b); }, - [](auto){}, - [](){}); + [&restored_snapshot](auto s) { restored_snapshot = s; }, + [robot_restore](auto b) { robot_restore->write(b); }, + [](auto) {}, + []() {}); auto mock_restored_task = std::dynamic_pointer_cast( - restored_task); + restored_task); REQUIRE(mock_restored_task); REQUIRE(mock_restored_task->_restored_state.has_value()); @@ -101,6 +206,7 @@ SCENARIO("Back up to file") // When the task is restored, it should resume where mock_active_task left off // and it should issue a phase snapshot to reflect that + mock_restored_task->_active_phase->send_update(); REQUIRE(restored_snapshot); CHECK(restored_snapshot->tag()->id() == phase_snapshot->tag()->id()); CHECK(restored_snapshot->tag()->name() == phase_snapshot->tag()->name()); diff --git a/rmf_task/test/mock/MockCondition.cpp b/rmf_task/test/mock/MockCondition.cpp index 85166d76..ba8428e0 100644 --- a/rmf_task/test/mock/MockCondition.cpp +++ b/rmf_task/test/mock/MockCondition.cpp @@ -24,9 +24,9 @@ MockCondition::MockCondition( std::string name_, std::string detail_, Status initial_status) - : _status(initial_status), - _name(std::move(name_)), - _detail(std::move(detail_)) +: _status(initial_status), + _name(std::move(name_)), + _detail(std::move(detail_)) { // Do nothing } diff --git a/rmf_task/test/mock/MockDelivery.cpp b/rmf_task/test/mock/MockDelivery.cpp index 3c5806ca..6248b1b4 100644 --- a/rmf_task/test/mock/MockDelivery.cpp +++ b/rmf_task/test/mock/MockDelivery.cpp @@ -16,6 +16,7 @@ */ #include "MockDelivery.hpp" +#include namespace test_rmf_task { @@ -49,7 +50,9 @@ auto MockDelivery::make_activator() -> Activator auto MockDelivery::Active::backup() const -> Backup { - return Backup::make(_backup_seq++, "Hello, I am a backup"); + std::stringstream ss; + ss << this->active_phase()->tag()->id(); + return Backup::make(_backup_seq++, ss.str()); } } // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockDelivery.hpp b/rmf_task/test/mock/MockDelivery.hpp index 2e4512eb..8fd66ea0 100644 --- a/rmf_task/test/mock/MockDelivery.hpp +++ b/rmf_task/test/mock/MockDelivery.hpp @@ -45,7 +45,7 @@ class MockDelivery : public rmf_task::Task Active( const Description& desc, std::optional backup, - Args&&... args) + Args&& ... args) : MockTask::Active(std::forward(args)...), _description(desc), _restored_state(std::move(backup)) @@ -53,13 +53,28 @@ class MockDelivery : public rmf_task::Task // TODO(MXG): We could use the description, state, and parameters to // get the actual estimate for the pending phases using namespace std::chrono_literals; - add_pending_phase("Go to pick up", "Pretending to go to a pick up point", 1min); + add_pending_phase("Go to pick up", "Pretending to go to a pick up point", + 1min); add_pending_phase("Pick up", "Pretending to pick something up", 30s); - add_pending_phase("Go to drop off", "Pretending to go to a drop off point", 1min); + add_pending_phase("Go to drop off", + "Pretending to go to a drop off point", 1min); add_pending_phase("Drop off", "Pretending to drop something off", 30s); - // TODO(MXG): We could use the _restored_state here to fast-forward this - // mock task to the phase that the previous Task::Active left off from. + if (_restored_state.has_value()) + { + uint64_t phase_id = std::stoul(_restored_state.value()); + const uint64_t min_phase_id = 0; + const uint64_t max_phase_id = pending_phases().size(); + if (phase_id < min_phase_id && phase_id > max_phase_id) + { + throw std::runtime_error("[MockDelivery::Active] phase_id given was " + std::to_string( + phase_id) + " not in range of " + std::to_string( + min_phase_id) + "," + std::to_string(max_phase_id) + "."); + } + _pending_phases.erase(_pending_phases.begin(), + _pending_phases.begin()+phase_id); + } + start_next_phase(rmf_traffic::Time()); } Backup backup() const override; diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index 8b443a69..6be6883d 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -25,12 +25,12 @@ MockPhase::Active::Active( ConstTagPtr tag_, std::function update_, std::function phase_finished_) - : _tag(std::move(tag_)), - _condition(std::make_shared( +: _tag(std::move(tag_)), + _condition(std::make_shared( "Mock condition", "This is a mocked up condition")), - _start_time(start_time_), - _update(std::move(update_)), - _phase_finished(std::move(phase_finished_)) + _start_time(start_time_), + _update(std::move(update_)), + _phase_finished(std::move(phase_finished_)) { // Do nothing } diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index 2d748663..6b186e00 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -61,7 +61,7 @@ auto MockTask::Active::interrupt(std::function task_is_interrupted) -> Resume { task_is_interrupted(); - return make_resumer([](){}); + return make_resumer([]() {}); } //============================================================================== @@ -100,7 +100,7 @@ MockTask::Active::Active( std::function phase_finished, std::function task_finished) : _tag(std::make_shared( - booking, "Mock Task", "Mocked up task", rmf_traffic::Time())), + booking, "Mock Task", "Mocked up task", rmf_traffic::Time())), _ctag(_tag), _update(std::move(update)), _checkpoint(std::move(checkpoint)), @@ -129,7 +129,7 @@ void MockTask::Active::start_next_phase(rmf_traffic::Time current_time) _phase_finished( std::make_shared( _active_phase->tag(), - _active_phase->finish_condition()->log(), + rmf_task::Phase::Snapshot::make(*_active_phase), _active_phase->_start_time, current_time)); } @@ -142,8 +142,8 @@ void MockTask::Active::start_next_phase(rmf_traffic::Time current_time) _active_phase = std::make_shared( current_time, next_phase.tag(), - [update = _update](Phase::ConstSnapshotPtr u){ update(std::move(u)); }, - [](){}); + [update = _update](Phase::ConstSnapshotPtr u) { update(std::move(u)); }, + []() {}); } //============================================================================== diff --git a/rmf_task/test/unit/test_Activator.cpp b/rmf_task/test/unit/test_Activator.cpp index f0e17d86..217f5c5e 100644 --- a/rmf_task/test/unit/test_Activator.cpp +++ b/rmf_task/test/unit/test_Activator.cpp @@ -34,8 +34,8 @@ SCENARIO("Activate fresh task") nullptr, nullptr, *request, - [&phase_snapshot](auto s){ phase_snapshot = s; }, - [&backup](auto b){ backup = b; }, + [&phase_snapshot](auto s) { phase_snapshot = s; }, + [&backup](auto b) { backup = b; }, [](auto) {}, []() {}); @@ -46,8 +46,6 @@ SCENARIO("Activate fresh task") REQUIRE(mock_active); - CHECK(phase_snapshot == nullptr); - mock_active->start_next_phase(rmf_traffic::Time()); CHECK(phase_snapshot == nullptr); mock_active->_active_phase->send_update(); @@ -66,17 +64,17 @@ SCENARIO("Activate fresh task") mock_active->issue_backup(); REQUIRE(backup.has_value()); CHECK(backup->sequence() == 0); - CHECK(backup->state() == "Hello, I am a backup"); + CHECK(backup->state() == "1"); auto restored = activator.restore( nullptr, nullptr, *request, backup->state(), - [&phase_snapshot](auto s){ phase_snapshot = s; }, - [&backup](auto b){ backup = b; }, - [](auto){}, - [](){}); + [&phase_snapshot](auto s) { phase_snapshot = s; }, + [&backup](auto b) { backup = b; }, + [](auto) {}, + []() {}); auto mock_restored = std::dynamic_pointer_cast(restored); diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index a0863381..d9526d38 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -29,7 +29,8 @@ class Phase::Activator::Implementation }; //============================================================================== -Phase::ActivePtr Phase::Activator::activate(std::function get_state, +Phase::ActivePtr Phase::Activator::activate( + std::function get_state, ConstTagPtr tag, const Description& description, std::optional backup_state, diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 82e017b2..fbd9a467 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -300,7 +300,7 @@ void Task::Active::_load_backup(std::string backup_state_str) { restore_phase_log.error( "Error in state data while trying to restore task from backup: " + - result->message); + result->message + "\n - Original message:" + get_state_text()); return failed_to_restore(); } From a7b3bc43b26dab9c6fe0a9eec380546f6b938290 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 14 Oct 2021 22:18:32 +0800 Subject: [PATCH 41/85] Rename Condition to Event Signed-off-by: Michael X. Grey --- .../rmf_task/{Condition.hpp => Event.hpp} | 68 +++++++++---------- rmf_task/include/rmf_task/Phase.hpp | 8 +-- .../include/rmf_task/phases/RestoreBackup.hpp | 2 +- rmf_task/src/rmf_task/Condition.cpp | 34 +++++----- rmf_task/src/rmf_task/Phase.cpp | 8 +-- .../mock/{MockCondition.cpp => MockEvent.cpp} | 18 ++--- .../mock/{MockCondition.hpp => MockEvent.hpp} | 14 ++-- rmf_task/test/mock/MockPhase.cpp | 6 +- rmf_task/test/mock/MockPhase.hpp | 6 +- 9 files changed, 82 insertions(+), 82 deletions(-) rename rmf_task/include/rmf_task/{Condition.hpp => Event.hpp} (55%) rename rmf_task/test/mock/{MockCondition.cpp => MockEvent.cpp} (77%) rename rmf_task/test/mock/{MockCondition.hpp => MockEvent.hpp} (79%) diff --git a/rmf_task/include/rmf_task/Condition.hpp b/rmf_task/include/rmf_task/Event.hpp similarity index 55% rename from rmf_task/include/rmf_task/Condition.hpp rename to rmf_task/include/rmf_task/Event.hpp index 19ba623b..c67b1cc5 100644 --- a/rmf_task/include/rmf_task/Condition.hpp +++ b/rmf_task/include/rmf_task/Event.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__EXECUTE__CONDITION_HPP -#define RMF_TASK__EXECUTE__CONDITION_HPP +#ifndef RMF_TASK__EVENT_HPP +#define RMF_TASK__EVENT_HPP #include @@ -29,35 +29,35 @@ namespace rmf_task { -class Condition; -using ConstConditionPtr = std::shared_ptr; +class Event; +using ConstEventPtr = std::shared_ptr; //============================================================================== -class Condition +class Event { public: /// A simple computer-friendly indicator of the current status of this - /// condition. This enum may be used to automatically identify when a - /// condition requires special attention, e.g. logging a warning or alerting + /// event. This enum may be used to automatically identify when an + /// event requires special attention, e.g. logging a warning or alerting /// an operator. enum class Status : uint32_t { - /// The condition status has not been initialized. This is a sentinel value + /// The event status has not been initialized. This is a sentinel value /// that should not generally be used. Uninitialized = 0, - /// The condition is on standby. It cannot be satisfied yet, and that is its + /// The event is on standby. It cannot be started yet, and that is its /// expected status. Standby, - /// The condition is underway, and coming along as expected. + /// The event is underway, and proceeding as expected. Underway, - /// The condition is underway but it has been temporarily delayed. + /// The event is underway but it has been temporarily delayed. Delayed, - /// The condition is underway but it has been blocked. The blockage may + /// The event is underway but it has been blocked. The blockage may /// require manual intervention to fix. Blocked, @@ -65,60 +65,60 @@ class Condition /// deal with. Manual intervention is needed to get the task back on track. Error, - /// An operator has instructed this condition to be skipped + /// An operator has instructed this event to be skipped Skipped, - /// An operator has instructed this condition to be canceled + /// An operator has instructed this event to be canceled Canceled, - /// An operator has instructed this condition to be killed + /// An operator has instructed this event to be killed Killed, - /// The condition cannot ever be satisfied, even with manual intervention. + /// The event cannot ever finish correctly, even with manual intervention. /// This may mean that the Task cannot be completed if it does not have /// an automated way to recover from this failure state. Failed, - /// The condition is satisfied. + /// The event is finished. Finished }; - /// The current Status of this condition + /// The current Status of this event virtual Status status() const = 0; - /// Simple wrapper for identifying when a condition is finished + /// Simple wrapper for identifying when an event is finished inline bool finished() const { return status() == Status::Finished; } - /// The "name" of this condition. Ideally a short, simple piece of text that - /// helps a human being intuit what this condition is expecting at a glance. + /// The "name" of this event. Ideally a short, simple piece of text that + /// helps a human being intuit what this event is expecting at a glance. virtual std::string name() const = 0; - /// A detailed explanation of this condition. + /// A detailed explanation of this event. virtual std::string detail() const = 0; - /// A view of the event log for this condition. + /// A view of the log for this event. virtual Log::View log() const = 0; - /// Get more granular subconditions of this condition, if any exist. - virtual std::vector subconditions() const = 0; + /// Get more granular dependencies of this event, if any exist. + virtual std::vector dependencies() const = 0; class Snapshot; // Virtual destructor - virtual ~Condition() = default; + virtual ~Event() = default; }; //============================================================================== -/// A snapshot of the state of a condition. This snapshot can be read while the -/// original condition is arbitrarily changed, and there is no risk of a race -/// condition, as long as the snapshot is not being created while the condition +/// A snapshot of the state of an event. This snapshot can be read while the +/// original event is arbitrarily changed, and there is no risk of a race +/// condition, as long as the snapshot is not being created while the event /// is changing. -class Condition::Snapshot : public Condition +class Event::Snapshot : public Event { public: - /// Make a snapshot of the current state of a Condition - static ConstConditionPtr make(const Condition& other); + /// Make a snapshot of the current state of an Event + static ConstEventPtr make(const Event& other); // Documentation inherited Status status() const final; @@ -133,7 +133,7 @@ class Condition::Snapshot : public Condition Log::View log() const final; // Documentation inherited - std::vector subconditions() const final; + std::vector dependencies() const final; class Implementation; private: @@ -143,4 +143,4 @@ class Condition::Snapshot : public Condition } // namespace rmf_task -#endif // RMF_TASK__EXECUTE__CONDITION_HPP +#endif // RMF_TASK__EVENT_HPP diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 6693e6dd..af1ef3fb 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -18,7 +18,7 @@ #ifndef RMF_TASK__EXECUTE__PHASE_HPP #define RMF_TASK__EXECUTE__PHASE_HPP -#include +#include #include @@ -134,8 +134,8 @@ class Phase::Active /// Tag of the phase virtual ConstTagPtr tag() const = 0; - /// The condition that needs to be satisfied for this phase to be complete - virtual ConstConditionPtr finish_condition() const = 0; + /// The Event that needs to finish for this phase to be complete + virtual ConstEventPtr finish_event() const = 0; /// The estimated finish time for this phase virtual rmf_traffic::Time estimate_finish_time() const = 0; @@ -156,7 +156,7 @@ class Phase::Snapshot : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - ConstConditionPtr finish_condition() const final; + ConstEventPtr finish_event() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index 5494c86e..db50efb2 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -47,7 +47,7 @@ class RestoreBackup::Active : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - ConstConditionPtr finish_condition() const final; + ConstEventPtr finish_event() const final; class Implementation; private: diff --git a/rmf_task/src/rmf_task/Condition.cpp b/rmf_task/src/rmf_task/Condition.cpp index 6f9ee122..92922439 100644 --- a/rmf_task/src/rmf_task/Condition.cpp +++ b/rmf_task/src/rmf_task/Condition.cpp @@ -15,30 +15,30 @@ * */ -#include +#include namespace rmf_task { namespace { //============================================================================== -std::vector snapshot_conditions( - const std::vector& queue) +std::vector snapshot_dependencies( + const std::vector& queue) { // NOTE(MXG): This implementation is using recursion. That should be fine - // since I don't expect much depth in the trees of subconditions, but we may + // since I don't expect much depth in the trees of dependencies, but we may // want to revisit this and implement it as a queue instead if we ever find // a use-case with deep recursion. - std::vector output; + std::vector output; output.reserve(queue.size()); for (const auto& c : queue) - output.push_back(Condition::Snapshot::make(*c)); + output.push_back(Event::Snapshot::make(*c)); return output; } } // anonymous namespace //============================================================================== -class Condition::Snapshot::Implementation +class Event::Snapshot::Implementation { public: @@ -46,12 +46,12 @@ class Condition::Snapshot::Implementation std::string name; std::string detail; Log::View log; - std::vector subconditions; + std::vector dependencies; }; //============================================================================== -ConstConditionPtr Condition::Snapshot::make(const Condition& other) +ConstEventPtr Event::Snapshot::make(const Event& other) { Snapshot output; output._pimpl = rmf_utils::make_impl( @@ -60,44 +60,44 @@ ConstConditionPtr Condition::Snapshot::make(const Condition& other) other.name(), other.detail(), other.log(), - snapshot_conditions(other.subconditions()) + snapshot_dependencies(other.dependencies()) }); return std::make_shared(std::move(output)); } //============================================================================== -auto Condition::Snapshot::status() const -> Status +auto Event::Snapshot::status() const -> Status { return _pimpl->status; } //============================================================================== -std::string Condition::Snapshot::name() const +std::string Event::Snapshot::name() const { return _pimpl->name; } //============================================================================== -std::string Condition::Snapshot::detail() const +std::string Event::Snapshot::detail() const { return _pimpl->detail; } //============================================================================== -Log::View Condition::Snapshot::log() const +Log::View Event::Snapshot::log() const { return _pimpl->log; } //============================================================================== -std::vector Condition::Snapshot::subconditions() const +std::vector Event::Snapshot::dependencies() const { - return _pimpl->subconditions; + return _pimpl->dependencies; } //============================================================================== -Condition::Snapshot::Snapshot() +Event::Snapshot::Snapshot() { // Do nothing } diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 66ddc20f..ef612f8b 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -129,7 +129,7 @@ class Phase::Snapshot::Implementation { public: ConstTagPtr tag; - ConstConditionPtr finish_condition; + ConstEventPtr finish_event; rmf_traffic::Time estimated_finish_time; }; @@ -140,7 +140,7 @@ Phase::ConstSnapshotPtr Phase::Snapshot::make(const Active& active) output._pimpl = rmf_utils::make_impl( Implementation{ active.tag(), - Condition::Snapshot::make(*active.finish_condition()), + Event::Snapshot::make(*active.finish_event()), active.estimate_finish_time() }); @@ -154,9 +154,9 @@ Phase::ConstTagPtr Phase::Snapshot::tag() const } //============================================================================== -ConstConditionPtr Phase::Snapshot::finish_condition() const +ConstEventPtr Phase::Snapshot::finish_event() const { - return _pimpl->finish_condition; + return _pimpl->finish_event; } //============================================================================== diff --git a/rmf_task/test/mock/MockCondition.cpp b/rmf_task/test/mock/MockEvent.cpp similarity index 77% rename from rmf_task/test/mock/MockCondition.cpp rename to rmf_task/test/mock/MockEvent.cpp index ba8428e0..8a41c980 100644 --- a/rmf_task/test/mock/MockCondition.cpp +++ b/rmf_task/test/mock/MockEvent.cpp @@ -15,12 +15,12 @@ * */ -#include "MockCondition.hpp" +#include "MockEvent.hpp" namespace test_rmf_task { //============================================================================== -MockCondition::MockCondition( +MockEvent::MockEvent( std::string name_, std::string detail_, Status initial_status) @@ -32,34 +32,34 @@ MockCondition::MockCondition( } //============================================================================== -auto MockCondition::status() const -> Status +auto MockEvent::status() const -> Status { return _status; } //============================================================================== -std::string MockCondition::name() const +std::string MockEvent::name() const { return _name; } //============================================================================== -std::string MockCondition::detail() const +std::string MockEvent::detail() const { return _detail; } //============================================================================== -rmf_task::Log::View MockCondition::log() const +rmf_task::Log::View MockEvent::log() const { return _log.view(); } //============================================================================== -std::vector MockCondition::subconditions() const +std::vector MockEvent::dependencies() const { - return std::vector( - _subconditions.begin(), _subconditions.end()); + return std::vector( + _dependencies.begin(), _dependencies.end()); } } // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockCondition.hpp b/rmf_task/test/mock/MockEvent.hpp similarity index 79% rename from rmf_task/test/mock/MockCondition.hpp rename to rmf_task/test/mock/MockEvent.hpp index b3f8ec7e..49364971 100644 --- a/rmf_task/test/mock/MockCondition.hpp +++ b/rmf_task/test/mock/MockEvent.hpp @@ -15,19 +15,19 @@ * */ -#ifndef TEST__MOCK__MOCKCONDITION_HPP -#define TEST__MOCK__MOCKCONDITION_HPP +#ifndef TEST__MOCK__MOCKEVENT_HPP +#define TEST__MOCK__MOCKEVENT_HPP -#include +#include namespace test_rmf_task { //============================================================================== -class MockCondition : public rmf_task::Condition +class MockEvent : public rmf_task::Event { public: - MockCondition( + MockEvent( std::string name_, std::string detail_, Status initial_status = Status::Standby); @@ -37,14 +37,14 @@ class MockCondition : public rmf_task::Condition std::string name() const final; std::string detail() const final; rmf_task::Log::View log() const final; - std::vector subconditions() const final; + std::vector dependencies() const final; // Fields Status _status; std::string _name; std::string _detail; rmf_task::Log _log; - std::vector> _subconditions; + std::vector> _dependencies; }; diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index 6be6883d..ac5ecc13 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -26,7 +26,7 @@ MockPhase::Active::Active( std::function update_, std::function phase_finished_) : _tag(std::move(tag_)), - _condition(std::make_shared( + _event(std::make_shared( "Mock condition", "This is a mocked up condition")), _start_time(start_time_), _update(std::move(update_)), @@ -42,9 +42,9 @@ rmf_task::Phase::ConstTagPtr MockPhase::Active::tag() const } //============================================================================== -rmf_task::ConstConditionPtr MockPhase::Active::finish_condition() const +rmf_task::ConstEventPtr MockPhase::Active::finish_event() const { - return _condition; + return _event; } //============================================================================== diff --git a/rmf_task/test/mock/MockPhase.hpp b/rmf_task/test/mock/MockPhase.hpp index 9e6051e6..f5fcc306 100644 --- a/rmf_task/test/mock/MockPhase.hpp +++ b/rmf_task/test/mock/MockPhase.hpp @@ -20,7 +20,7 @@ #include -#include "MockCondition.hpp" +#include "MockEvent.hpp" namespace test_rmf_task { @@ -41,13 +41,13 @@ class MockPhase : public rmf_task::Phase std::function phase_finished_); ConstTagPtr tag() const final; - rmf_task::ConstConditionPtr finish_condition() const final; + rmf_task::ConstEventPtr finish_event() const final; rmf_traffic::Time estimate_finish_time() const final; void send_update() const; ConstTagPtr _tag; - std::shared_ptr _condition; + std::shared_ptr _event; rmf_traffic::Time _start_time; std::function _update; std::function _phase_finished; From 7aee6bcd462d791b8fe326351aa437abe65d8823 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 15 Oct 2021 15:52:39 +0800 Subject: [PATCH 42/85] Introducing VersionedString and using it for event names + details Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Event.hpp | 11 +- rmf_task/include/rmf_task/VersionedString.hpp | 95 ++++++++++ .../include/rmf_task/events/SimpleEvent.hpp | 47 +++++ .../include/rmf_task/phases/RestoreBackup.hpp | 8 +- rmf_task/src/rmf_task/Condition.cpp | 8 +- rmf_task/src/rmf_task/Log.cpp | 12 +- rmf_task/src/rmf_task/VersionedString.cpp | 172 ++++++++++++++++++ rmf_task/test/mock/MockEvent.cpp | 8 +- rmf_task/test/mock/MockEvent.hpp | 8 +- .../src/rmf_task_sequence/Task.cpp | 2 +- 10 files changed, 350 insertions(+), 21 deletions(-) create mode 100644 rmf_task/include/rmf_task/VersionedString.hpp create mode 100644 rmf_task/include/rmf_task/events/SimpleEvent.hpp create mode 100644 rmf_task/src/rmf_task/VersionedString.cpp diff --git a/rmf_task/include/rmf_task/Event.hpp b/rmf_task/include/rmf_task/Event.hpp index c67b1cc5..2d89b484 100644 --- a/rmf_task/include/rmf_task/Event.hpp +++ b/rmf_task/include/rmf_task/Event.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__EVENT_HPP #include +#include #include @@ -37,6 +38,8 @@ class Event { public: + using Id = uint64_t; + /// A simple computer-friendly indicator of the current status of this /// event. This enum may be used to automatically identify when an /// event requires special attention, e.g. logging a warning or alerting @@ -91,10 +94,10 @@ class Event /// The "name" of this event. Ideally a short, simple piece of text that /// helps a human being intuit what this event is expecting at a glance. - virtual std::string name() const = 0; + virtual VersionedString::View name() const = 0; /// A detailed explanation of this event. - virtual std::string detail() const = 0; + virtual VersionedString::View detail() const = 0; /// A view of the log for this event. virtual Log::View log() const = 0; @@ -124,10 +127,10 @@ class Event::Snapshot : public Event Status status() const final; // Documentation inherited - std::string name() const final; + VersionedString::View name() const final; // Documentation inherited - std::string detail() const final; + VersionedString::View detail() const final; // Documentation inherited Log::View log() const final; diff --git a/rmf_task/include/rmf_task/VersionedString.hpp b/rmf_task/include/rmf_task/VersionedString.hpp new file mode 100644 index 00000000..10d9b1a1 --- /dev/null +++ b/rmf_task/include/rmf_task/VersionedString.hpp @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__VERSIONEDSTRING_HPP +#define RMF_TASK__VERSIONEDSTRING_HPP + +#include + +namespace rmf_task { + +//============================================================================== +class VersionedString +{ +public: + + class View; + class Reader; + + /// Construct a versioned string + /// + /// \param[in] initial_value + /// The initial value of this versioned string + VersionedString(std::string initial_value); + + /// Update the value of this versioned string + /// + /// \param[in] new_value + /// The new value for this versioned string + void update(std::string new_value); + + /// Get a view of the current version of the string + View view() const; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +//============================================================================== +/// A snapshot view of a VersionedString. This is thread-safe to read even while +/// the VersionedString is being modified. Each VersionedString::Reader instance +/// will only view this object once; after the first viewing it will return a +/// nullptr. +/// +/// The contents of this View can only be retrieved by a VersionedString::Reader +class VersionedString::View +{ +public: + class Implementation; +private: + View(); + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class VersionedString::Reader +{ +public: + + /// Construct a Reader + Reader(); + + /// Read from the View. + /// + /// If this Reader has never seen this View before, then this function will + /// return a reference to the string that the View contains. Otherwise, if + /// this Reader has seen this View before, then this function will return a + /// nullptr. + /// + /// \param[in] view + /// The view that the Reader should look at + std::shared_ptr read(const View& view); + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__VERSIONEDSTRING_HPP diff --git a/rmf_task/include/rmf_task/events/SimpleEvent.hpp b/rmf_task/include/rmf_task/events/SimpleEvent.hpp new file mode 100644 index 00000000..5bc43b79 --- /dev/null +++ b/rmf_task/include/rmf_task/events/SimpleEvent.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__EVENTS__SIMPLEEVENT_HPP +#define RMF_TASK__EVENTS__SIMPLEEVENT_HPP + +#include + +namespace rmf_task { +namespace events { + +//============================================================================== +class SimpleEvent : public Event +{ +public: + + // Documentation inherited + Status status() const final; + + /// Update the status of this event + SimpleEvent& update_status(Status new_status); + + // Documentation inherited + + + + +}; + +} // namespace events +} // namespace rmf_task + +#endif // RMF_TASK__EVENTS__SIMPLEEVENT_HPP diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index db50efb2..fb3f6c9c 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -31,8 +31,6 @@ class RestoreBackup : public Phase class Active; using ActivePtr = std::shared_ptr; - class Condition; - using ConditionPtr = std::shared_ptr; }; //============================================================================== @@ -49,6 +47,12 @@ class RestoreBackup::Active : public Phase::Active // Documentation inherited ConstEventPtr finish_event() const final; + /// Call this function if the parsing fails + void parsing_failed(std::string error_message); + + /// Call this function if the parsing succeeds + void parsing_succeeded(); + class Implementation; private: Active(); diff --git a/rmf_task/src/rmf_task/Condition.cpp b/rmf_task/src/rmf_task/Condition.cpp index 92922439..0b6bb556 100644 --- a/rmf_task/src/rmf_task/Condition.cpp +++ b/rmf_task/src/rmf_task/Condition.cpp @@ -43,8 +43,8 @@ class Event::Snapshot::Implementation public: Status status; - std::string name; - std::string detail; + VersionedString::View name; + VersionedString::View detail; Log::View log; std::vector dependencies; @@ -73,13 +73,13 @@ auto Event::Snapshot::status() const -> Status } //============================================================================== -std::string Event::Snapshot::name() const +VersionedString::View Event::Snapshot::name() const { return _pimpl->name; } //============================================================================== -std::string Event::Snapshot::detail() const +VersionedString::View Event::Snapshot::detail() const { return _pimpl->detail; } diff --git a/rmf_task/src/rmf_task/Log.cpp b/rmf_task/src/rmf_task/Log.cpp index a6d69619..a40b067e 100644 --- a/rmf_task/src/rmf_task/Log.cpp +++ b/rmf_task/src/rmf_task/Log.cpp @@ -128,7 +128,7 @@ class Log::Reader::Implementation std::unordered_map memories; - Iterable iterate(const View& view); + Iterable read(const View& view); }; //============================================================================== @@ -191,7 +191,7 @@ Log::Reader::Iterable Log::Reader::Iterable::Implementation::make( } //============================================================================== -auto Log::Reader::Implementation::iterate(const View& view) -> Iterable +auto Log::Reader::Implementation::read(const View& view) -> Iterable { const auto& v = View::Implementation::get(view); const auto it = memories.insert({v.shared.get(), Memory()}).first; @@ -200,6 +200,8 @@ auto Log::Reader::Implementation::iterate(const View& view) -> Iterable { if (!memory.last.has_value()) memory.last = v.begin; + else + ++(*memory.last); } else { @@ -296,6 +298,12 @@ Log::Reader::Reader() // Do nothing } +//============================================================================== +auto Log::Reader::read(const View& view) -> Iterable +{ + return _pimpl->read(view); +} + //============================================================================== auto Log::Reader::Iterable::begin() const -> iterator { diff --git a/rmf_task/src/rmf_task/VersionedString.cpp b/rmf_task/src/rmf_task/VersionedString.cpp new file mode 100644 index 00000000..028c402a --- /dev/null +++ b/rmf_task/src/rmf_task/VersionedString.cpp @@ -0,0 +1,172 @@ +/* + * Copyright (C) 2021 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 + +#include + +namespace rmf_task { + +//============================================================================== +class VersionedString::Implementation +{ +public: + + Implementation(std::string initial_value) + : value(std::make_shared(std::move(initial_value))) + { + // Do nothing + } + + using ValuePtr = std::shared_ptr; + ValuePtr value; + + // The token is used to uniquely identify this VersionedString. + struct Token { }; + using TokenPtr = std::shared_ptr; + TokenPtr token = std::make_shared(); + + View make_view() const; +}; + +//============================================================================== +class VersionedString::View::Implementation +{ +public: + + using ValuePtr = VersionedString::Implementation::ValuePtr; + using TokenPtr = VersionedString::Implementation::TokenPtr; + + static View make(ValuePtr value, TokenPtr token) + { + View output; + output._pimpl = rmf_utils::make_impl( + Implementation{ + std::move(value), + std::move(token) + }); + + return output; + } + + static const Implementation& get(const View& view) + { + return *view._pimpl; + } + + ValuePtr value; + TokenPtr token; + +}; + +//============================================================================== +class VersionedString::Reader::Implementation +{ +public: + + using ValuePtr = VersionedString::Implementation::ValuePtr; + using WeakValuePtr = ValuePtr::weak_type; + + using Token = VersionedString::Implementation::Token; + using TokenPtr = VersionedString::Implementation::TokenPtr; + using WeakTokenPtr = TokenPtr::weak_type; + + struct Memory + { + WeakValuePtr last_value; + WeakTokenPtr token; + + Memory() + { + // Do nothing + } + }; + + std::unordered_map memories; + + ValuePtr read(const View& view) + { + const auto& v = View::Implementation::get(view); + const auto it = memories.insert({v.token.get(), Memory()}).first; + auto& memory = it->second; + if (memory.token.lock()) + { + // If we can successfully lock the memory token, then we do remember this + // same versioned string. + if (const auto last_value = memory.last_value.lock()) + { + if (last_value == v.value) + { + // If our memory of the last_value is still valid and matches with the + // value that we are receiving now, then this value is a duplicate. + return nullptr; + } + } + } + + memory.token = v.token; + memory.last_value = v.value; + return v.value; + } +}; + +//============================================================================== +auto VersionedString::Implementation::make_view() const -> View +{ + return VersionedString::View::Implementation::make(value, token); +} + +//============================================================================== +VersionedString::VersionedString(std::string initial_value) +: _pimpl(rmf_utils::make_unique_impl(std::move(initial_value))) +{ + // Do nothing +} + +//============================================================================== +void VersionedString::update(std::string new_value) +{ + _pimpl->value = std::make_shared(std::move(new_value)); +} + +//============================================================================== +auto VersionedString::view() const -> View +{ + return _pimpl->make_view(); +} + +//============================================================================== +VersionedString::View::View() +{ + // Do nothing +} + +//============================================================================== +VersionedString::Reader::Reader() +: _pimpl(rmf_utils::make_unique_impl()) +{ + // Do nothing +} + +//============================================================================== +std::shared_ptr VersionedString::Reader::read( + const View& view) +{ + return _pimpl->read(view); +} + +} // namespace rmf_task diff --git a/rmf_task/test/mock/MockEvent.cpp b/rmf_task/test/mock/MockEvent.cpp index 8a41c980..a5594497 100644 --- a/rmf_task/test/mock/MockEvent.cpp +++ b/rmf_task/test/mock/MockEvent.cpp @@ -38,15 +38,15 @@ auto MockEvent::status() const -> Status } //============================================================================== -std::string MockEvent::name() const +rmf_task::VersionedString::View MockEvent::name() const { - return _name; + return _name.view(); } //============================================================================== -std::string MockEvent::detail() const +rmf_task::VersionedString::View MockEvent::detail() const { - return _detail; + return _detail.view(); } //============================================================================== diff --git a/rmf_task/test/mock/MockEvent.hpp b/rmf_task/test/mock/MockEvent.hpp index 49364971..10af728f 100644 --- a/rmf_task/test/mock/MockEvent.hpp +++ b/rmf_task/test/mock/MockEvent.hpp @@ -34,15 +34,15 @@ class MockEvent : public rmf_task::Event // Interface Status status() const final; - std::string name() const final; - std::string detail() const final; + rmf_task::VersionedString::View name() const final; + rmf_task::VersionedString::View detail() const final; rmf_task::Log::View log() const final; std::vector dependencies() const final; // Fields Status _status; - std::string _name; - std::string _detail; + rmf_task::VersionedString _name; + rmf_task::VersionedString _detail; rmf_task::Log _log; std::vector> _dependencies; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index fbd9a467..b10da18e 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -414,7 +414,7 @@ void Task::Active::_finish_phase() const auto completed_phase = std::make_shared( _active_phase->tag(), - _active_phase->finish_condition()->log(), + _active_phase->finish_event(), _current_phase_start_time.value(), phase_finish_time); From fbb2160acb64cf83b7a906c213ebf24978ac3025 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 15 Oct 2021 18:12:50 +0800 Subject: [PATCH 43/85] Implementing the SimpleEvent class Signed-off-by: Michael X. Grey --- rmf_task/CMakeLists.txt | 5 +- .../include/rmf_task/events/SimpleEvent.hpp | 37 +++++ rmf_task/src/rmf_task/events/SimpleEvent.cpp | 128 ++++++++++++++++++ 3 files changed, 166 insertions(+), 4 deletions(-) create mode 100644 rmf_task/src/rmf_task/events/SimpleEvent.cpp diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 9e132e3e..82976322 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -29,10 +29,7 @@ find_package(ament_cmake_catch2 QUIET) find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Tasks library -file(GLOB lib_srcs - "src/rmf_task/detail/*.cpp" - "src/rmf_task/requests/*.cpp" - "src/rmf_task/requests/factory/*.cpp" +file(GLOB_RECURSE lib_srcs "src/rmf_task/*.cpp" ) diff --git a/rmf_task/include/rmf_task/events/SimpleEvent.hpp b/rmf_task/include/rmf_task/events/SimpleEvent.hpp index 5bc43b79..8de144c4 100644 --- a/rmf_task/include/rmf_task/events/SimpleEvent.hpp +++ b/rmf_task/include/rmf_task/events/SimpleEvent.hpp @@ -24,10 +24,24 @@ namespace rmf_task { namespace events { //============================================================================== +/// This class is the simplest possible implementation for directly managing the +/// required fields of the Event interface. +/// +/// This may be useful if you have a Phase implementation that takes care of the +/// logic for tracking your event(s) but you still need an Event object to +/// satisfy the Phase interface's finish_event() function. Your Phase +/// implementation can create an instance of this class and then manage its +/// fields directly. class SimpleEvent : public Event { public: + static std::shared_ptr make( + std::string name, + std::string detail, + Status initial_status, + std::vector dependencies = {}); + // Documentation inherited Status status() const final; @@ -35,10 +49,33 @@ class SimpleEvent : public Event SimpleEvent& update_status(Status new_status); // Documentation inherited + VersionedString::View name() const final; + + /// Update the name of this event + SimpleEvent& update_name(std::string new_name); + + // Documentation inherited + VersionedString::View detail() const final; + /// Update the detail of this event + SimpleEvent& update_detail(std::string new_detail); + // Documentation inherited + Log::View log() const final; + + /// Update the log + Log& update_log(); + + // Documentation inherited + std::vector dependencies() const final; + /// Update the dependencies + SimpleEvent& update_dependencies(std::vector new_dependencies); + class Implementation; +private: + SimpleEvent(); + rmf_utils::unique_impl_ptr _pimpl; }; } // namespace events diff --git a/rmf_task/src/rmf_task/events/SimpleEvent.cpp b/rmf_task/src/rmf_task/events/SimpleEvent.cpp new file mode 100644 index 00000000..4cf6efc6 --- /dev/null +++ b/rmf_task/src/rmf_task/events/SimpleEvent.cpp @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { +namespace events { + +//============================================================================== +class SimpleEvent::Implementation +{ +public: + + Status status; + VersionedString name; + VersionedString detail; + Log log; + std::vector dependencies; + +}; + +//============================================================================== +std::shared_ptr SimpleEvent::make( + std::string name, + std::string detail, + Status initial_status, + std::vector dependencies) +{ + SimpleEvent output; + output._pimpl = rmf_utils::make_unique_impl( + Implementation{ + initial_status, + std::move(name), + std::move(detail), + Log(), + std::move(dependencies) + }); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +auto SimpleEvent::status() const -> Status +{ + return _pimpl->status; +} + +//============================================================================== +SimpleEvent& SimpleEvent::update_status(Status new_status) +{ + _pimpl->status = new_status; + return *this; +} + +//============================================================================== +VersionedString::View SimpleEvent::name() const +{ + return _pimpl->name.view(); +} + +//============================================================================== +SimpleEvent& SimpleEvent::update_name(std::string new_name) +{ + _pimpl->name.update(std::move(new_name)); + return *this; +} + +//============================================================================== +VersionedString::View SimpleEvent::detail() const +{ + return _pimpl->detail.view(); +} + +//============================================================================== +SimpleEvent& SimpleEvent::update_detail(std::string new_detail) +{ + _pimpl->detail.update(std::move(new_detail)); + return *this; +} + +//============================================================================== +Log::View SimpleEvent::log() const +{ + return _pimpl->log.view(); +} + +//============================================================================== +Log& SimpleEvent::update_log() +{ + return _pimpl->log; +} + +//============================================================================== +std::vector SimpleEvent::dependencies() const +{ + return _pimpl->dependencies; +} + +//============================================================================== +SimpleEvent& SimpleEvent::update_dependencies( + std::vector new_dependencies) +{ + _pimpl->dependencies = new_dependencies; + return *this; +} + +//============================================================================== +SimpleEvent::SimpleEvent() +{ + // Do nothing +} + +} // namespace events +} // namespace rmf_task From f1d3f6286aa8f8a6e56f6e750d4553c1303bbe0f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 18 Oct 2021 16:16:14 +0800 Subject: [PATCH 44/85] Migrate to using the RestoreBackup phase Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Phase.hpp | 8 +- .../include/rmf_task/phases/RestoreBackup.hpp | 23 +++- rmf_task/src/rmf_task/Phase.cpp | 13 +- .../src/rmf_task/phases/RestoreBackup.cpp | 121 ++++++++++++++++++ rmf_task/test/mock/MockPhase.cpp | 2 +- rmf_task/test/mock/MockPhase.hpp | 2 +- rmf_task/test/mock/MockTask.cpp | 1 - .../src/rmf_task_sequence/Task.cpp | 41 ++---- 8 files changed, 160 insertions(+), 51 deletions(-) create mode 100644 rmf_task/src/rmf_task/phases/RestoreBackup.cpp diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index af1ef3fb..74e21371 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -104,14 +104,10 @@ class Phase::Completed /// Constructor Completed( - ConstTagPtr tag_, ConstSnapshotPtr snapshot_, rmf_traffic::Time start_, rmf_traffic::Time finish_); - /// Tag of the phase - const ConstTagPtr& tag() const; - /// The final log of the phase const ConstSnapshotPtr& snapshot() const; @@ -135,7 +131,7 @@ class Phase::Active virtual ConstTagPtr tag() const = 0; /// The Event that needs to finish for this phase to be complete - virtual ConstEventPtr finish_event() const = 0; + virtual ConstEventPtr final_event() const = 0; /// The estimated finish time for this phase virtual rmf_traffic::Time estimate_finish_time() const = 0; @@ -156,7 +152,7 @@ class Phase::Snapshot : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - ConstEventPtr finish_event() const final; + ConstEventPtr final_event() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index fb3f6c9c..e8c46059 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -34,24 +34,39 @@ class RestoreBackup : public Phase }; //============================================================================== +/// This is a special phase type designated for restoring the backup of a task. +/// +/// This phase type uses a reserved phase ID of 0. class RestoreBackup::Active : public Phase::Active { public: /// Make an active RestoreBackup phase - static ActivePtr make(std::string backup_state_str); + static ActivePtr make( + const std::string& backup_state_str, + rmf_traffic::Time estimated_finish_time); // Documentation inherited ConstTagPtr tag() const final; // Documentation inherited - ConstEventPtr finish_event() const final; + ConstEventPtr final_event() const final; + + // Documentation inherited + rmf_traffic::Time estimate_finish_time() const final; /// Call this function if the parsing fails - void parsing_failed(std::string error_message); + void parsing_failed(const std::string& error_message); + + /// Call this function if the restoration fails for some reason besides + /// parsing + void restoration_failed(const std::string& error_message); /// Call this function if the parsing succeeds - void parsing_succeeded(); + void restoration_succeeded(); + + /// Get the log to pass in some other kind of message + Log& update_log(); class Implementation; private: diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index ef612f8b..7bf75ed7 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -77,7 +77,6 @@ class Phase::Completed::Implementation { public: - ConstTagPtr tag; ConstSnapshotPtr snapshot; rmf_traffic::Time start; rmf_traffic::Time finish; @@ -85,13 +84,11 @@ class Phase::Completed::Implementation //============================================================================== Phase::Completed::Completed( - ConstTagPtr tag_, ConstSnapshotPtr snapshot_, rmf_traffic::Time start_, rmf_traffic::Time finish_) : _pimpl(rmf_utils::make_impl( Implementation{ - std::move(tag_), std::move(snapshot_), start_, finish_ @@ -100,12 +97,6 @@ Phase::Completed::Completed( // Do nothing } -//============================================================================== -auto Phase::Completed::tag() const -> const ConstTagPtr& -{ - return _pimpl->tag; -} - //============================================================================== auto Phase::Completed::snapshot() const -> const ConstSnapshotPtr& { @@ -140,7 +131,7 @@ Phase::ConstSnapshotPtr Phase::Snapshot::make(const Active& active) output._pimpl = rmf_utils::make_impl( Implementation{ active.tag(), - Event::Snapshot::make(*active.finish_event()), + Event::Snapshot::make(*active.final_event()), active.estimate_finish_time() }); @@ -154,7 +145,7 @@ Phase::ConstTagPtr Phase::Snapshot::tag() const } //============================================================================== -ConstEventPtr Phase::Snapshot::finish_event() const +ConstEventPtr Phase::Snapshot::final_event() const { return _pimpl->finish_event; } diff --git a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp new file mode 100644 index 00000000..a56df548 --- /dev/null +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2021 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 +#include + +namespace rmf_task { +namespace phases { + +//============================================================================== +class RestoreBackup::Active::Implementation +{ +public: + + static ConstTagPtr make_tag() + { + return std::make_shared( + 0, + "Restore from backup", + "The task progress is being restored from a backed up state", + rmf_traffic::Duration(0)); + } + + Implementation( + const std::string& backup_state_str, + rmf_traffic::Time estimated_finish_time_) + : tag(make_tag()), + event(events::SimpleEvent::make( + tag->name(), tag->detail(), rmf_task::Event::Status::Underway)), + estimated_finish_time(estimated_finish_time_) + { + event->update_log().info( + "Parsing backup state:\n```\n" + backup_state_str + "\n```"); + } + + ConstTagPtr tag; + std::shared_ptr event; + rmf_traffic::Time estimated_finish_time; + +}; + +//============================================================================== +auto RestoreBackup::Active::make( + const std::string& backup_state_str, + rmf_traffic::Time estimated_finish_time) -> ActivePtr +{ + Active output; + output._pimpl = rmf_utils::make_unique_impl( + std::move(backup_state_str), estimated_finish_time); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +auto RestoreBackup::Active::tag() const -> ConstTagPtr +{ + return _pimpl->tag; +} + +//============================================================================== +auto RestoreBackup::Active::final_event() const -> ConstEventPtr +{ + return _pimpl->event; +} + +//============================================================================== +rmf_traffic::Time RestoreBackup::Active::estimate_finish_time() const +{ + return _pimpl->estimated_finish_time; +} + +//============================================================================== +void RestoreBackup::Active::parsing_failed(const std::string& error_message) +{ + _pimpl->event + ->update_status(Event::Status::Error) + .update_log().error("Parsing failed: " + error_message); +} + +//============================================================================== +void RestoreBackup::Active::restoration_failed(const std::string& error_message) +{ + _pimpl->event + ->update_status(Event::Status::Error) + .update_log().error("Restoration failed: " + error_message); +} + +//============================================================================== +void RestoreBackup::Active::restoration_succeeded() +{ + _pimpl->event->update_status(Event::Status::Finished); +} + +//============================================================================== +Log& RestoreBackup::Active::update_log() +{ + return _pimpl->event->update_log(); +} + +//============================================================================== +RestoreBackup::Active::Active() +{ + // Do nothing +} + +} // namespace phases +} // namespace rmf_task diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index ac5ecc13..6c7ff747 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -42,7 +42,7 @@ rmf_task::Phase::ConstTagPtr MockPhase::Active::tag() const } //============================================================================== -rmf_task::ConstEventPtr MockPhase::Active::finish_event() const +rmf_task::ConstEventPtr MockPhase::Active::final_event() const { return _event; } diff --git a/rmf_task/test/mock/MockPhase.hpp b/rmf_task/test/mock/MockPhase.hpp index f5fcc306..1ae154a6 100644 --- a/rmf_task/test/mock/MockPhase.hpp +++ b/rmf_task/test/mock/MockPhase.hpp @@ -41,7 +41,7 @@ class MockPhase : public rmf_task::Phase std::function phase_finished_); ConstTagPtr tag() const final; - rmf_task::ConstEventPtr finish_event() const final; + rmf_task::ConstEventPtr final_event() const final; rmf_traffic::Time estimate_finish_time() const final; void send_update() const; diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index 6b186e00..65c4c43d 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -128,7 +128,6 @@ void MockTask::Active::start_next_phase(rmf_traffic::Time current_time) { _phase_finished( std::make_shared( - _active_phase->tag(), rmf_task::Phase::Snapshot::make(*_active_phase), _active_phase->_start_time, current_time)); diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index b10da18e..0734a80b 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -17,10 +17,9 @@ #include -#include - -#include +#include +#include #include #include @@ -28,6 +27,8 @@ #include +#include + namespace rmf_task_sequence { namespace { @@ -266,28 +267,18 @@ auto Task::Active::backup() const -> Backup //============================================================================== void Task::Active::_load_backup(std::string backup_state_str) { - auto restore_phase_tag = std::make_shared( - 0, - "Restore from backup", - "The task progress is being restored from a backed up state", - rmf_traffic::Duration(0)); + const auto restore_phase = rmf_task::phases::RestoreBackup::Active::make( + backup_state_str, std::chrono::steady_clock::now()); // TODO(MXG): Allow users to specify a custom clock for the log const auto start_time = std::chrono::steady_clock::now(); - auto restore_phase_log = rmf_task::Log(); - - const auto get_state_text = [&]() -> std::string - { - return "\n```\n" + backup_state_str + "\n```\n"; - }; const auto failed_to_restore = [&]() -> void { _pending_stages.clear(); _phase_finished( std::make_shared( - std::move(restore_phase_tag), - restore_phase_log.view(), + rmf_task::Phase::Snapshot::make(*restore_phase), start_time, std::chrono::steady_clock::now())); @@ -298,10 +289,7 @@ void Task::Active::_load_backup(std::string backup_state_str) if (const auto result = schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) { - restore_phase_log.error( - "Error in state data while trying to restore task from backup: " + - result->message + "\n - Original message:" + get_state_text()); - + restore_phase->parsing_failed(result->message); return failed_to_restore(); } @@ -312,10 +300,10 @@ void Task::Active::_load_backup(std::string backup_state_str) const auto cancelled_from = cancelled_from_json.get(); if (cancelled_from >= _cancel_sequence_initial_id) { - restore_phase_log.error( + restore_phase->parsing_failed( "Invalid value [" + std::to_string(cancelled_from) + "] for [cancelled_from]. Value must be less than [" - + std::to_string(_cancel_sequence_initial_id) + "]" + get_state_text()); + + std::to_string(_cancel_sequence_initial_id) + "]"); return failed_to_restore(); } @@ -347,7 +335,7 @@ void Task::Active::_load_backup(std::string backup_state_str) if (_pending_stages.empty()) { - restore_phase_log.error( + restore_phase->parsing_failed( "Invalid value [" + std::to_string(current_phase_id) + "] for [current_phase/id]. " "Value is higher than all available phase IDs."); @@ -379,8 +367,8 @@ void Task::Active::_load_backup(std::string backup_state_str) { // This shouldn't happen, but it's not a critical error. In the worst // case, the operator needs to resend a skip command. - restore_phase_log.warn( - "Unexpected ordering of phase skip IDs" + get_state_text()); + restore_phase->update_log().warn( + "Unexpected ordering of phase skip IDs"); continue; } @@ -413,8 +401,7 @@ void Task::Active::_finish_phase() const auto phase_finish_time = _clock(); const auto completed_phase = std::make_shared( - _active_phase->tag(), - _active_phase->finish_event(), + rmf_task::Phase::Snapshot::make(*_active_phase), _current_phase_start_time.value(), phase_finish_time); From 6b4676596d2ab4437a9bf226b9502297e278d6ee Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 18 Oct 2021 21:40:55 +0800 Subject: [PATCH 45/85] Abstracting Phase and Event interfaces into Activity Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Event.hpp | 34 ++- rmf_task/include/rmf_task/Log.hpp | 6 +- rmf_task/include/rmf_task/Phase.hpp | 10 +- rmf_task/include/rmf_task/Task.hpp | 4 +- .../include/rmf_task/events/SimpleEvent.hpp | 9 +- .../include/rmf_task/phases/RestoreBackup.hpp | 2 +- rmf_task/src/rmf_task/Condition.cpp | 12 +- rmf_task/src/rmf_task/Phase.cpp | 4 +- rmf_task/src/rmf_task/events/SimpleEvent.cpp | 17 +- .../src/rmf_task/phases/RestoreBackup.cpp | 2 +- rmf_task/test/mock/MockEvent.cpp | 4 +- rmf_task/test/mock/MockEvent.hpp | 4 +- rmf_task/test/mock/MockPhase.cpp | 2 +- rmf_task/test/mock/MockPhase.hpp | 2 +- .../include/rmf_task_sequence/Activity.hpp | 205 ++++++++++++++++++ .../include/rmf_task_sequence/Event.hpp | 65 ++++++ .../include/rmf_task_sequence/Phase.hpp | 200 +---------------- .../rmf_task_sequence/detail/Backup.hpp | 66 ++++++ 18 files changed, 403 insertions(+), 245 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/Activity.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/Event.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp diff --git a/rmf_task/include/rmf_task/Event.hpp b/rmf_task/include/rmf_task/Event.hpp index 2d89b484..18519f05 100644 --- a/rmf_task/include/rmf_task/Event.hpp +++ b/rmf_task/include/rmf_task/Event.hpp @@ -30,16 +30,11 @@ namespace rmf_task { -class Event; -using ConstEventPtr = std::shared_ptr; - //============================================================================== class Event { public: - using Id = uint64_t; - /// A simple computer-friendly indicator of the current status of this /// event. This enum may be used to automatically identify when an /// event requires special attention, e.g. logging a warning or alerting @@ -86,6 +81,23 @@ class Event Finished }; + class Active; + using ConstActivePtr = std::shared_ptr; + + class Snapshot; + using ConstSnapshotPtr = std::shared_ptr; + +}; + +//============================================================================== +/// The interface to an active event. +class Event::Active +{ +public: + + using Status = Event::Status; + using ConstActivePtr = Event::ConstActivePtr; + /// The current Status of this event virtual Status status() const = 0; @@ -103,12 +115,10 @@ class Event virtual Log::View log() const = 0; /// Get more granular dependencies of this event, if any exist. - virtual std::vector dependencies() const = 0; - - class Snapshot; + virtual std::vector dependencies() const = 0; // Virtual destructor - virtual ~Event() = default; + virtual ~Active() = default; }; //============================================================================== @@ -116,12 +126,12 @@ class Event /// original event is arbitrarily changed, and there is no risk of a race /// condition, as long as the snapshot is not being created while the event /// is changing. -class Event::Snapshot : public Event +class Event::Snapshot : public Event::Active { public: /// Make a snapshot of the current state of an Event - static ConstEventPtr make(const Event& other); + static ConstSnapshotPtr make(const Active& other); // Documentation inherited Status status() const final; @@ -136,7 +146,7 @@ class Event::Snapshot : public Event Log::View log() const final; // Documentation inherited - std::vector dependencies() const final; + std::vector dependencies() const final; class Implementation; private: diff --git a/rmf_task/include/rmf_task/Log.hpp b/rmf_task/include/rmf_task/Log.hpp index 842a20c7..6df5cd37 100644 --- a/rmf_task/include/rmf_task/Log.hpp +++ b/rmf_task/include/rmf_task/Log.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__EXECUTE__LOG_HPP -#define RMF_TASK__EXECUTE__LOG_HPP +#ifndef RMF_TASK__LOG_HPP +#define RMF_TASK__LOG_HPP #include @@ -209,4 +209,4 @@ class Log::Reader::Iterable::iterator } // namespace rmf_task -#endif // RMF_TASK__EXECUTE__LOG_HPP +#endif // RMF_TASK__LOG_HPP diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 74e21371..5c3b4dea 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__EXECUTE__PHASE_HPP -#define RMF_TASK__EXECUTE__PHASE_HPP +#ifndef RMF_TASK__PHASE_HPP +#define RMF_TASK__PHASE_HPP #include @@ -131,7 +131,7 @@ class Phase::Active virtual ConstTagPtr tag() const = 0; /// The Event that needs to finish for this phase to be complete - virtual ConstEventPtr final_event() const = 0; + virtual Event::ConstActivePtr final_event() const = 0; /// The estimated finish time for this phase virtual rmf_traffic::Time estimate_finish_time() const = 0; @@ -152,7 +152,7 @@ class Phase::Snapshot : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - ConstEventPtr final_event() const final; + Event::ConstActivePtr final_event() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; @@ -187,4 +187,4 @@ class Phase::Pending } // namespace rmf_task -#endif // RMF_TASK__EXECUTE__PHASE_HPP +#endif // RMF_TASK__PHASE_HPP diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 95623e9a..89b83d9b 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__EXECUTE__TASK_HPP -#define RMF_TASK__EXECUTE__TASK_HPP +#ifndef RMF_TASK__TASK_HPP +#define RMF_TASK__TASK_HPP #include #include diff --git a/rmf_task/include/rmf_task/events/SimpleEvent.hpp b/rmf_task/include/rmf_task/events/SimpleEvent.hpp index 8de144c4..b522a3ba 100644 --- a/rmf_task/include/rmf_task/events/SimpleEvent.hpp +++ b/rmf_task/include/rmf_task/events/SimpleEvent.hpp @@ -32,7 +32,7 @@ namespace events { /// satisfy the Phase interface's finish_event() function. Your Phase /// implementation can create an instance of this class and then manage its /// fields directly. -class SimpleEvent : public Event +class SimpleEvent : public Event::Active { public: @@ -40,7 +40,7 @@ class SimpleEvent : public Event std::string name, std::string detail, Status initial_status, - std::vector dependencies = {}); + std::vector dependencies = {}); // Documentation inherited Status status() const final; @@ -67,10 +67,11 @@ class SimpleEvent : public Event Log& update_log(); // Documentation inherited - std::vector dependencies() const final; + std::vector dependencies() const final; /// Update the dependencies - SimpleEvent& update_dependencies(std::vector new_dependencies); + SimpleEvent& update_dependencies( + std::vector new_dependencies); class Implementation; private: diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index e8c46059..9fa4ac32 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -50,7 +50,7 @@ class RestoreBackup::Active : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - ConstEventPtr final_event() const final; + Event::ConstActivePtr final_event() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; diff --git a/rmf_task/src/rmf_task/Condition.cpp b/rmf_task/src/rmf_task/Condition.cpp index 0b6bb556..cd1a52c7 100644 --- a/rmf_task/src/rmf_task/Condition.cpp +++ b/rmf_task/src/rmf_task/Condition.cpp @@ -21,14 +21,14 @@ namespace rmf_task { namespace { //============================================================================== -std::vector snapshot_dependencies( - const std::vector& queue) +std::vector snapshot_dependencies( + const std::vector& queue) { // NOTE(MXG): This implementation is using recursion. That should be fine // since I don't expect much depth in the trees of dependencies, but we may // want to revisit this and implement it as a queue instead if we ever find // a use-case with deep recursion. - std::vector output; + std::vector output; output.reserve(queue.size()); for (const auto& c : queue) output.push_back(Event::Snapshot::make(*c)); @@ -46,12 +46,12 @@ class Event::Snapshot::Implementation VersionedString::View name; VersionedString::View detail; Log::View log; - std::vector dependencies; + std::vector dependencies; }; //============================================================================== -ConstEventPtr Event::Snapshot::make(const Event& other) +auto Event::Snapshot::make(const Active& other) -> ConstSnapshotPtr { Snapshot output; output._pimpl = rmf_utils::make_impl( @@ -91,7 +91,7 @@ Log::View Event::Snapshot::log() const } //============================================================================== -std::vector Event::Snapshot::dependencies() const +std::vector Event::Snapshot::dependencies() const { return _pimpl->dependencies; } diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 7bf75ed7..0db31895 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -120,7 +120,7 @@ class Phase::Snapshot::Implementation { public: ConstTagPtr tag; - ConstEventPtr finish_event; + Event::ConstActivePtr finish_event; rmf_traffic::Time estimated_finish_time; }; @@ -145,7 +145,7 @@ Phase::ConstTagPtr Phase::Snapshot::tag() const } //============================================================================== -ConstEventPtr Phase::Snapshot::final_event() const +Event::ConstActivePtr Phase::Snapshot::final_event() const { return _pimpl->finish_event; } diff --git a/rmf_task/src/rmf_task/events/SimpleEvent.cpp b/rmf_task/src/rmf_task/events/SimpleEvent.cpp index 4cf6efc6..e53055d8 100644 --- a/rmf_task/src/rmf_task/events/SimpleEvent.cpp +++ b/rmf_task/src/rmf_task/events/SimpleEvent.cpp @@ -29,16 +29,15 @@ class SimpleEvent::Implementation VersionedString name; VersionedString detail; Log log; - std::vector dependencies; + std::vector dependencies; }; //============================================================================== -std::shared_ptr SimpleEvent::make( - std::string name, +std::shared_ptr SimpleEvent::make(std::string name, std::string detail, - Status initial_status, - std::vector dependencies) + Event::Status initial_status, + std::vector dependencies) { SimpleEvent output; output._pimpl = rmf_utils::make_unique_impl( @@ -54,13 +53,13 @@ std::shared_ptr SimpleEvent::make( } //============================================================================== -auto SimpleEvent::status() const -> Status +Event::Status SimpleEvent::status() const { return _pimpl->status; } //============================================================================== -SimpleEvent& SimpleEvent::update_status(Status new_status) +SimpleEvent& SimpleEvent::update_status(Event::Status new_status) { _pimpl->status = new_status; return *this; @@ -105,14 +104,14 @@ Log& SimpleEvent::update_log() } //============================================================================== -std::vector SimpleEvent::dependencies() const +std::vector SimpleEvent::dependencies() const { return _pimpl->dependencies; } //============================================================================== SimpleEvent& SimpleEvent::update_dependencies( - std::vector new_dependencies) + std::vector new_dependencies) { _pimpl->dependencies = new_dependencies; return *this; diff --git a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp index a56df548..9de0e694 100644 --- a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -72,7 +72,7 @@ auto RestoreBackup::Active::tag() const -> ConstTagPtr } //============================================================================== -auto RestoreBackup::Active::final_event() const -> ConstEventPtr +Event::ConstActivePtr RestoreBackup::Active::final_event() const { return _pimpl->event; } diff --git a/rmf_task/test/mock/MockEvent.cpp b/rmf_task/test/mock/MockEvent.cpp index a5594497..a8d88df9 100644 --- a/rmf_task/test/mock/MockEvent.cpp +++ b/rmf_task/test/mock/MockEvent.cpp @@ -56,9 +56,9 @@ rmf_task::Log::View MockEvent::log() const } //============================================================================== -std::vector MockEvent::dependencies() const +std::vector MockEvent::dependencies() const { - return std::vector( + return std::vector( _dependencies.begin(), _dependencies.end()); } diff --git a/rmf_task/test/mock/MockEvent.hpp b/rmf_task/test/mock/MockEvent.hpp index 10af728f..e0a1f44a 100644 --- a/rmf_task/test/mock/MockEvent.hpp +++ b/rmf_task/test/mock/MockEvent.hpp @@ -23,7 +23,7 @@ namespace test_rmf_task { //============================================================================== -class MockEvent : public rmf_task::Event +class MockEvent : public rmf_task::Event::Active { public: @@ -37,7 +37,7 @@ class MockEvent : public rmf_task::Event rmf_task::VersionedString::View name() const final; rmf_task::VersionedString::View detail() const final; rmf_task::Log::View log() const final; - std::vector dependencies() const final; + std::vector dependencies() const final; // Fields Status _status; diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index 6c7ff747..ec113614 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -42,7 +42,7 @@ rmf_task::Phase::ConstTagPtr MockPhase::Active::tag() const } //============================================================================== -rmf_task::ConstEventPtr MockPhase::Active::final_event() const +rmf_task::Event::ConstActivePtr MockPhase::Active::final_event() const { return _event; } diff --git a/rmf_task/test/mock/MockPhase.hpp b/rmf_task/test/mock/MockPhase.hpp index 1ae154a6..08912c78 100644 --- a/rmf_task/test/mock/MockPhase.hpp +++ b/rmf_task/test/mock/MockPhase.hpp @@ -41,7 +41,7 @@ class MockPhase : public rmf_task::Phase std::function phase_finished_); ConstTagPtr tag() const final; - rmf_task::ConstEventPtr final_event() const final; + rmf_task::Event::ConstActivePtr final_event() const final; rmf_traffic::Time estimate_finish_time() const final; void send_update() const; diff --git a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp new file mode 100644 index 00000000..b632c8c0 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp @@ -0,0 +1,205 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__ACTIVITY_HPP +#define RMF_TASK_SEQUENCE__ACTIVITY_HPP + +#include + +#include +#include + +namespace rmf_task_sequence { + +//============================================================================== +/// The Activity namespace class provides abstract interfaces that are shared +/// between the Event and Phase namespace classes. +class Activity +{ +public: + + class Active; + + class Description; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; + using ConstModelPtr = std::shared_ptr; + + class SequenceModel; +}; + +//============================================================================== +/// The interface for an active activity. This interface deals with backing up +/// the current state, interrupting the activity, and cancelling or killing it. +class Activity::Active +{ +public: + + using Backup = detail::Backup; + + /// Get a backup for this Phase + virtual Backup backup() const = 0; + + /// The Resume class keeps track of when the phase is allowed to Resume. + /// You can either call the Resume object's operator() or let the object + /// expire to tell the phase that it may resume. + using Resume = rmf_task::detail::Resume; + + /// Tell this phase that it needs to be interrupted. An interruption means + /// the robot may be commanded to do other tasks before this phase resumes. + /// + /// Interruptions may occur to allow operators to take manual control of the + /// robot, or to engage automatic behaviors in response to emergencies, e.g. + /// fire alarms or code blues. + virtual Resume interrupt(std::function task_is_interrupted) = 0; + + /// Tell the phase that it has been canceled. The behavior that follows a + /// cancellation will vary between different phases, but generally it means + /// that the robot should no longer try to complete its Task and should + /// instead try to return itself to an unencumbered state as quickly as + /// possible. + /// + /// The phase may continue to perform some actions after being canceled. + /// + /// The phase should continue to be tracked as normal. When its finished + /// callback is triggered, the cancellation is complete. + virtual void cancel() = 0; + + /// Kill this phase. The behavior that follows a kill will vary between + /// different phases, but generally it means that the robot should be returned + /// to a safe idle state as soon as possible, even if it remains encumbered by + /// something related to this Task. + /// + /// The phase should continue to be tracked as normal. When its finished + /// callback is triggered, the killing is complete. + /// + /// The kill() command supersedes the cancel() command. Calling cancel() after + /// calling kill() will have no effect. + virtual void kill() = 0; + + // Virtual destructor + virtual ~Active() = default; +}; + +//============================================================================== +class Activity::Description +{ +public: + + /// Generate a Model for this phase based on its description, parameters, and + /// the invariants of its initial state. + /// + /// \param[in] invariant_initial_state + /// A partial state that represents the state components which will + /// definitely be true when this phase begins. + /// + /// \param[in] parameters + /// The parameters for the robot. + /// + /// \return a model based on the given start state and parameters. + virtual ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const = 0; + + /// Serialize this activity description into a JSON data structure. + virtual nlohmann::json serialize() const = 0; + + // Virtual destructor + virtual ~Description() = default; +}; + +//============================================================================== +class Activity::Model +{ +public: + + /// Estimate the state that the robot will have when the phase is finished. + /// + /// \param[in] initial_state + /// The expected initial state when the phase begins + /// + /// \param[in] constraints + /// Constraints on the robot during the phase + /// + /// \param[in] + /// + /// \return an estimated state for the robot when the phase is finished. + virtual std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const = 0; + + /// Estimate the invariant component of the request's duration. + virtual rmf_traffic::Duration invariant_duration() const = 0; + + /// Get the components of the finish state that this phase is guaranteed to + /// result in once the phase is finished. + virtual State invariant_finish_state() const = 0; + + // Virtual destructor + virtual ~Model() = default; +}; + +//============================================================================== +class Activity::SequenceModel : public Activity::Model +{ +public: + + /// Make a SequenceModel by providing a vector of descriptions and the + /// arguments that are given to Phase::Description::make_model(~). + /// + /// \param[in] descriptions + /// The Phase descriptions that are being modelled. The ordering of the + /// descriptions may impact model outcomes. The order of the descriptions + /// in the vector should reflect the actual order that the phases would + /// occur in. + /// + /// \param[in] invariant_initial_state + /// A partial state that represents the state components which will + /// definitely be true when this phase begins. + /// + /// \param[in] parameters + /// The parameters for the robot + /// + /// \return A Phase::Model implemented as a SequenceModel. + static ConstModelPtr make( + const std::vector& descriptions, + State invariant_initial_state, + const Parameters& parameters); + + // Documentation inherited + std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + // Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + // Documentation inherited + State invariant_finish_state() const final; + + class Implementation; +private: + SequenceModel(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__ACTIVITY_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp new file mode 100644 index 00000000..bd4446a2 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENT_HPP +#define RMF_TASK_SEQUENCE__EVENT_HPP + +#include +#include +#include +#include + +#include + +#include +#include + +namespace rmf_task_sequence { + +//============================================================================== +class Event : public rmf_task::Event +{ +public: + + class Active; + using ActivePtr = std::shared_ptr; + + class Activator; + using ActivatorPtr = std::shared_ptr; + using ConstActivatorPtr = std::shared_ptr; + + class Description; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; + using ConstModelPtr = std::shared_ptr; +}; + +//============================================================================== +class Event::Active : public rmf_task::Event::Active +{ +public: + + using Backup = detail::Backup; + + virtual Backup backup() const = 0; + +}; + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENT_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 78695fdb..ea0ef899 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -18,17 +18,12 @@ #ifndef RMF_TASK_SEQUENCE__PHASE_HPP #define RMF_TASK_SEQUENCE__PHASE_HPP -#include -#include -#include -#include #include -#include -#include +#include #include -#include +#include namespace rmf_task_sequence { @@ -48,99 +43,13 @@ class Phase : public rmf_task::Phase class Description; using ConstDescriptionPtr = std::shared_ptr; - - class Model; - using ConstModelPtr = std::shared_ptr; - - class SequenceModel; }; //============================================================================== /// The interface for an Active phase within a phase sequence task. -class Phase::Active : public rmf_task::Phase::Active -{ -public: - - /// Backup data for the Phase. The state of the phase is represented by a - /// YAML::Node. The meaning and format of the Node is up to the phase - /// implementation to decide. - /// - /// Each Backup is tagged with a sequence number. As the Phase makes progress, - /// it can issue new Backups with higher sequence numbers. Only the Backup - /// with the highest sequence number will be kept. - class Backup; - - /// Get a backup for this Phase - virtual Backup backup() const = 0; - - /// The Resume class keeps track of when the phase is allowed to Resume. - /// You can either call the Resume object's operator() or let the object - /// expire to tell the phase that it may resume. - using Resume = rmf_task::detail::Resume; - - /// Tell this phase that it needs to be interrupted. An interruption means - /// the robot may be commanded to do other tasks before this phase resumes. - /// - /// Interruptions may occur to allow operators to take manual control of the - /// robot, or to engage automatic behaviors in response to emergencies, e.g. - /// fire alarms or code blues. - virtual Resume interrupt(std::function task_is_interrupted) = 0; - - /// Tell the phase that it has been canceled. The behavior that follows a - /// cancellation will vary between different phases, but generally it means - /// that the robot should no longer try to complete its Task and should - /// instead try to return itself to an unencumbered state as quickly as - /// possible. - /// - /// The phase may continue to perform some actions after being canceled. - /// - /// The phase should continue to be tracked as normal. When its finished - /// callback is triggered, the cancellation is complete. - virtual void cancel() = 0; - - /// Kill this phase. The behavior that follows a kill will vary between - /// different phases, but generally it means that the robot should be returned - /// to a safe idle state as soon as possible, even if it remains encumbered by - /// something related to this Task. - /// - /// The phase should continue to be tracked as normal. When its finished - /// callback is triggered, the killing is complete. - /// - /// The kill() command supersedes the cancel() command. Calling cancel() after - /// calling kill() will have no effect. - virtual void kill() = 0; - - // Virtual destructor - virtual ~Active() = default; -}; - -//============================================================================== -class Phase::Active::Backup -{ -public: - - /// Make a backup of the phase - /// - /// \param[in] seq - /// Sequence number. The Backup from this phase with the highest sequence - /// number will be held onto until a Backup with a higher sequence number is - /// issued. - /// - /// \param[in] state - /// A serialization of the phase's state. This will be used by - /// Phase::Activator when restoring a Task. - static Backup make(uint64_t seq, nlohmann::json state); - - /// Get the sequence number - uint64_t sequence() const; - - /// Get the YAML representation of the backed up state - const nlohmann::json& state() const; - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; +class Phase::Active + : public rmf_task::Phase::Active, + public Activity::Active { }; //============================================================================== class Phase::Activator @@ -250,25 +159,10 @@ class Phase::Activator }; //============================================================================== -class Phase::Description +class Phase::Description : public Activity::Description { public: - /// Generate a Model for this phase based on its description, parameters, and - /// the invariants of its initial state. - /// - /// \param[in] invariant_initial_state - /// A partial state that represents the state components which will - /// definitely be true when this phase begins. - /// - /// \param[in] parameters - /// The parameters for the robot. - /// - /// \return a model based on the given start state and parameters. - virtual ConstModelPtr make_model( - State invariant_initial_state, - const Parameters& parameters) const = 0; - /// Get the human-friendly information about this phase /// /// \param[in] initial_state @@ -281,92 +175,10 @@ class Phase::Description const State& initial_state, const Parameters& parameters) const = 0; - /// Serialize this phase description into a string. - virtual nlohmann::json serialize() const = 0; - // Virtual destructor virtual ~Description() = default; }; -//============================================================================== -class Phase::Model -{ -public: - - /// Estimate the state that the robot will have when the phase is finished. - /// - /// \param[in] initial_state - /// The expected initial state when the phase begins - /// - /// \param[in] constraints - /// Constraints on the robot during the phase - /// - /// \param[in] - /// - /// \return an estimated state for the robot when the phase is finished. - virtual std::optional estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const = 0; - - /// Estimate the invariant component of the request's duration. - virtual rmf_traffic::Duration invariant_duration() const = 0; - - /// Get the components of the finish state that this phase is guaranteed to - /// result in once the phase is finished. - virtual State invariant_finish_state() const = 0; - - // Virtual destructor - virtual ~Model() = default; -}; - -//============================================================================== -/// An implementation of a Phase::Model that models a sequence of -/// Phase::Descriptions. -class Phase::SequenceModel : public Phase::Model -{ -public: - - /// Make a SequenceModel by providing a vector of descriptions and the - /// arguments that are given to Phase::Description::make_model(~). - /// - /// \param[in] descriptions - /// The Phase descriptions that are being modelled. The ordering of the - /// descriptions may impact model outcomes. The order of the descriptions - /// in the vector should reflect the actual order that the phases would - /// occur in. - /// - /// \param[in] invariant_initial_state - /// A partial state that represents the state components which will - /// definitely be true when this phase begins. - /// - /// \param[in] parameters - /// The parameters for the robot - /// - /// \return A Phase::Model implemented as a SequenceModel. - static Phase::ConstModelPtr make( - const std::vector& descriptions, - State invariant_initial_state, - const Parameters& parameters); - - // Documentation inherited - std::optional estimate_finish( - State initial_state, - const Constraints& constraints, - const TravelEstimator& travel_estimator) const final; - - // Documentation inherited - rmf_traffic::Duration invariant_duration() const final; - - // Documentation inherited - State invariant_finish_state() const final; - - class Implementation; -private: - SequenceModel(); - rmf_utils::unique_impl_ptr _pimpl; -}; - } // namespace rmf_task_sequence #include diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp new file mode 100644 index 00000000..8f000aef --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__DETAIL__BACKUP_HPP +#define RMF_TASK_SEQUENCE__DETAIL__BACKUP_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace detail { + +//============================================================================== +/// Backup data for a Phase or Event. The state is represented by a +/// nlohmann::json data structure. The meaning and format of the json structure +/// is up to the phase or event implementation to decide. +/// +/// Each Backup is tagged with a sequence number. As the phase or event makes +/// progress, it can issue new Backups with higher sequence numbers. Only the +/// Backup with the highest sequence number will be kept. +class Backup +{ +public: + + /// Make a backup of the phase + /// + /// \param[in] seq + /// Sequence number. The Backup from this phase with the highest sequence + /// number will be held onto until a Backup with a higher sequence number is + /// issued. + /// + /// \param[in] state + /// A serialization of the phase's state. This will be used by + /// Phase::Activator when restoring a Task. + static Backup make(uint64_t seq, nlohmann::json state); + + /// Get the sequence number + uint64_t sequence() const; + + /// Get the YAML representation of the backed up state + const nlohmann::json& state() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace detail +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__BACKUP_HPP From cfb1e90dd32806becab35ef7989a47d0bf3c023a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Oct 2021 18:19:18 +0800 Subject: [PATCH 46/85] Finish refactoring to introduce Activity class namespace Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Header.hpp | 65 ++++++++++++ rmf_task/include/rmf_task/Phase.hpp | 28 ++---- rmf_task/include/rmf_task/Task.hpp | 15 +-- rmf_task/src/rmf_task/Header.cpp | 64 ++++++++++++ rmf_task/src/rmf_task/Phase.cpp | 28 +----- rmf_task/src/rmf_task/Task.cpp | 28 +----- rmf_task/src/rmf_task/VersionedString.cpp | 2 +- .../src/rmf_task/phases/RestoreBackup.cpp | 19 ++-- rmf_task/test/integration/test_backups.cpp | 6 +- rmf_task/test/mock/MockPhase.cpp | 2 +- rmf_task/test/mock/MockTask.cpp | 13 ++- rmf_task/test/mock/MockTask.hpp | 3 +- rmf_task_sequence/CMakeLists.txt | 3 +- .../include/rmf_task_sequence/Activity.hpp | 19 +++- .../include/rmf_task_sequence/Event.hpp | 21 +--- .../include/rmf_task_sequence/Phase.hpp | 26 +---- .../rmf_task_sequence/events/GoToPlace.hpp | 78 +++++++++++++++ .../{phases => events}/WaitFor.hpp | 16 ++- .../rmf_task_sequence/phases/DropOff.hpp | 8 +- .../rmf_task_sequence/phases/GoToPlace.hpp | 56 ----------- .../rmf_task_sequence/phases/PickUp.hpp | 8 +- .../include/rmf_task_sequence/typedefs.hpp | 2 + .../src/rmf_task_sequence/Activity.cpp | 98 +++++++++++++++++++ .../src/rmf_task_sequence/Phase.cpp | 75 -------------- .../src/rmf_task_sequence/Task.cpp | 50 +++++----- .../{phases => events}/GoToPlace.cpp | 69 ++++++++----- .../{phases => events}/WaitFor.cpp | 25 +++-- .../src/rmf_task_sequence/phases/DropOff.cpp | 20 ++-- .../phases/PayloadTransfer.cpp | 20 ++-- .../src/rmf_task_sequence/phases/PickUp.cpp | 19 ++-- .../phases/internal_PayloadTransfer.hpp | 15 ++- 31 files changed, 503 insertions(+), 398 deletions(-) create mode 100644 rmf_task/include/rmf_task/Header.hpp create mode 100644 rmf_task/src/rmf_task/Header.cpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp rename rmf_task_sequence/include/rmf_task_sequence/{phases => events}/WaitFor.hpp (89%) create mode 100644 rmf_task_sequence/src/rmf_task_sequence/Activity.cpp rename rmf_task_sequence/src/rmf_task_sequence/{phases => events}/GoToPlace.cpp (80%) rename rmf_task_sequence/src/rmf_task_sequence/{phases => events}/WaitFor.cpp (87%) diff --git a/rmf_task/include/rmf_task/Header.hpp b/rmf_task/include/rmf_task/Header.hpp new file mode 100644 index 00000000..b9924968 --- /dev/null +++ b/rmf_task/include/rmf_task/Header.hpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2021 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 RMF_TASK__HEADER_HPP +#define RMF_TASK__HEADER_HPP + +#include + +#include + +#include + +namespace rmf_task { + +//============================================================================== +class Header +{ +public: + + /// Constructor + /// + /// \param[in] category_ + /// Category of the subject + /// + /// \param[in] detail_ + /// Details about the subject + /// + /// \param[in] estimate_ + /// The original (ideal) estimate of how long the subject will last + Header( + std::string category_, + std::string detail_, + rmf_traffic::Duration estimate_); + + /// Category of the subject + const std::string& category() const; + + /// Details about the subject + const std::string& detail() const; + + /// The original (ideal) estimate of how long the subject will last + rmf_traffic::Duration original_duration_estimate() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__HEADER_HPP diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 5c3b4dea..f438ffc5 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__PHASE_HPP #include +#include #include @@ -65,31 +66,16 @@ class Phase::Tag /// \param[in] id_ /// ID of the phase. This phase ID must be unique within its Task instance. /// - /// \param[in] name_ - /// Name of the phase - /// - /// \param[in] detail_ - /// Details about the phase - /// - /// \param[in] estimate_ - /// The original (ideal) estimate of how long the phase will last - Tag( - Id id_, - std::string name_, - std::string detail_, - rmf_traffic::Duration estimate_); + /// \param[in] header_ + /// Header of the phase. + Tag(Id id_, Header header_); /// Unique ID of the phase Id id() const; - /// Name of the phase - const std::string& name() const; - - /// Details about the phase - const std::string& detail() const; - - /// The original (ideal) estimate of how long the phase will last - rmf_traffic::Duration original_duration_estimate() const; + /// Header of the phase, containing human-friendly high-level information + /// about the phase. + const Header& header() const; class Implementation; private: diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 89b83d9b..a06cea94 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -18,6 +18,7 @@ #ifndef RMF_TASK__TASK_HPP #define RMF_TASK__TASK_HPP +#include #include #include #include @@ -109,21 +110,13 @@ class Task::Tag /// Constructor Tag( ConstBookingPtr booking_, - std::string category_, - std::string detail_, - rmf_traffic::Time original_finish_estimate_); + Header header_); /// The booking information of the request that this Task is carrying out const ConstBookingPtr& booking() const; - /// The category of this Task. - const std::string& category() const; - - /// Details about this Task. - const std::string& detail() const; - - /// The original finish estimate of this Task. - rmf_traffic::Time original_finish_estimate() const; + /// The header for this Task + const Header& header() const; class Implementation; private: diff --git a/rmf_task/src/rmf_task/Header.cpp b/rmf_task/src/rmf_task/Header.cpp new file mode 100644 index 00000000..a46bf733 --- /dev/null +++ b/rmf_task/src/rmf_task/Header.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task { + +//============================================================================== +class Header::Implementation +{ +public: + + std::string category; + std::string detail; + rmf_traffic::Duration duration; + +}; + +//============================================================================== +Header::Header( + std::string category_, + std::string detail_, + rmf_traffic::Duration estimate_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(category_), std::move(detail_), estimate_ + })) +{ + // Do nothing +} + +//============================================================================== +const std::string& Header::category() const +{ + return _pimpl->category; +} + +//============================================================================== +const std::string& Header::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +rmf_traffic::Duration Header::original_duration_estimate() const +{ + return _pimpl->duration; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 0db31895..123360c4 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -25,24 +25,18 @@ class Phase::Tag::Implementation public: Id id; - std::string name; - std::string detail; - rmf_traffic::Duration duration; + Header header; }; //============================================================================== Phase::Tag::Tag( Id id_, - std::string name_, - std::string detail_, - rmf_traffic::Duration estimate_) + Header header_) : _pimpl(rmf_utils::make_impl( Implementation{ id_, - std::move(name_), - std::move(detail_), - estimate_ + std::move(header_) })) { // Do nothing @@ -55,21 +49,9 @@ auto Phase::Tag::id() const -> Id } //============================================================================== -const std::string& Phase::Tag::name() const +const Header& Phase::Tag::header() const { - return _pimpl->name; -} - -//============================================================================== -const std::string& Phase::Tag::detail() const -{ - return _pimpl->detail; -} - -//============================================================================== -rmf_traffic::Duration Phase::Tag::original_duration_estimate() const -{ - return _pimpl->duration; + return _pimpl->header; } //============================================================================== diff --git a/rmf_task/src/rmf_task/Task.cpp b/rmf_task/src/rmf_task/Task.cpp index aebdff98..0897829c 100644 --- a/rmf_task/src/rmf_task/Task.cpp +++ b/rmf_task/src/rmf_task/Task.cpp @@ -77,23 +77,17 @@ class Task::Tag::Implementation { public: ConstBookingPtr booking; - std::string category; - std::string detail; - rmf_traffic::Time original_finish_estimate; + Header header; }; //============================================================================== Task::Tag::Tag( ConstBookingPtr booking_, - std::string category_, - std::string detail_, - rmf_traffic::Time original_finish_estimate_) + Header header_) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(booking_), - std::move(category_), - std::move(detail_), - original_finish_estimate_ + std::move(header_) })) { // Do nothing @@ -106,21 +100,9 @@ auto Task::Tag::booking() const -> const ConstBookingPtr& } //============================================================================== -const std::string& Task::Tag::category() const +const Header& Task::Tag::header() const { - return _pimpl->category; -} - -//============================================================================== -const std::string& Task::Tag::detail() const -{ - return _pimpl->detail; -} - -//============================================================================== -rmf_traffic::Time Task::Tag::original_finish_estimate() const -{ - return _pimpl->original_finish_estimate; + return _pimpl->header; } //============================================================================== diff --git a/rmf_task/src/rmf_task/VersionedString.cpp b/rmf_task/src/rmf_task/VersionedString.cpp index 028c402a..b6dd6d03 100644 --- a/rmf_task/src/rmf_task/VersionedString.cpp +++ b/rmf_task/src/rmf_task/VersionedString.cpp @@ -36,7 +36,7 @@ class VersionedString::Implementation ValuePtr value; // The token is used to uniquely identify this VersionedString. - struct Token { }; + struct Token {}; using TokenPtr = std::shared_ptr; TokenPtr token = std::make_shared(); diff --git a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp index 9de0e694..8811816c 100644 --- a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -30,9 +30,10 @@ class RestoreBackup::Active::Implementation { return std::make_shared( 0, - "Restore from backup", - "The task progress is being restored from a backed up state", - rmf_traffic::Duration(0)); + Header( + "Restore from backup", + "The task progress is being restored from a backed up state", + rmf_traffic::Duration(0))); } Implementation( @@ -40,7 +41,9 @@ class RestoreBackup::Active::Implementation rmf_traffic::Time estimated_finish_time_) : tag(make_tag()), event(events::SimpleEvent::make( - tag->name(), tag->detail(), rmf_task::Event::Status::Underway)), + tag->header().category(), + tag->header().detail(), + rmf_task::Event::Status::Underway)), estimated_finish_time(estimated_finish_time_) { event->update_log().info( @@ -87,16 +90,16 @@ rmf_traffic::Time RestoreBackup::Active::estimate_finish_time() const void RestoreBackup::Active::parsing_failed(const std::string& error_message) { _pimpl->event - ->update_status(Event::Status::Error) - .update_log().error("Parsing failed: " + error_message); + ->update_status(Event::Status::Error) + .update_log().error("Parsing failed: " + error_message); } //============================================================================== void RestoreBackup::Active::restoration_failed(const std::string& error_message) { _pimpl->event - ->update_status(Event::Status::Error) - .update_log().error("Restoration failed: " + error_message); + ->update_status(Event::Status::Error) + .update_log().error("Restoration failed: " + error_message); } //============================================================================== diff --git a/rmf_task/test/integration/test_backups.cpp b/rmf_task/test/integration/test_backups.cpp index 233a762e..c23464a9 100644 --- a/rmf_task/test/integration/test_backups.cpp +++ b/rmf_task/test/integration/test_backups.cpp @@ -209,6 +209,8 @@ SCENARIO("Back up to file") mock_restored_task->_active_phase->send_update(); REQUIRE(restored_snapshot); CHECK(restored_snapshot->tag()->id() == phase_snapshot->tag()->id()); - CHECK(restored_snapshot->tag()->name() == phase_snapshot->tag()->name()); - CHECK(restored_snapshot->tag()->detail() == phase_snapshot->tag()->detail()); + CHECK(restored_snapshot->tag()->header().category() + == phase_snapshot->tag()->header().category()); + CHECK(restored_snapshot->tag()->header().detail() + == phase_snapshot->tag()->header().detail()); } diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index ec113614..79fbef4f 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -50,7 +50,7 @@ rmf_task::Event::ConstActivePtr MockPhase::Active::final_event() const //============================================================================== rmf_traffic::Time MockPhase::Active::estimate_finish_time() const { - return _start_time + _tag->original_duration_estimate(); + return _start_time + _tag->header().original_duration_estimate(); } //============================================================================== diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index 65c4c43d..fd4d053d 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -41,13 +41,14 @@ auto MockTask::Active::pending_phases() const -> std::vector //============================================================================== auto MockTask::Active::tag() const -> const ConstTagPtr& { - return _ctag; + return _tag; } //============================================================================== rmf_traffic::Time MockTask::Active::estimate_finish_time() const { - return _tag->original_finish_estimate(); + return std::chrono::steady_clock::now() + + _tag->header().original_duration_estimate(); } //============================================================================== @@ -100,8 +101,9 @@ MockTask::Active::Active( std::function phase_finished, std::function task_finished) : _tag(std::make_shared( - booking, "Mock Task", "Mocked up task", rmf_traffic::Time())), - _ctag(_tag), + booking, + rmf_task::Header( + "Mock Task", "Mocked up task", rmf_traffic::Duration(0)))), _update(std::move(update)), _checkpoint(std::move(checkpoint)), _phase_finished(std::move(phase_finished)), @@ -118,7 +120,8 @@ void MockTask::Active::add_pending_phase( { _pending_phases.emplace_back( std::make_shared( - _next_phase_id++, std::move(name), std::move(detail), estimate)); + _next_phase_id++, + rmf_task::Header(std::move(name), std::move(detail), estimate))); } //============================================================================== diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp index 88a5f66d..9e288ce4 100644 --- a/rmf_task/test/mock/MockTask.hpp +++ b/rmf_task/test/mock/MockTask.hpp @@ -76,8 +76,7 @@ class MockTask : public rmf_task::Task void issue_backup(); - std::shared_ptr _tag; - ConstTagPtr _ctag; + ConstTagPtr _tag; std::vector _completed_phases; std::shared_ptr _active_phase; std::vector _pending_phases; diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 15388570..bb360087 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -28,9 +28,8 @@ find_package(ament_cmake_catch2 QUIET) find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Task Sequence Library -file(GLOB lib_srcs +file(GLOB_RECURSE lib_srcs "src/rmf_task_sequence/*.cpp" - "src/rmf_task_sequence/phases/*.cpp" ) add_library(rmf_task_sequence SHARED diff --git a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp index b632c8c0..e728bf06 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp @@ -19,6 +19,7 @@ #define RMF_TASK_SEQUENCE__ACTIVITY_HPP #include +#include #include #include @@ -101,12 +102,12 @@ class Activity::Description { public: - /// Generate a Model for this phase based on its description, parameters, and - /// the invariants of its initial state. + /// Generate a Model for this Activity based on its description, parameters, + /// and the invariants of its initial state. /// /// \param[in] invariant_initial_state /// A partial state that represents the state components which will - /// definitely be true when this phase begins. + /// definitely be true when this Activity begins. /// /// \param[in] parameters /// The parameters for the robot. @@ -116,8 +117,16 @@ class Activity::Description State invariant_initial_state, const Parameters& parameters) const = 0; - /// Serialize this activity description into a JSON data structure. - virtual nlohmann::json serialize() const = 0; + /// Generate human-friendly header information for this Activity. + /// + /// \param[in] initial_state + /// The expected initial state when the activity begins + /// + /// \param[in] parameters + /// Parameters of the robot during the Activity + virtual Header generate_header( + const State& initial_state, + const Parameters& parameters) const = 0; // Virtual destructor virtual ~Description() = default; diff --git a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp index bd4446a2..a1a7b91e 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -25,8 +25,7 @@ #include -#include -#include +#include namespace rmf_task_sequence { @@ -35,29 +34,15 @@ class Event : public rmf_task::Event { public: - class Active; + class Active : public Activity::Active {}; using ActivePtr = std::shared_ptr; class Activator; using ActivatorPtr = std::shared_ptr; using ConstActivatorPtr = std::shared_ptr; - class Description; + class Description : public Activity::Description {}; using ConstDescriptionPtr = std::shared_ptr; - - class Model; - using ConstModelPtr = std::shared_ptr; -}; - -//============================================================================== -class Event::Active : public rmf_task::Event::Active -{ -public: - - using Backup = detail::Backup; - - virtual Backup backup() const = 0; - }; } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index ea0ef899..9dddc20c 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -47,9 +47,9 @@ class Phase : public rmf_task::Phase //============================================================================== /// The interface for an Active phase within a phase sequence task. -class Phase::Active - : public rmf_task::Phase::Active, - public Activity::Active { }; +class Phase::Active : + public rmf_task::Phase::Active, + public Activity::Active {}; //============================================================================== class Phase::Activator @@ -159,25 +159,7 @@ class Phase::Activator }; //============================================================================== -class Phase::Description : public Activity::Description -{ -public: - - /// Get the human-friendly information about this phase - /// - /// \param[in] initial_state - /// The expected initial state when the phase begins - /// - /// \param[in] constraints - /// Constraints on the robot during the phase - virtual rmf_task::Phase::ConstTagPtr make_tag( - rmf_task::Phase::Tag::Id id, - const State& initial_state, - const Parameters& parameters) const = 0; - - // Virtual destructor - virtual ~Description() = default; -}; +class Phase::Description : public Activity::Description {}; } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp new file mode 100644 index 00000000..05f709b2 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__GOTOPLACE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__GOTOPLACE_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class GoToPlace +{ +public: + using Goal = rmf_traffic::agv::Plan::Goal; + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class GoToPlace::Description : public Event::Description +{ +public: + + /// Make a GoToPlace description using a goal. + static DescriptionPtr make(Goal goal); + + /// Get the current goal for this description. + const Goal& destination() const; + + /// Set the current goal for this description. + Description& destination(Goal new_goal); + + /// Get the name of the goal. If the goal does not have an explicit name, it + /// will be referred to as "#x" where "x" is the index number of the waypoint. + std::string destination_name(const rmf_task::Parameters& parameters) const; + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__GOTOPLACE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp similarity index 89% rename from rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp rename to rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp index faa4a41e..864c85af 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp @@ -15,15 +15,15 @@ * */ -#ifndef RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP -#define RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP +#ifndef RMF_TASK_SEQUENCE__EVENTS__WAITFOR_HPP +#define RMF_TASK_SEQUENCE__EVENTS__WAITFOR_HPP #include #include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== /// A WaitFor phase encompasses having the robot sit in place and wait for a @@ -63,26 +63,22 @@ class WaitFor::Description : public Phase::Description Description& duration(rmf_traffic::Duration new_duration); // Documentation inherited - Phase::ConstModelPtr make_model( + Activity::ConstModelPtr make_model( rmf_task::State invariant_initial_state, const rmf_task::Parameters& parameters) const final; // Documentation inherited - Phase::ConstTagPtr make_tag( - Phase::Tag::Id id, + rmf_task::Header generate_header( const rmf_task::State& initial_state, const rmf_task::Parameters& parameters) const final; - // Documentation inherited - nlohmann::json serialize() const final; - class Implementation; private: Description(); rmf_utils::impl_ptr _pimpl; }; -} // namespace phases +} // namespace events } // namespace rmf_task_sequence #endif // RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp index f4d57e13..7aabedb8 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp @@ -92,19 +92,15 @@ class DropOff::Description : public Phase::Description Description& unloading_duration_estimate(rmf_traffic::Duration new_duration); // Documentation inherited - Phase::ConstModelPtr make_model( + Activity::ConstModelPtr make_model( State invariant_initial_state, const Parameters& parameters) const final; // Documentation inherited - Phase::ConstTagPtr make_tag( - Phase::Tag::Id id, + Header generate_header( const State& initial_state, const Parameters& parameters) const final; - // Documentation inherited - nlohmann::json serialize() const final; - class Implementation; private: Description(); diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp index 6d889998..07ebdf8a 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp @@ -22,61 +22,5 @@ #include -namespace rmf_task_sequence { -namespace phases { - -//============================================================================== -class GoToPlace -{ -public: - using Goal = rmf_traffic::agv::Plan::Goal; - - class Description; - using DescriptionPtr = std::shared_ptr; - using ConstDescriptionPtr = std::shared_ptr; - - class Model; -}; - -//============================================================================== -class GoToPlace::Description : public Phase::Description -{ -public: - - /// Make a GoToPlace description using a goal. - static DescriptionPtr make(Goal goal); - - /// Get the current goal for this description. - const Goal& goal() const; - - /// Set the current goal for this description. - Description& goal(Goal new_goal); - - /// Get the name of the goal. If the goal does not have an explicit name, it - /// will be referred to as "#x" where "x" is the index number of the waypoint. - std::string goal_name(const rmf_task::Parameters& parameters) const; - - // Documentation inherited - Phase::ConstModelPtr make_model( - rmf_task::State invariant_initial_state, - const rmf_task::Parameters& parameters) const final; - - // Documentation inherited - Phase::ConstTagPtr make_tag( - Phase::Tag::Id id, - const rmf_task::State& initial_state, - const rmf_task::Parameters& parameters) const final; - - // Documentation inherited - nlohmann::json serialize() const final; - - class Implementation; -private: - Description(); - rmf_utils::impl_ptr _pimpl; -}; - -} // namespace phases -} // namespace rmf_task_sequence #endif // RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp index b59a5a6a..d4ed9923 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp @@ -92,19 +92,15 @@ class PickUp::Description : public Phase::Description Description& loading_duration_estimate(rmf_traffic::Duration new_duration); // Documentation inherited - Phase::ConstModelPtr make_model( + Activity::ConstModelPtr make_model( State invariant_initial_state, const Parameters& parameters) const final; // Documentation inherited - Phase::ConstTagPtr make_tag( - Phase::Tag::Id id, + Header generate_header( const State& initial_state, const Parameters& parameters) const final; - // Documentation inherited - nlohmann::json serialize() const final; - class Implementation; private: Description(); diff --git a/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp index a2d4f2c2..9aa8f74e 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp @@ -18,6 +18,7 @@ #ifndef RMF_TASK_SEQUENCE__TYPEDEFS_HPP #define RMF_TASK_SEQUENCE__TYPEDEFS_HPP +#include #include #include #include @@ -26,6 +27,7 @@ namespace rmf_task_sequence { +using Header = rmf_task::Header; using State = rmf_task::State; using Parameters = rmf_task::Parameters; using ConstParametersPtr = rmf_task::ConstParametersPtr; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp b/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp new file mode 100644 index 00000000..26511b02 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { + +//============================================================================== +class Activity::SequenceModel::Implementation +{ +public: + std::vector models; + rmf_task::State invariant_finish_state; + rmf_traffic::Duration invariant_duration; +}; + +//============================================================================== +Activity::ConstModelPtr Activity::SequenceModel::make( + const std::vector& descriptions, + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) +{ + std::vector models; + rmf_task::State invariant_finish_state = invariant_initial_state; + rmf_traffic::Duration invariant_duration = rmf_traffic::Duration(0); + for (const auto& desc : descriptions) + { + auto next_model = desc->make_model(invariant_finish_state, parameters); + invariant_finish_state = next_model->invariant_finish_state(); + invariant_duration += next_model->invariant_duration(); + + models.emplace_back(std::move(next_model)); + } + + auto output = std::shared_ptr(new SequenceModel); + output->_pimpl = rmf_utils::make_unique_impl( + Implementation{ + std::move(models), + std::move(invariant_finish_state), + invariant_duration + }); + + return output; +} + +//============================================================================== +std::optional Activity::SequenceModel::estimate_finish( + rmf_task::State initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const +{ + std::optional finish_state = std::move(initial_state); + for (const auto& model : _pimpl->models) + { + finish_state = model->estimate_finish( + std::move(*finish_state), constraints, travel_estimator); + + if (!finish_state.has_value()) + return std::nullopt; + } + + return *finish_state; +} + +//============================================================================== +rmf_traffic::Duration Activity::SequenceModel::invariant_duration() const +{ + return _pimpl->invariant_duration; +} + +//============================================================================== +rmf_task::State Activity::SequenceModel::invariant_finish_state() const +{ + return _pimpl->invariant_finish_state; +} + +//============================================================================== +Activity::SequenceModel::SequenceModel() +{ + // Do nothing +} + + +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index d9526d38..818215f3 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -60,79 +60,4 @@ void Phase::Activator::_add_activator( _pimpl->activators.insert_or_assign(type, std::move(activator)); } -//============================================================================== -class Phase::SequenceModel::Implementation -{ -public: - std::vector models; - rmf_task::State invariant_finish_state; - rmf_traffic::Duration invariant_duration; -}; - -//============================================================================== -Phase::ConstModelPtr Phase::SequenceModel::make( - const std::vector& descriptions, - rmf_task::State invariant_initial_state, - const rmf_task::Parameters& parameters) -{ - std::vector models; - rmf_task::State invariant_finish_state = invariant_initial_state; - rmf_traffic::Duration invariant_duration = rmf_traffic::Duration(0); - for (const auto& desc : descriptions) - { - auto next_model = desc->make_model(invariant_finish_state, parameters); - invariant_finish_state = next_model->invariant_finish_state(); - invariant_duration += next_model->invariant_duration(); - - models.emplace_back(std::move(next_model)); - } - - auto output = std::shared_ptr(new SequenceModel); - output->_pimpl = rmf_utils::make_unique_impl( - Implementation{ - std::move(models), - std::move(invariant_finish_state), - invariant_duration - }); - - return output; -} - -//============================================================================== -std::optional Phase::SequenceModel::estimate_finish( - rmf_task::State initial_state, - const rmf_task::Constraints& constraints, - const rmf_task::TravelEstimator& travel_estimator) const -{ - std::optional finish_state = std::move(initial_state); - for (const auto& model : _pimpl->models) - { - finish_state = model->estimate_finish( - std::move(*finish_state), constraints, travel_estimator); - - if (!finish_state.has_value()) - return std::nullopt; - } - - return *finish_state; -} - -//============================================================================== -rmf_traffic::Duration Phase::SequenceModel::invariant_duration() const -{ - return _pimpl->invariant_duration; -} - -//============================================================================== -rmf_task::State Phase::SequenceModel::invariant_finish_state() const -{ - return _pimpl->invariant_finish_state; -} - -//============================================================================== -Phase::SequenceModel::SequenceModel() -{ - // Do nothing -} - } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 0734a80b..9695c21a 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -186,17 +186,17 @@ class Task::Active std::function checkpoint, std::function phase_finished, std::function task_finished) - : _phase_activator(std::move(phase_activator)), - _clock(std::move(clock)), - _get_state(std::move(get_state)), - _parameters(parameters), - _booking(std::move(booking)), - _update(std::move(update)), - _checkpoint(std::move(checkpoint)), - _phase_finished(std::move(phase_finished)), - _task_finished(std::move(task_finished)), - _pending_stages(Description::Implementation::get_stages(description)), - _cancel_sequence_initial_id(_pending_stages.size()+1) + : _phase_activator(std::move(phase_activator)), + _clock(std::move(clock)), + _get_state(std::move(get_state)), + _parameters(parameters), + _booking(std::move(booking)), + _update(std::move(update)), + _checkpoint(std::move(checkpoint)), + _phase_finished(std::move(phase_finished)), + _task_finished(std::move(task_finished)), + _pending_stages(Description::Implementation::get_stages(description)), + _cancel_sequence_initial_id(_pending_stages.size()+1) { // Do nothing } @@ -235,7 +235,7 @@ class Task::Active const nlohmann::json_schema::json_validator Task::Active::backup_schema_validator = nlohmann::json_schema::json_validator( - schemas::backup_PhaseSequenceTask_v0_1); + schemas::backup_PhaseSequenceTask_v0_1); //============================================================================== auto Task::Builder::add_phase( @@ -287,7 +287,7 @@ void Task::Active::_load_backup(std::string backup_state_str) const auto backup_state = nlohmann::json::parse(backup_state_str); if (const auto result = - schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) + schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) { restore_phase->parsing_failed(result->message); return failed_to_restore(); @@ -338,7 +338,7 @@ void Task::Active::_load_backup(std::string backup_state_str) restore_phase->parsing_failed( "Invalid value [" + std::to_string(current_phase_id) + "] for [current_phase/id]. " - "Value is higher than all available phase IDs."); + "Value is higher than all available phase IDs."); return failed_to_restore(); } @@ -388,7 +388,12 @@ void Task::Active::_generate_pending_phases() { _pending_phases.push_back( std::make_shared( - s->description->make_tag(s->id, state, *_parameters))); + std::make_shared( + s->id, + s->description->generate_header(state, *_parameters) + ) + ) + ); } } @@ -399,11 +404,10 @@ void Task::Active::_finish_phase() _active_stage = nullptr; const auto phase_finish_time = _clock(); - const auto completed_phase = - std::make_shared( - rmf_task::Phase::Snapshot::make(*_active_phase), - _current_phase_start_time.value(), - phase_finish_time); + const auto completed_phase = std::make_shared( + rmf_task::Phase::Snapshot::make(*_active_phase), + _current_phase_start_time.value(), + phase_finish_time); _completed_phases.push_back(completed_phase); _phase_finished(completed_phase); @@ -540,9 +544,9 @@ auto Task::make_activator( -> rmf_task::Activator::Activate { return [ - phase_activator = std::move(phase_activator), - clock = std::move(clock) - ]( + phase_activator = std::move(phase_activator), + clock = std::move(clock) + ]( std::function get_state, const ConstParametersPtr& parameters, const ConstBookingPtr& booking, diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp similarity index 80% rename from rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp rename to rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp index 4d7aa24f..ef7c5a0f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/GoToPlace.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp @@ -15,10 +15,10 @@ * */ -#include +#include namespace rmf_task_sequence { -namespace phases { +namespace events { namespace { //============================================================================== @@ -42,11 +42,11 @@ std::optional estimate_duration( } // anonymous namespace //============================================================================== -class GoToPlace::Model : public Phase::Model +class GoToPlace::Model : public Activity::Model { public: - static Phase::ConstModelPtr make( + static Activity::ConstModelPtr make( State invariant_initial_state, const Parameters& parameters, Goal goal); @@ -55,7 +55,7 @@ class GoToPlace::Model : public Phase::Model std::optional estimate_finish( State initial_state, const Constraints& constraints, - const TravelEstimator& ) const final; + const TravelEstimator& estimator) const final; // Documentation inherited rmf_traffic::Duration invariant_duration() const final; @@ -76,7 +76,7 @@ class GoToPlace::Model : public Phase::Model }; //============================================================================== -Phase::ConstModelPtr GoToPlace::Model::make( +Activity::ConstModelPtr GoToPlace::Model::make( State invariant_initial_state, const Parameters& parameters, Goal goal) @@ -170,7 +170,7 @@ class GoToPlace::Description::Implementation { public: - rmf_traffic::agv::Plan::Goal goal; + rmf_traffic::agv::Plan::Goal destination; static std::string waypoint_name( const std::size_t index, @@ -200,70 +200,87 @@ auto GoToPlace::Description::make(Goal goal) -> DescriptionPtr } //============================================================================== -Phase::ConstModelPtr GoToPlace::Description::make_model( +Activity::ConstModelPtr GoToPlace::Description::make_model( State invariant_initial_state, const Parameters& parameters) const { return Model::make( std::move(invariant_initial_state), parameters, - _pimpl->goal); + _pimpl->destination); } //============================================================================== -Phase::ConstTagPtr GoToPlace::Description::make_tag( - Phase::Tag::Id id, +Header GoToPlace::Description::generate_header( const State& initial_state, const Parameters& parameters) const { + const auto fail = [](const std::string& msg) + { + throw std::runtime_error( + "[GoToPlace::Description::generate_header] " + msg); + }; + const auto start_wp_opt = initial_state.waypoint(); if (!start_wp_opt) - return nullptr; + fail("Initial state is missing a waypoint"); const auto start_wp = *start_wp_opt; const auto& graph = parameters.planner()->get_configuration().graph(); if (graph.num_waypoints() <= start_wp) - return nullptr; + { + fail("Initial waypoint [" + std::to_string(start_wp) + + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + + "]"); + } const auto start_name = Implementation::waypoint_name(start_wp, parameters); - if (graph.num_waypoints() <= _pimpl->goal.waypoint()) - return nullptr; + if (graph.num_waypoints() <= _pimpl->destination.waypoint()) + { + fail("Destination waypoint [" + + std::to_string(_pimpl->destination.waypoint()) + + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + + "]"); + } - const auto goal_name_ = goal_name(parameters); + const auto goal_name_ = destination_name(parameters); const auto estimate = estimate_duration( - parameters.planner(), initial_state, _pimpl->goal); + parameters.planner(), initial_state, _pimpl->destination); if (!estimate.has_value()) - return nullptr; + { + fail("Cannot find a path from [" + + start_name + "] to [" + goal_name_ + "]"); + } - return std::make_shared( - id, + return Header( "Go to [" + goal_name_ + "]", "Moving the robot from [" + start_name + "] to [" + goal_name_ + "]", *estimate); } //============================================================================== -auto GoToPlace::Description::goal() const -> const Goal& +auto GoToPlace::Description::destination() const -> const Goal& { - return _pimpl->goal; + return _pimpl->destination; } //============================================================================== -auto GoToPlace::Description::goal(Goal new_goal) -> Description& +auto GoToPlace::Description::destination(Goal new_goal) -> Description& { - _pimpl->goal = std::move(new_goal); + _pimpl->destination = std::move(new_goal); return *this; } //============================================================================== -std::string GoToPlace::Description::goal_name( +std::string GoToPlace::Description::destination_name( const Parameters& parameters) const { - return Implementation::waypoint_name(_pimpl->goal.waypoint(), parameters); + return Implementation::waypoint_name( + _pimpl->destination.waypoint(), parameters); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp similarity index 87% rename from rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp rename to rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp index 31cbdc1d..4897dd5b 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/WaitFor.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp @@ -15,13 +15,13 @@ * */ -#include +#include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== -class WaitFor::Model : public Phase::Model +class WaitFor::Model : public Activity::Model { public: @@ -30,14 +30,14 @@ class WaitFor::Model : public Phase::Model rmf_traffic::Duration duration, const rmf_task::Parameters& parameters); - std::optional estimate_finish( + std::optional estimate_finish( rmf_task::State initial_state, - const rmf_task::Constraints& constraints, - const rmf_task::TravelEstimator& travel_estimator) const final; + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; rmf_traffic::Duration invariant_duration() const final; - rmf_task::State invariant_finish_state() const final; + State invariant_finish_state() const final; private: rmf_task::State _invariant_finish_state; @@ -80,7 +80,7 @@ auto WaitFor::Description::duration(rmf_traffic::Duration new_duration) } //============================================================================== -Phase::ConstModelPtr WaitFor::Description::make_model( +Activity::ConstModelPtr WaitFor::Description::make_model( State invariant_initial_state, const Parameters& parameters) const { @@ -89,14 +89,13 @@ Phase::ConstModelPtr WaitFor::Description::make_model( } //============================================================================== -Phase::ConstTagPtr WaitFor::Description::make_tag( - Phase::Tag::Id id, const State&, const Parameters&) const +Header WaitFor::Description::generate_header( + const State&, const Parameters&) const { const auto seconds = std::chrono::duration_cast( _pimpl->duration); - return std::make_shared( - id, + return Header( "Waiting", "Waiting for [" + std::to_string(seconds.count()) + "] seconds to elapse", _pimpl->duration); @@ -118,7 +117,7 @@ WaitFor::Model::Model( { _invariant_battery_drain = parameters.ambient_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(_duration)); + rmf_traffic::time::to_seconds(_duration)); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp index 28301f6e..96ca321d 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp @@ -40,10 +40,10 @@ auto DropOff::Description::make( output->_pimpl = rmf_utils::make_unique_impl( Implementation{ PayloadTransfer( - std::move(drop_off_location), - std::move(into_ingestor), - std::move(payload), - unloading_duration_estimate) + std::move(drop_off_location), + std::move(into_ingestor), + std::move(payload), + unloading_duration_estimate) }); return output; @@ -52,14 +52,14 @@ auto DropOff::Description::make( //============================================================================== auto DropOff::Description::drop_off_location() const -> const Location& { - return _pimpl->transfer.go_to_place->goal(); + return _pimpl->transfer.go_to_place->destination(); } //============================================================================== auto DropOff::Description::drop_off_location(Location new_location) -> Description& { - _pimpl->transfer.go_to_place->goal(std::move(new_location)); + _pimpl->transfer.go_to_place->destination(std::move(new_location)); return *this; } @@ -105,7 +105,7 @@ auto DropOff::Description::unloading_duration_estimate( } //============================================================================== -Phase::ConstModelPtr DropOff::Description::make_model( +Activity::ConstModelPtr DropOff::Description::make_model( State invariant_initial_state, const Parameters& parameters) const { @@ -114,12 +114,12 @@ Phase::ConstModelPtr DropOff::Description::make_model( } //============================================================================== -Phase::ConstTagPtr DropOff::Description::make_tag( - Phase::Tag::Id id, +Header DropOff::Description::generate_header( const State& initial_state, const Parameters& parameters) const { - return _pimpl->transfer.make_tag("Drop off", id, initial_state, parameters); + return _pimpl->transfer + .generate_header("Drop off", initial_state, parameters); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp index 114d54b0..11506fd2 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp @@ -26,39 +26,37 @@ PayloadTransfer::PayloadTransfer( std::string target_, Payload payload_, rmf_traffic::Duration loading_duration_estimate) - : target(std::move(target_)), - payload(std::move(payload_)), - go_to_place(GoToPlace::Description::make(std::move(location_))), - wait_for(WaitFor::Description::make(loading_duration_estimate)) +: target(std::move(target_)), + payload(std::move(payload_)), + go_to_place(events::GoToPlace::Description::make(std::move(location_))), + wait_for(events::WaitFor::Description::make(loading_duration_estimate)) { descriptions = {go_to_place, wait_for}; } //============================================================================== -Phase::ConstModelPtr PayloadTransfer::make_model( +Activity::ConstModelPtr PayloadTransfer::make_model( State invariant_initial_state, const Parameters& parameters) const { - return Phase::SequenceModel::make( + return Activity::SequenceModel::make( descriptions, std::move(invariant_initial_state), parameters); } //============================================================================== -Phase::ConstTagPtr PayloadTransfer::make_tag( +Header PayloadTransfer::generate_header( const std::string& type, - Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const { const auto model = make_model(initial_state, parameters); - return std::make_shared( - id, + return Header( type, type + " " + payload.brief("into") + " at " - + go_to_place->goal_name(parameters), + + go_to_place->destination_name(parameters), model->invariant_duration()); } diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp index b0ba20da..9e7a33ce 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp @@ -40,10 +40,10 @@ auto PickUp::Description::make( output->_pimpl = rmf_utils::make_unique_impl( Implementation{ PayloadTransfer( - std::move(pickup_location), - std::move(from_dispenser), - std::move(payload), - loading_duration_estimate) + std::move(pickup_location), + std::move(from_dispenser), + std::move(payload), + loading_duration_estimate) }); return output; @@ -52,13 +52,13 @@ auto PickUp::Description::make( //============================================================================== auto PickUp::Description::pickup_location() const -> const Location& { - return _pimpl->transfer.go_to_place->goal(); + return _pimpl->transfer.go_to_place->destination(); } //============================================================================== auto PickUp::Description::pickup_location(Location new_location) -> Description& { - _pimpl->transfer.go_to_place->goal(std::move(new_location)); + _pimpl->transfer.go_to_place->destination(std::move(new_location)); return *this; } @@ -104,7 +104,7 @@ auto PickUp::Description::loading_duration_estimate( } //============================================================================== -Phase::ConstModelPtr PickUp::Description::make_model( +Activity::ConstModelPtr PickUp::Description::make_model( State invariant_initial_state, const Parameters& parameters) const { @@ -113,12 +113,11 @@ Phase::ConstModelPtr PickUp::Description::make_model( } //============================================================================== -Phase::ConstTagPtr PickUp::Description::make_tag( - Phase::Tag::Id id, +Header PickUp::Description::generate_header( const State& initial_state, const Parameters& parameters) const { - return _pimpl->transfer.make_tag("Pick up", id, initial_state, parameters); + return _pimpl->transfer.generate_header("Pick up", initial_state, parameters); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp index b0ac7ba6..fd7ac310 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include @@ -37,9 +37,9 @@ class PayloadTransfer std::string target; Payload payload; - GoToPlace::DescriptionPtr go_to_place; - WaitFor::DescriptionPtr wait_for; - std::vector descriptions; + events::GoToPlace::DescriptionPtr go_to_place; + events::WaitFor::DescriptionPtr wait_for; + std::vector descriptions; PayloadTransfer( Location location_, @@ -47,13 +47,12 @@ class PayloadTransfer Payload payload_, rmf_traffic::Duration loading_duration_estimate); - Phase::ConstModelPtr make_model( + Activity::ConstModelPtr make_model( State invariant_initial_state, const Parameters& parameters) const; - Phase::ConstTagPtr make_tag( + Header generate_header( const std::string& type, - Phase::Tag::Id id, const State& initial_state, const Parameters& parameters) const; }; From 0611526d4b8e95486dc3c111b447184971fb569f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Oct 2021 18:22:39 +0800 Subject: [PATCH 47/85] Remove the GoToPlace phase in favor of GoToPlace event Signed-off-by: Michael X. Grey --- .../rmf_task_sequence/phases/GoToPlace.hpp | 26 ------------------- 1 file changed, 26 deletions(-) delete mode 100644 rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp deleted file mode 100644 index 07ebdf8a..00000000 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/GoToPlace.hpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP -#define RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP - -#include - -#include - - -#endif // RMF_TASK_SEQUENCE__PHASES__GOTOPLACE_HPP From d19c7ad84cc3675aa30601cb7e52de13cf2e0856 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Oct 2021 10:21:24 +0800 Subject: [PATCH 48/85] Remember to include Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Log.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_task/include/rmf_task/Log.hpp b/rmf_task/include/rmf_task/Log.hpp index 6df5cd37..717b52df 100644 --- a/rmf_task/include/rmf_task/Log.hpp +++ b/rmf_task/include/rmf_task/Log.hpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace rmf_task { From 7d8db73091b4e40fb10d95399053c9f7e7f412fd Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Oct 2021 10:43:02 +0800 Subject: [PATCH 49/85] Use a const reference for pending tasks Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Phase.hpp | 2 -- rmf_task/include/rmf_task/Task.hpp | 2 +- rmf_task/test/mock/MockTask.cpp | 3 +- rmf_task/test/mock/MockTask.hpp | 2 +- .../src/rmf_task_sequence/Task.cpp | 28 +++++++++---------- 5 files changed, 17 insertions(+), 20 deletions(-) diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index f438ffc5..417e4801 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -47,8 +47,6 @@ class Phase using ConstSnapshotPtr = std::shared_ptr; class Pending; - using PendingPtr = std::shared_ptr; - using ConstPendingPtr = std::shared_ptr; }; //============================================================================== diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index a06cea94..4f3df5fb 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -188,7 +188,7 @@ class Task::Active virtual Phase::ConstActivePtr active_phase() const = 0; /// Descriptions of the phases that are expected in the future - virtual std::vector pending_phases() const = 0; + virtual const std::vector& pending_phases() const = 0; /// The tag of this Task virtual const ConstTagPtr& tag() const = 0; diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index fd4d053d..c5de8dfb 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -33,7 +33,8 @@ auto MockTask::Active::active_phase() const -> Phase::ConstActivePtr } //============================================================================== -auto MockTask::Active::pending_phases() const -> std::vector +auto MockTask::Active::pending_phases() const +-> const std::vector& { return _pending_phases; } diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp index 9e288ce4..96c89497 100644 --- a/rmf_task/test/mock/MockTask.hpp +++ b/rmf_task/test/mock/MockTask.hpp @@ -40,7 +40,7 @@ class MockTask : public rmf_task::Task Phase::ConstActivePtr active_phase() const override; - std::vector pending_phases() const override; + const std::vector& pending_phases() const override; const ConstTagPtr& tag() const override; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 9695c21a..0b897c8e 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -118,7 +118,7 @@ class Task::Active Phase::ConstActivePtr active_phase() const final; // Documentation inherited - std::vector pending_phases() const final; + const std::vector& pending_phases() const final; // Documentation inherited const ConstTagPtr& tag() const final; @@ -212,7 +212,7 @@ class Task::Active std::function _task_finished; std::list _pending_stages; - std::vector _pending_phases; + std::vector _pending_phases; ConstStagePtr _active_stage; Phase::ActivePtr _active_phase; @@ -358,12 +358,12 @@ void Task::Active::_load_backup(std::string backup_state_str) continue; } - while (pending_it != pending_end && (*pending_it)->tag()->id() < id) + while (pending_it != pending_end && pending_it->tag()->id() < id) { ++pending_it; } - if (pending_it == pending_end || id < (*pending_it)->tag()->id()) + if (pending_it == pending_end || id < pending_it->tag()->id()) { // This shouldn't happen, but it's not a critical error. In the worst // case, the operator needs to resend a skip command. @@ -372,7 +372,7 @@ void Task::Active::_load_backup(std::string backup_state_str) continue; } - (*pending_it)->will_be_skipped(true); + pending_it->will_be_skipped(true); } } @@ -386,12 +386,10 @@ void Task::Active::_generate_pending_phases() _pending_phases.reserve(_pending_stages.size()); for (const auto& s : _pending_stages) { - _pending_phases.push_back( - std::make_shared( - std::make_shared( - s->id, - s->description->generate_header(state, *_parameters) - ) + _pending_phases.emplace_back( + std::make_shared( + s->id, + s->description->generate_header(state, *_parameters) ) ); } @@ -423,10 +421,10 @@ void Task::Active::_begin_next_stage(std::optional restore) assert(!_pending_phases.empty()); _active_stage = _pending_stages.front(); - assert(_active_stage->id == _pending_phases.front()->tag()->id()); + assert(_active_stage->id == _pending_phases.front().tag()->id()); _pending_stages.pop_front(); - auto tag = _pending_phases.front()->tag(); + auto tag = _pending_phases.front().tag(); _pending_phases.erase(_pending_phases.begin()); // Reset our memory of phase backup sequence number @@ -524,8 +522,8 @@ auto Task::Active::_generate_backup( std::vector skipping_phases; for (const auto& p : _pending_phases) { - if (p->will_be_skipped()) - skipping_phases.push_back(p->tag()->id()); + if (p.will_be_skipped()) + skipping_phases.push_back(p.tag()->id()); } nlohmann::json root; From 11831e0ca4a51a88a52b265e22a77519c42efb0f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Oct 2021 10:46:15 +0800 Subject: [PATCH 50/85] Remember to include pt. 2 Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/VersionedString.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmf_task/include/rmf_task/VersionedString.hpp b/rmf_task/include/rmf_task/VersionedString.hpp index 10d9b1a1..337a3aaa 100644 --- a/rmf_task/include/rmf_task/VersionedString.hpp +++ b/rmf_task/include/rmf_task/VersionedString.hpp @@ -20,6 +20,8 @@ #include +#include + namespace rmf_task { //============================================================================== From 52f15bb0f01912dcb0dcd799f9022820b47e383c Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 27 Oct 2021 20:36:32 +0800 Subject: [PATCH 51/85] Implemented PerformAction event Signed-off-by: Yadunund --- rmf_task_sequence/CMakeLists.txt | 2 + .../events/PerformAction.hpp | 111 ++++++++ .../events/PerformAction.cpp | 252 ++++++++++++++++++ .../src/rmf_task_sequence/events/utils.cpp | 40 +++ .../src/rmf_task_sequence/events/utils.hpp | 44 +++ 5 files changed, 449 insertions(+) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index bb360087..0fcfdf6f 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -30,6 +30,8 @@ find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Task Sequence Library file(GLOB_RECURSE lib_srcs "src/rmf_task_sequence/*.cpp" + "src/rmf_task_sequence/phases/*.cpp" + "src/rmf_task_sequence/events/*.cpp" ) add_library(rmf_task_sequence SHARED diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp new file mode 100644 index 00000000..656cd24f --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp @@ -0,0 +1,111 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__PERFORMACTION_HPP +#define RMF_TASK_SEQUENCE__EVENTS__PERFORMACTION_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class PerformAction +{ +public: + using Location = rmf_traffic::agv::Plan::Goal; + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class PerformAction::Description : public Event::Description +{ +public: + + /// Make a PerformAction description. + /// + /// \param[in] action_name + /// A name for this action + /// + /// \param[in] action_duration_estimate + /// An estimate for how long it will take for the action to complete + /// + /// \param[in] use_tool_sink + /// If true, battery drain from peripheral tools will be accounted for + /// while performing the action + /// + /// \param[in] expected_finish_location + /// An optional location to indicate where the robot will end up after + /// performing the action. Use nullopt to indicate that after performing + /// the action, the robot will be at its initial location. + static DescriptionPtr make( + const std::string& action_name, + rmf_traffic::Duration action_duration_estimate, + bool use_tool_sink, + std::optional expected_finish_location = std::nullopt); + + /// Get the action name + const std::string& action_name() const; + + /// Set the action name + Description& action_name(const std::string& new_name); + + /// Get the action duration estimate + const rmf_traffic::Duration& action_duration_estimate() const; + + /// Set the action duration estimate + Description& action_duration_estimate(rmf_traffic::Duration new_duration); + + /// Check whether to account for battery drain from tools + bool use_tool_sink() const; + + /// Set whether to account for battery drain from tools + Description& use_tool_sink(bool use_tool); + + /// Get the expected finish location + std::optional expected_finish_location() const; + + /// Set the expected finish location + Description& expected_finish_location(std::optional new_location); + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__PERFORMACTION_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp new file mode 100644 index 00000000..bdb2aacb --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -0,0 +1,252 @@ +/* + * Copyright (C) 2021 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 + +#include "utils.hpp" + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class PerformAction::Model : public Activity::Model +{ +public: + + Model( + rmf_task::State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + bool use_tool_sink, + const Parameters& parameters); + + std::optional estimate_finish( + rmf_task::State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + rmf_traffic::Duration invariant_duration() const final; + + State invariant_finish_state() const final; + +private: + rmf_task::State _invariant_finish_state; + double _invariant_battery_drain; + rmf_traffic::Duration _invariant_duration; + bool _use_tool_sink; +}; + +//============================================================================== +PerformAction::Model::Model( + rmf_task::State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + bool use_tool_sink, + const Parameters& parameters) +: _invariant_finish_state(std::move(invariant_finish_state)), + _invariant_duration(invariant_duration), + _use_tool_sink(use_tool_sink) +{ + _invariant_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_invariant_duration)); + + if (_use_tool_sink && parameters.tool_sink() != nullptr) + { + _invariant_battery_drain += + parameters.tool_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_invariant_duration)); + } +} + +//============================================================================== +std::optional PerformAction::Model::estimate_finish( + rmf_task::State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const +{ + initial_state.time(initial_state.time().value() + _invariant_duration); + initial_state.waypoint(_invariant_finish_state.waypoint().value()); + if (_invariant_finish_state.orientation().has_value()) + initial_state.orientation(_invariant_finish_state.orientation().value()); + + if (constraints.drain_battery()) + initial_state.battery_soc( + initial_state.battery_soc().value() - _invariant_battery_drain); + + if (initial_state.battery_soc().value() <= constraints.threshold_soc()) + return std::nullopt; + + return initial_state; +} + +//============================================================================== +rmf_traffic::Duration PerformAction::Model::invariant_duration() const +{ + return _invariant_duration; +} + +//============================================================================== +State PerformAction::Model::invariant_finish_state() const +{ + return _invariant_finish_state; +} + +//============================================================================== +class PerformAction::Description::Implementation +{ +public: + + std::string action_name; + rmf_traffic::Duration action_duration_estimate; + bool use_tool_sink; + std::optional expected_finish_location; +}; + +//============================================================================== +auto PerformAction::Description::make( + const std::string& action_name, + rmf_traffic::Duration duration, + bool use_tool_sink, + std::optional location) -> DescriptionPtr +{ + auto description = std::shared_ptr(new Description); + description->_pimpl = rmf_utils::make_impl( + Implementation + { + std::move(action_name), + std::move(duration), + std::move(use_tool_sink), + std::move(location) + }); + + return description; +} + +//============================================================================== +PerformAction::Description::Description() +{ + // Do nothing +} + +//============================================================================== +const std::string& +PerformAction::Description::action_name() const +{ + return _pimpl->action_name; +} + +//============================================================================== +auto PerformAction::Description::action_name( + const std::string& new_name) -> Description& +{ + _pimpl->action_name = new_name; + return *this; +} + +//============================================================================== +const rmf_traffic::Duration& +PerformAction::Description::action_duration_estimate() const +{ + return _pimpl->action_duration_estimate; +} + +//============================================================================== +auto PerformAction::Description::action_duration_estimate( + rmf_traffic::Duration new_duration) -> Description& +{ + _pimpl->action_duration_estimate = std::move(new_duration); + return *this; +} + +//============================================================================== +bool PerformAction::Description::use_tool_sink() const +{ + return _pimpl->use_tool_sink; +} + +//============================================================================== +auto PerformAction::Description::use_tool_sink( + bool use_tool) -> Description& +{ + _pimpl->use_tool_sink = std::move(use_tool); + return *this; +} + +//============================================================================== +auto PerformAction::Description::expected_finish_location() const +-> std::optional +{ + return _pimpl->expected_finish_location; +} + +//============================================================================== +auto PerformAction::Description::expected_finish_location( + std::optional new_location) -> Description& +{ + _pimpl->expected_finish_location = std::move(new_location); + return *this; +} + +//============================================================================== +Activity::ConstModelPtr PerformAction::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + auto invariant_finish_state = invariant_initial_state; + if (_pimpl->expected_finish_location.has_value()) + { + const auto& goal = _pimpl->expected_finish_location.value(); + invariant_finish_state.waypoint(goal.waypoint()); + if (goal.orientation() != nullptr) + { + invariant_finish_state.orientation(*goal.orientation()); + } + } + + return std::make_shared( + invariant_finish_state, + _pimpl->action_duration_estimate, + _pimpl->use_tool_sink, + parameters); +} + +//============================================================================== +Header PerformAction::Description::generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const +{ + + const std::string& error_header = + "[PerformAction::Description::generate_header]"; + + const auto start_wp_opt = initial_state.waypoint(); + if (!start_wp_opt) + utils::fail( + error_header, + "Initial state is missing a waypoint"); + + const auto start_wp = *start_wp_opt; + const auto start_name = utils::waypoint_name(start_wp, parameters); + + return Header( + "Perform " + _pimpl->action_name + " action", + "Performing " + _pimpl->action_name + " at waypoint [" + start_name + "]", + _pimpl->action_duration_estimate); + +} + +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp new file mode 100644 index 00000000..6bbda90f --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2021 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 "utils.hpp" + +namespace rmf_task_sequence { +namespace events { +namespace utils { + +std::string waypoint_name( + const std::size_t index, + const rmf_task::Parameters& parameters) +{ + const auto& graph = parameters.planner()->get_configuration().graph(); + if (index < graph.num_waypoints()) + { + if (const auto* name = graph.get_waypoint(index).name()) + return *name; + } + + return "#" + std::to_string(index); +} + +} // namespace utils +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp new file mode 100644 index 00000000..9259cfca --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp @@ -0,0 +1,44 @@ + +/* + * Copyright (C) 2021 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__EVENTS__UTILS_HPP +#define SRC__RMF_TASK_SEQUENCE__EVENTS__UTILS_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { +namespace utils { + +std::string waypoint_name( + const std::size_t index, + const rmf_task::Parameters& parameters); + +const auto fail = [](const std::string& header, const std::string& msg) + { + throw std::runtime_error( + header + " " + msg); + }; + +} // namespace utils +} // namespace events +} // namespace rmf_task_sequence + +# endif // SRC__RMF_TASK_SEQUENCE__EVENTS__UTILS_HPP From 3a660be440d5bd4d4947231f54fbc4dbbe794ff1 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 27 Oct 2021 20:41:17 +0800 Subject: [PATCH 52/85] Update GoToPlace to use utils Signed-off-by: Yadunund --- .../rmf_task_sequence/events/GoToPlace.cpp | 34 +++++-------------- 1 file changed, 9 insertions(+), 25 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp index ef7c5a0f..4b778168 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp @@ -17,6 +17,8 @@ #include +#include "utils.hpp" + namespace rmf_task_sequence { namespace events { @@ -171,20 +173,6 @@ class GoToPlace::Description::Implementation public: rmf_traffic::agv::Plan::Goal destination; - - static std::string waypoint_name( - const std::size_t index, - const Parameters& parameters) - { - const auto& graph = parameters.planner()->get_configuration().graph(); - if (index < graph.num_waypoints()) - { - if (const auto* name = graph.get_waypoint(index).name()) - return *name; - } - - return "#" + std::to_string(index); - } }; //============================================================================== @@ -215,31 +203,27 @@ Header GoToPlace::Description::generate_header( const State& initial_state, const Parameters& parameters) const { - const auto fail = [](const std::string& msg) - { - throw std::runtime_error( - "[GoToPlace::Description::generate_header] " + msg); - }; + const std::string& fail_header = "[GoToPlace::Description::generate_header]"; const auto start_wp_opt = initial_state.waypoint(); if (!start_wp_opt) - fail("Initial state is missing a waypoint"); + utils::fail(fail_header, "Initial state is missing a waypoint"); const auto start_wp = *start_wp_opt; const auto& graph = parameters.planner()->get_configuration().graph(); if (graph.num_waypoints() <= start_wp) { - fail("Initial waypoint [" + std::to_string(start_wp) + utils::fail(fail_header, "Initial waypoint [" + std::to_string(start_wp) + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + "]"); } - const auto start_name = Implementation::waypoint_name(start_wp, parameters); + const auto start_name = utils::waypoint_name(start_wp, parameters); if (graph.num_waypoints() <= _pimpl->destination.waypoint()) { - fail("Destination waypoint [" + utils::fail(fail_header, "Destination waypoint [" + std::to_string(_pimpl->destination.waypoint()) + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + "]"); @@ -252,7 +236,7 @@ Header GoToPlace::Description::generate_header( if (!estimate.has_value()) { - fail("Cannot find a path from [" + utils::fail(fail_header, "Cannot find a path from [" + start_name + "] to [" + goal_name_ + "]"); } @@ -279,7 +263,7 @@ auto GoToPlace::Description::destination(Goal new_goal) -> Description& std::string GoToPlace::Description::destination_name( const Parameters& parameters) const { - return Implementation::waypoint_name( + return utils::waypoint_name( _pimpl->destination.waypoint(), parameters); } From 436b6fdd27e1daec4b5195c58bb50af6fdb18a3f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Oct 2021 18:06:50 +0800 Subject: [PATCH 53/85] Added call and sms events Signed-off-by: Yadunund --- .../rmf_task_sequence/detail/ContactCard.hpp | 93 +++++++++ .../include/rmf_task_sequence/events/Call.hpp | 88 ++++++++ .../include/rmf_task_sequence/events/SMS.hpp | 98 +++++++++ .../rmf_task_sequence/events/WaitFor.hpp | 8 +- .../rmf_task_sequence/detail/ContactCard.cpp | 104 ++++++++++ .../src/rmf_task_sequence/events/Call.cpp | 177 ++++++++++++++++ .../src/rmf_task_sequence/events/SMS.cpp | 192 ++++++++++++++++++ 7 files changed, 756 insertions(+), 4 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/Call.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/SMS.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp new file mode 100644 index 00000000..9c56c92b --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__DETAIL__CONTACTCARD_HPP +#define RMF_TASK_SEQUENCE__DETAIL__CONTACTCARD_HPP + +#include + +namespace rmf_task_sequence { +namespace detail { + +//============================================================================== +struct PhoneNumber +{ + /// The country code without "+"" + uint32_t country_code; + + /// The phone number + uint32_t number; +}; + +//============================================================================== +/// Store contact details for telecommunication +class ContactCard +{ +public: + + /// Constructor + /// + /// \param[in] name + /// The name of the contact + /// + /// \param[in] address + /// The physical address of the contact + /// + /// \param[in] email + /// The email address of the contact + /// + /// \param[in] country_code + /// The country code + ContactCard( + const std::string& name, + const std::string& address, + const std::string& email, + PhoneNumber number); + + /// Get the name + const std::string& name() const; + + /// Set the name + ContactCard& name(const std::string& new_name); + + /// Get the address + const std::string& address() const; + + /// Set the address + ContactCard& address(const std::string& new_address); + + /// Get the email + const std::string& email() const; + + /// Set the email + ContactCard& email(const std::string& new_email); + + /// Get the phone number + const PhoneNumber& number() const; + + /// Set the phone number + ContactCard& number(PhoneNumber new_number); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace detail +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__CONTACTCARD_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Call.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Call.hpp new file mode 100644 index 00000000..6247000d --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Call.hpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__CALL_HPP +#define RMF_TASK_SEQUENCE__EVENTS__CALL_HPP + +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// Make a phone call +class Call +{ +public: + using ContactCard = detail::ContactCard; + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class Call::Description : public Event::Description +{ +public: + + /// Make a Call description. + /// + /// \param[in] contact + /// The contact of the entity to call + /// + /// \param[in] call_duration_estimate + /// An estimate for how long it will take for the call to complete + static DescriptionPtr make( + ContactCard contact, + rmf_traffic::Duration call_duration_estimate); + + /// Get the contact + const ContactCard& contact() const; + + /// Set the contact + Description& contact(ContactCard new_contact); + + /// Get the call duration estimate + rmf_traffic::Duration call_duration_estimate() const; + + /// Set the action duration estimate + Description& call_duration_estimate(rmf_traffic::Duration new_duration); + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__CALL_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/SMS.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/SMS.hpp new file mode 100644 index 00000000..8fdf7fa2 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/SMS.hpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__SMS_HPP +#define RMF_TASK_SEQUENCE__EVENTS__SMS_HPP + +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// Send an SMS +class SMS +{ +public: + using ContactCard = detail::ContactCard; + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class SMS::Description : public Event::Description +{ +public: + + /// Make a SMS description. + /// + /// \param[in] message + /// The message to send + /// + /// \param[in] contact + /// The contact of the entity to SMS + /// + /// \param[in] sms_duration_estimate + /// An estimate for how long it will take to send the SMS + static DescriptionPtr make( + const std::string& message, + ContactCard contact, + rmf_traffic::Duration sms_duration_estimate); + + /// Get the message + const std::string& message() const; + + /// Set the message + Description& message(const std::string& new_message); + + /// Get the contact + const ContactCard& contact() const; + + /// Set the contact + Description& contact(ContactCard new_contact); + + /// Get the SMS duration estimate + rmf_traffic::Duration sms_duration_estimate() const; + + /// Set the action duration estimate + Description& sms_duration_estimate(rmf_traffic::Duration new_duration); + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__SMS_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp index 864c85af..03b56ba9 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp @@ -20,16 +20,16 @@ #include -#include +#include namespace rmf_task_sequence { namespace events { //============================================================================== -/// A WaitFor phase encompasses having the robot sit in place and wait for a +/// A WaitFor event encompasses having the robot sit in place and wait for a /// period of time to pass. /// -/// The Model of this phase may be useful for calculating the Models of other +/// The Model of this event may be useful for calculating the Models of other /// phases that include a period of time where the robot is waiting for a /// process to finish. E.g. the PickUp and DropOff Models use WaitFor::Model to /// calculate how much the robot's battery drains while waiting for the payload @@ -46,7 +46,7 @@ class WaitFor }; //============================================================================== -class WaitFor::Description : public Phase::Description +class WaitFor::Description : public Event::Description { public: diff --git a/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp b/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp new file mode 100644 index 00000000..d7cf8394 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { +namespace detail{ + +//============================================================================== +class ContactCard::Implementation +{ +public: + + std::string name; + std::string address; + std::string email; + PhoneNumber number; +}; + +//============================================================================== +ContactCard::ContactCard( + const std::string& name, + const std::string& address, + const std::string& email, + PhoneNumber number) +: _pimpl(rmf_utils::make_impl( + Implementation{ + name, + address, + email, + std::move(number) + })) +{ + // Do nothing +} + +//============================================================================== +const std::string& ContactCard::name() const +{ + return _pimpl->name; +} + +//============================================================================== +ContactCard& ContactCard::name(const std::string& new_name) +{ + _pimpl->name = new_name; + return *this; +} + +//============================================================================== +const std::string& ContactCard::address() const +{ + return _pimpl->address; +} + +//============================================================================== +ContactCard& ContactCard::address(const std::string& new_address) +{ + _pimpl->address = new_address; + return *this; +} + +//============================================================================== +const std::string& ContactCard::email() const +{ + return _pimpl->email; +} + +//============================================================================== +ContactCard& ContactCard::email(const std::string& new_email) +{ + _pimpl->email = new_email; + return *this; +} + +//============================================================================== +const PhoneNumber& ContactCard::number() const +{ + return _pimpl->number; +} + +//============================================================================== +ContactCard& ContactCard::number(PhoneNumber new_number) +{ + _pimpl->number = std::move(new_number); + return *this; +} + +} // namespace detail +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp new file mode 100644 index 00000000..c0e19646 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2021 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 +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class Call::Model : public Activity::Model +{ +public: + + Model( + rmf_task::State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + const Parameters& parameters); + + /// Documentation inherited + std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + /// Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + /// Documentation inherited + State invariant_finish_state() const final; + +private: + rmf_task::State _invariant_finish_state; + double _invariant_battery_drain; + rmf_traffic::Duration _invariant_duration; +}; + +//============================================================================== +Call::Model::Model( + State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + const Parameters& parameters) +: _invariant_finish_state(std::move(invariant_finish_state)), + _invariant_duration(invariant_duration) +{ + _invariant_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_invariant_duration)); +} + +//============================================================================== +std::optional Call::Model::estimate_finish( + State state, + const Constraints& constraints, + const TravelEstimator&) const +{ + state.time(state.time().value() + _invariant_duration); + + if (constraints.drain_battery()) + state.battery_soc(state.battery_soc().value() - _invariant_battery_drain); + + if (state.battery_soc().value() <= constraints.threshold_soc()) + return std::nullopt; + + return state; +} + +//============================================================================== +rmf_traffic::Duration Call::Model::invariant_duration() const +{ + return _invariant_duration; +} + +//============================================================================== +State Call::Model::invariant_finish_state() const +{ + return _invariant_finish_state; +} + +//============================================================================== +class Call::Description::Implementation +{ +public: + + ContactCard contact; + rmf_traffic::Duration call_duration_estimate; + +}; + +//============================================================================== +auto Call::Description::make( + ContactCard contact, + rmf_traffic::Duration wait_duration) -> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = + rmf_utils::make_impl( + Implementation{ + std::move(contact), + wait_duration}); + + return output; +} + +//============================================================================== +auto Call::Description::contact() const -> const ContactCard& +{ + return _pimpl->contact; +} + +//============================================================================== +auto Call::Description::contact( + ContactCard new_contact)-> Description& +{ + _pimpl->contact = std::move(new_contact); + return *this; +} + +//============================================================================== +rmf_traffic::Duration Call::Description::call_duration_estimate() const +{ + return _pimpl->call_duration_estimate; +} + +//============================================================================== +auto Call::Description::call_duration_estimate( + rmf_traffic::Duration new_duration) -> Description& +{ + _pimpl->call_duration_estimate = new_duration; + return *this; +} + +//============================================================================== +Activity::ConstModelPtr Call::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return std::make_shared( + invariant_initial_state, _pimpl->call_duration_estimate, parameters); +} + +//============================================================================== +Header Call::Description::generate_header( + const State&, const Parameters&) const +{ + const std::string& number = "+" + + std::to_string(_pimpl->contact.number().country_code) + + " " + std::to_string(_pimpl->contact.number().number); + + return Header( + "Calling", + "Calling " + _pimpl->contact.name() + " at " + number, + _pimpl->call_duration_estimate); +} + +//============================================================================== +Call::Description::Description() +{ + // Do nothing +} + +} // namespace phases +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp new file mode 100644 index 00000000..cb5221da --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp @@ -0,0 +1,192 @@ +/* + * Copyright (C) 2021 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 +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class SMS::Model : public Activity::Model +{ +public: + + Model( + rmf_task::State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + const Parameters& parameters); + + /// Documentation inherited + std::optional estimate_finish( + State initial_state, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const final; + + /// Documentation inherited + rmf_traffic::Duration invariant_duration() const final; + + /// Documentation inherited + State invariant_finish_state() const final; + +private: + rmf_task::State _invariant_finish_state; + double _invariant_battery_drain; + rmf_traffic::Duration _invariant_duration; +}; + +//============================================================================== +SMS::Model::Model( + State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + const Parameters& parameters) +: _invariant_finish_state(std::move(invariant_finish_state)), + _invariant_duration(invariant_duration) +{ + _invariant_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_invariant_duration)); +} + +//============================================================================== +std::optional SMS::Model::estimate_finish( + State state, + const Constraints& constraints, + const TravelEstimator&) const +{ + state.time(state.time().value() + _invariant_duration); + + if (constraints.drain_battery()) + state.battery_soc(state.battery_soc().value() - _invariant_battery_drain); + + if (state.battery_soc().value() <= constraints.threshold_soc()) + return std::nullopt; + + return state; +} + +//============================================================================== +rmf_traffic::Duration SMS::Model::invariant_duration() const +{ + return _invariant_duration; +} + +//============================================================================== +State SMS::Model::invariant_finish_state() const +{ + return _invariant_finish_state; +} + +//============================================================================== +class SMS::Description::Implementation +{ +public: + + std::string message; + ContactCard contact; + rmf_traffic::Duration sms_duration_estimate; + +}; + +//============================================================================== +auto SMS::Description::make( + const std::string& message, + ContactCard contact, + rmf_traffic::Duration wait_duration) -> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = + rmf_utils::make_impl( + Implementation{ + message, + std::move(contact), + wait_duration}); + + return output; +} + +//============================================================================== +const std::string& SMS::Description::message() const +{ + return _pimpl->message; +} + +//============================================================================== +auto SMS::Description::message(const std::string& new_message) -> Description& +{ + _pimpl->message = std::move(new_message); + return *this; +} +//============================================================================== +auto SMS::Description::contact() const -> const ContactCard& +{ + return _pimpl->contact; +} + +//============================================================================== +auto SMS::Description::contact( + ContactCard new_contact)-> Description& +{ + _pimpl->contact = std::move(new_contact); + return *this; +} + +//============================================================================== +rmf_traffic::Duration SMS::Description::sms_duration_estimate() const +{ + return _pimpl->sms_duration_estimate; +} + +//============================================================================== +auto SMS::Description::sms_duration_estimate( + rmf_traffic::Duration new_duration) -> Description& +{ + _pimpl->sms_duration_estimate = new_duration; + return *this; +} + +//============================================================================== +Activity::ConstModelPtr SMS::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return std::make_shared( + invariant_initial_state, _pimpl->sms_duration_estimate, parameters); +} + +//============================================================================== +Header SMS::Description::generate_header( + const State&, const Parameters&) const +{ + const std::string& number = "+" + + std::to_string(_pimpl->contact.number().country_code) + + " " + std::to_string(_pimpl->contact.number().number); + + return Header( + "Sending SMS", + "Sending SMS to " + _pimpl->contact.name() + " at " + number, + _pimpl->sms_duration_estimate); +} + +//============================================================================== +SMS::Description::Description() +{ + // Do nothing +} + +} // namespace phases +} // namespace rmf_task_sequence From c720ffb7f3d7816fccb5a8d28eaff666d62d6029 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 1 Nov 2021 18:13:44 +0800 Subject: [PATCH 54/85] Refactoring Event to State, Standby, and Active, and implementing Event Sequence Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Activator.hpp | 8 +- rmf_task/include/rmf_task/Event.hpp | 18 +- rmf_task/include/rmf_task/Phase.hpp | 4 +- .../include/rmf_task/events/SimpleEvent.hpp | 8 +- .../include/rmf_task/phases/RestoreBackup.hpp | 2 +- rmf_task/src/rmf_task/Condition.cpp | 12 +- rmf_task/src/rmf_task/Phase.cpp | 4 +- rmf_task/src/rmf_task/events/SimpleEvent.cpp | 8 +- .../src/rmf_task/phases/RestoreBackup.cpp | 2 +- rmf_task/test/mock/MockEvent.cpp | 4 +- rmf_task/test/mock/MockEvent.hpp | 4 +- rmf_task/test/mock/MockPhase.cpp | 2 +- rmf_task/test/mock/MockPhase.hpp | 2 +- .../include/rmf_task_sequence/Activity.hpp | 2 +- .../include/rmf_task_sequence/Event.hpp | 130 ++++++++++++- .../rmf_task_sequence/detail/impl_Event.hpp | 47 +++++ .../rmf_task_sequence/events/Sequence.hpp | 107 +++++++++++ .../src/rmf_task_sequence/Event.cpp | 65 +++++++ .../src/rmf_task_sequence/Phase.cpp | 7 + .../src/rmf_task_sequence/events/Sequence.cpp | 179 ++++++++++++++++++ 20 files changed, 569 insertions(+), 46 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/Event.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp diff --git a/rmf_task/include/rmf_task/Activator.hpp b/rmf_task/include/rmf_task/Activator.hpp index 41e5c0e5..1e07a509 100644 --- a/rmf_task/include/rmf_task/Activator.hpp +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -34,13 +34,13 @@ class Activator /// Signature for activating a task /// /// \tparam Description - /// A class that implements the Request::Description interface + /// A class that implements the Task::Description interface /// /// \param[in] get_state /// A callback for retrieving the current state of the robot /// /// \param[in] parameters - /// A reference to the parameters for the roboth + /// A reference to the parameters for the robot /// /// \param[in] booking /// An immutable reference to the booking information for the task @@ -80,7 +80,7 @@ class Activator std::function task_finished) >; - /// Add a callback to convert from a Request into an active Task. + /// Add a callback to convert from a Description into an active Task. /// /// \tparam Description /// A class that implements the Request::Description interface @@ -90,7 +90,7 @@ class Activator template void add_activator(Activate activator); - /// Activate a Task object based on a Request::Description. + /// Activate a Task object based on a Request. /// /// \param[in] get_state /// A callback for retrieving the current state of the robot diff --git a/rmf_task/include/rmf_task/Event.hpp b/rmf_task/include/rmf_task/Event.hpp index 18519f05..6f56d911 100644 --- a/rmf_task/include/rmf_task/Event.hpp +++ b/rmf_task/include/rmf_task/Event.hpp @@ -81,8 +81,8 @@ class Event Finished }; - class Active; - using ConstActivePtr = std::shared_ptr; + class State; + using ConstStatePtr = std::shared_ptr; class Snapshot; using ConstSnapshotPtr = std::shared_ptr; @@ -91,12 +91,12 @@ class Event //============================================================================== /// The interface to an active event. -class Event::Active +class Event::State { public: using Status = Event::Status; - using ConstActivePtr = Event::ConstActivePtr; + using ConstStatePtr = Event::ConstStatePtr; /// The current Status of this event virtual Status status() const = 0; @@ -115,10 +115,10 @@ class Event::Active virtual Log::View log() const = 0; /// Get more granular dependencies of this event, if any exist. - virtual std::vector dependencies() const = 0; + virtual std::vector dependencies() const = 0; // Virtual destructor - virtual ~Active() = default; + virtual ~State() = default; }; //============================================================================== @@ -126,12 +126,12 @@ class Event::Active /// original event is arbitrarily changed, and there is no risk of a race /// condition, as long as the snapshot is not being created while the event /// is changing. -class Event::Snapshot : public Event::Active +class Event::Snapshot : public Event::State { public: /// Make a snapshot of the current state of an Event - static ConstSnapshotPtr make(const Active& other); + static ConstSnapshotPtr make(const State& other); // Documentation inherited Status status() const final; @@ -146,7 +146,7 @@ class Event::Snapshot : public Event::Active Log::View log() const final; // Documentation inherited - std::vector dependencies() const final; + std::vector dependencies() const final; class Implementation; private: diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 417e4801..9eed8766 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -115,7 +115,7 @@ class Phase::Active virtual ConstTagPtr tag() const = 0; /// The Event that needs to finish for this phase to be complete - virtual Event::ConstActivePtr final_event() const = 0; + virtual Event::ConstStatePtr final_event() const = 0; /// The estimated finish time for this phase virtual rmf_traffic::Time estimate_finish_time() const = 0; @@ -136,7 +136,7 @@ class Phase::Snapshot : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - Event::ConstActivePtr final_event() const final; + Event::ConstStatePtr final_event() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; diff --git a/rmf_task/include/rmf_task/events/SimpleEvent.hpp b/rmf_task/include/rmf_task/events/SimpleEvent.hpp index b522a3ba..96364868 100644 --- a/rmf_task/include/rmf_task/events/SimpleEvent.hpp +++ b/rmf_task/include/rmf_task/events/SimpleEvent.hpp @@ -32,7 +32,7 @@ namespace events { /// satisfy the Phase interface's finish_event() function. Your Phase /// implementation can create an instance of this class and then manage its /// fields directly. -class SimpleEvent : public Event::Active +class SimpleEvent : public Event::State { public: @@ -40,7 +40,7 @@ class SimpleEvent : public Event::Active std::string name, std::string detail, Status initial_status, - std::vector dependencies = {}); + std::vector dependencies = {}); // Documentation inherited Status status() const final; @@ -67,11 +67,11 @@ class SimpleEvent : public Event::Active Log& update_log(); // Documentation inherited - std::vector dependencies() const final; + std::vector dependencies() const final; /// Update the dependencies SimpleEvent& update_dependencies( - std::vector new_dependencies); + std::vector new_dependencies); class Implementation; private: diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index 9fa4ac32..acfe61f1 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -50,7 +50,7 @@ class RestoreBackup::Active : public Phase::Active ConstTagPtr tag() const final; // Documentation inherited - Event::ConstActivePtr final_event() const final; + Event::ConstStatePtr final_event() const final; // Documentation inherited rmf_traffic::Time estimate_finish_time() const final; diff --git a/rmf_task/src/rmf_task/Condition.cpp b/rmf_task/src/rmf_task/Condition.cpp index cd1a52c7..542dbd14 100644 --- a/rmf_task/src/rmf_task/Condition.cpp +++ b/rmf_task/src/rmf_task/Condition.cpp @@ -21,14 +21,14 @@ namespace rmf_task { namespace { //============================================================================== -std::vector snapshot_dependencies( - const std::vector& queue) +std::vector snapshot_dependencies( + const std::vector& queue) { // NOTE(MXG): This implementation is using recursion. That should be fine // since I don't expect much depth in the trees of dependencies, but we may // want to revisit this and implement it as a queue instead if we ever find // a use-case with deep recursion. - std::vector output; + std::vector output; output.reserve(queue.size()); for (const auto& c : queue) output.push_back(Event::Snapshot::make(*c)); @@ -46,12 +46,12 @@ class Event::Snapshot::Implementation VersionedString::View name; VersionedString::View detail; Log::View log; - std::vector dependencies; + std::vector dependencies; }; //============================================================================== -auto Event::Snapshot::make(const Active& other) -> ConstSnapshotPtr +auto Event::Snapshot::make(const State& other) -> ConstSnapshotPtr { Snapshot output; output._pimpl = rmf_utils::make_impl( @@ -91,7 +91,7 @@ Log::View Event::Snapshot::log() const } //============================================================================== -std::vector Event::Snapshot::dependencies() const +std::vector Event::Snapshot::dependencies() const { return _pimpl->dependencies; } diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index 123360c4..b86e7065 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -102,7 +102,7 @@ class Phase::Snapshot::Implementation { public: ConstTagPtr tag; - Event::ConstActivePtr finish_event; + Event::ConstStatePtr finish_event; rmf_traffic::Time estimated_finish_time; }; @@ -127,7 +127,7 @@ Phase::ConstTagPtr Phase::Snapshot::tag() const } //============================================================================== -Event::ConstActivePtr Phase::Snapshot::final_event() const +Event::ConstStatePtr Phase::Snapshot::final_event() const { return _pimpl->finish_event; } diff --git a/rmf_task/src/rmf_task/events/SimpleEvent.cpp b/rmf_task/src/rmf_task/events/SimpleEvent.cpp index e53055d8..205d1f28 100644 --- a/rmf_task/src/rmf_task/events/SimpleEvent.cpp +++ b/rmf_task/src/rmf_task/events/SimpleEvent.cpp @@ -29,7 +29,7 @@ class SimpleEvent::Implementation VersionedString name; VersionedString detail; Log log; - std::vector dependencies; + std::vector dependencies; }; @@ -37,7 +37,7 @@ class SimpleEvent::Implementation std::shared_ptr SimpleEvent::make(std::string name, std::string detail, Event::Status initial_status, - std::vector dependencies) + std::vector dependencies) { SimpleEvent output; output._pimpl = rmf_utils::make_unique_impl( @@ -104,14 +104,14 @@ Log& SimpleEvent::update_log() } //============================================================================== -std::vector SimpleEvent::dependencies() const +std::vector SimpleEvent::dependencies() const { return _pimpl->dependencies; } //============================================================================== SimpleEvent& SimpleEvent::update_dependencies( - std::vector new_dependencies) + std::vector new_dependencies) { _pimpl->dependencies = new_dependencies; return *this; diff --git a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp index 8811816c..d1a54c1d 100644 --- a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -75,7 +75,7 @@ auto RestoreBackup::Active::tag() const -> ConstTagPtr } //============================================================================== -Event::ConstActivePtr RestoreBackup::Active::final_event() const +Event::ConstStatePtr RestoreBackup::Active::final_event() const { return _pimpl->event; } diff --git a/rmf_task/test/mock/MockEvent.cpp b/rmf_task/test/mock/MockEvent.cpp index a8d88df9..ba3f5e2f 100644 --- a/rmf_task/test/mock/MockEvent.cpp +++ b/rmf_task/test/mock/MockEvent.cpp @@ -56,9 +56,9 @@ rmf_task::Log::View MockEvent::log() const } //============================================================================== -std::vector MockEvent::dependencies() const +std::vector MockEvent::dependencies() const { - return std::vector( + return std::vector( _dependencies.begin(), _dependencies.end()); } diff --git a/rmf_task/test/mock/MockEvent.hpp b/rmf_task/test/mock/MockEvent.hpp index e0a1f44a..91d2e2da 100644 --- a/rmf_task/test/mock/MockEvent.hpp +++ b/rmf_task/test/mock/MockEvent.hpp @@ -23,7 +23,7 @@ namespace test_rmf_task { //============================================================================== -class MockEvent : public rmf_task::Event::Active +class MockEvent : public rmf_task::Event::State { public: @@ -37,7 +37,7 @@ class MockEvent : public rmf_task::Event::Active rmf_task::VersionedString::View name() const final; rmf_task::VersionedString::View detail() const final; rmf_task::Log::View log() const final; - std::vector dependencies() const final; + std::vector dependencies() const final; // Fields Status _status; diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index 79fbef4f..e0d537b7 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -42,7 +42,7 @@ rmf_task::Phase::ConstTagPtr MockPhase::Active::tag() const } //============================================================================== -rmf_task::Event::ConstActivePtr MockPhase::Active::final_event() const +rmf_task::Event::ConstStatePtr MockPhase::Active::final_event() const { return _event; } diff --git a/rmf_task/test/mock/MockPhase.hpp b/rmf_task/test/mock/MockPhase.hpp index 08912c78..85b59f5b 100644 --- a/rmf_task/test/mock/MockPhase.hpp +++ b/rmf_task/test/mock/MockPhase.hpp @@ -41,7 +41,7 @@ class MockPhase : public rmf_task::Phase std::function phase_finished_); ConstTagPtr tag() const final; - rmf_task::Event::ConstActivePtr final_event() const final; + rmf_task::Event::ConstStatePtr final_event() const final; rmf_traffic::Time estimate_finish_time() const final; void send_update() const; diff --git a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp index e728bf06..f4ae3272 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp @@ -173,7 +173,7 @@ class Activity::SequenceModel : public Activity::Model /// arguments that are given to Phase::Description::make_model(~). /// /// \param[in] descriptions - /// The Phase descriptions that are being modelled. The ordering of the + /// The Phase descriptions that are being modeled. The ordering of the /// descriptions may impact model outcomes. The order of the descriptions /// in the vector should reflect the actual order that the phases would /// occur in. diff --git a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp index a1a7b91e..5c14ed1e 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -34,17 +34,135 @@ class Event : public rmf_task::Event { public: - class Active : public Activity::Active {}; + // Event::Active simply uses the Activity::Active API + using Active = Activity::Active; using ActivePtr = std::shared_ptr; - class Activator; - using ActivatorPtr = std::shared_ptr; - using ConstActivatorPtr = std::shared_ptr; - - class Description : public Activity::Description {}; + // Event::Description simply uses the Activity::Description API + using Description = Activity::Description; using ConstDescriptionPtr = std::shared_ptr; + + class Standby; + using StandbyPtr = std::shared_ptr; + + class Initializer; + using InitializerPtr = std::shared_ptr; + using ConstInitializerPtr = std::shared_ptr; +}; + +//============================================================================== +/// The interface of an event that is in a standby mode. This interface is what +/// will be provided by the Event::Initializer. When the right conditions are +/// met for the event to begin, the owner of the event should trigger the +/// begin() function. +class Event::Standby +{ +public: + + /// Get the state of this Standby event + virtual const ConstStatePtr& state() const = 0; + + /// Estimate how long this event will take once it has started + virtual rmf_traffic::Duration duration_estimate() const = 0; + + /// Tell this event to begin. This function should be implemented to always + /// return the same Event::Active instance if it gets called more than once. + /// + /// \param[in] update + /// A callback that will be triggered when a notable change happens for this + /// event. + /// + /// \param[in] checkpoint + /// A callback that will be triggered when the event reaches a "checkpoint" + /// meaning that the task state should be backed up. + virtual ActivePtr begin( + std::function update, + std::function checkpoint) = 0; + + // Virtual destructor + virtual ~Standby() = default; +}; + +//============================================================================== +/// The Event::Initializer class is the Event equivalent to the +/// rmf_task::Activator class. It consumes an Event::Description and produces +/// +class Event::Initializer +{ +public: + + /// Construct an empty Initializer + Initializer(); + + /// Signature for initializing an Event + /// + /// \tparam Description + /// A class that implements the Event::Description interface + /// + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] description + /// The down-casted description of the event + /// + /// \param[in] backup_state + /// The serialized backup state of the Event, if the Event is being restored + /// from a crash or disconnection. If the Event is not being restored, a + /// std::nullopt will be passed in here. + /// + /// \return an Event in a Standby state + template + using Initialize = + std::function< + StandbyPtr( + std::function get_state, + const ConstParametersPtr& parameters, + const Description& description, + std::optional backup_state) + >; + + /// Add a callback to convert from a Description to an event in standby mode + template + void add_initializer(Initialize initializer); + + /// Initialize an event + /// + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] description + /// The description of the event + /// + /// \param[in] backup_state + /// The serialized backup state of the Event, if the Event is being restored + /// from a crash or disconnection. If the Event is not being restored, a + /// std::nullopt will be passed in here. + /// + /// \return an Event in a Standby state + StandbyPtr initialize( + std::function get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + std::optional backup_state); + + class Implementation; +private: + /// \private + void _add_initializer( + std::type_index type, + Initialize initializer); + + rmf_utils::impl_ptr _pimpl; }; } // namespace rmf_task_sequence +#include + #endif // RMF_TASK_SEQUENCE__EVENT_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp new file mode 100644 index 00000000..ee6a5494 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__DETAIL__IMPL_EVENT_HPP +#define RMF_TASK_SEQUENCE__DETAIL__IMPL_EVENT_HPP + +#include + +namespace rmf_task_sequence { + +//============================================================================== +template +void Event::Initializer::add_initializer(Initialize initializer) +{ + _add_initializer( + typeid(Desc), + [initializer]( + std::function get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + std::optional backup_state) + { + return initializer( + std::move(get_state), + parameters, + static_cast(description), + std::move(backup_state)); + }); +} + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__IMPL_EVENT_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp new file mode 100644 index 00000000..d21f2ad5 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__SEQUENCE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__SEQUENCE_HPP + +#include + +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class Sequence : public Event +{ +public: + + /// Make an initializer for Event Sequences + /// + /// \param[in] initializer + /// The Initializer that should be used to initialize other events + static Initializer::Initialize + make_initializer(ConstInitializerPtr initializer); + + class Description; +}; + +//============================================================================== +class Sequence::Description : public Event::Description +{ +public: + + using Elements = std::vector; + + /// Construct a Sequence Description + /// + /// \param[in] elements + /// These are the events that the sequence will run through. Each event in + /// the sequence will be in Standby mode until the previous one reaches a + /// finished status. + /// + /// \param[in] category + /// Optionally give a category to this sequence. If left unspecified, the + /// category will simply be "Sequence". + /// + /// \param[in] detail + /// Optionally give some detail to this sequence. If left unspecified, the + /// detail will simply aggregate the details of the dependencies. + Description( + Elements elements, + std::optional category = std::nullopt, + std::optional detail = std::nullopt); + + /// Get the elements of the sequence + const Elements& elements() const; + + /// Change the elements of the sequence + Description& elements(Elements new_elements); + + /// Get the category settings + const std::optional& category() const; + + /// Change the category settings + Description& category(std::optional new_category); + + /// Get the detail settings + const std::optional& detail() const; + + /// Change the detail settings + Description& detail(std::optional new_detail); + + // Documentation inherited + Activity::ConstModelPtr make_model( + rmf_task::State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + + +#endif // RMF_TASK_SEQUENCE__EVENTS__SEQUENCE_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp new file mode 100644 index 00000000..99ffafde --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { + +//============================================================================== +class Event::Initializer::Implementation +{ +public: + + std::unordered_map> initializers; + +}; + +//============================================================================== +Event::Initializer::Initializer() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +Event::StandbyPtr Event::Initializer::initialize( + std::function get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + std::optional backup_state) +{ + const auto& type = typeid(description); + const auto it = _pimpl->initializers.find(type); + if (it == _pimpl->initializers.end()) + return nullptr; + + return it->second( + std::move(get_state), + parameters, + description, + std::move(backup_state)); +} + +//============================================================================== +void Event::Initializer::_add_initializer( + std::type_index type, + Initialize initializer) +{ + _pimpl->initializers.insert_or_assign(type, std::move(initializer)); +} + +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index 818215f3..c4ee1776 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -28,6 +28,13 @@ class Phase::Activator::Implementation }; +//============================================================================== +Phase::Activator::Activator() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + //============================================================================== Phase::ActivePtr Phase::Activator::activate( std::function get_state, diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp new file mode 100644 index 00000000..ab3aabce --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -0,0 +1,179 @@ +/* + * Copyright (C) 2021 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 + +#include + +namespace rmf_task_sequence { +namespace events { + +namespace { +//============================================================================== +nlohmann::json convert_to_json(const std::string& input) +{ + nlohmann::json output; + try + { + output = nlohmann::json::parse(input); + } + catch (const std::exception&) + { + output = input; + } + + return output; +} +} // anonymous namespace + +//============================================================================== +class Sequence::Description::Implementation +{ +public: + + Elements elements; + std::optional category; + std::optional detail; + + std::string generate_category() const + { + if (category.has_value()) + return *category; + + return "Sequence"; + } + + Header generate_header( + rmf_task::State initial_state, + const Parameters& parameters) const + { + std::optional> detail_json; + if (!detail.has_value()) + detail_json = std::vector(); + + rmf_traffic::Duration duration_estimate = rmf_traffic::Duration(0); + + for (const auto& element : elements) + { + const auto element_header = + element->generate_header(initial_state, parameters); + + duration_estimate += element_header.original_duration_estimate(); + + initial_state = + element->make_model(initial_state, parameters) + ->invariant_finish_state(); + + if (detail_json.has_value()) + { + nlohmann::json element_output; + element_output["category"] = element_header.category(); + element_output["detail"] = convert_to_json(element_header.detail()); + detail_json->emplace_back(std::move(element_output)); + } + } + + std::string output_detail; + if (detail.has_value()) + output_detail = *detail; + else + output_detail = nlohmann::json(*detail_json).dump(); + + return Header( + generate_category(), + std::move(output_detail), + duration_estimate); + } +}; + +//============================================================================== +Sequence::Description::Description( + Elements elements, + std::optional category, + std::optional detail) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(elements), + std::move(category), + std::move(detail) + })) +{ + // Do nothing +} + +//============================================================================== +auto Sequence::Description::elements() const -> const Elements& +{ + return _pimpl->elements; +} + +//============================================================================== +auto Sequence::Description::elements(Elements new_elements) -> Description& +{ + _pimpl->elements = std::move(new_elements); + return *this; +} + +//============================================================================== +const std::optional& Sequence::Description::category() const +{ + return _pimpl->category; +} + +//============================================================================== +auto Sequence::Description::category(std::optional new_category) +-> Description& +{ + _pimpl->category = std::move(new_category); + return *this; +} + +//============================================================================== +const std::optional& Sequence::Description::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +auto Sequence::Description::detail(std::optional new_detail) +-> Description& +{ + _pimpl->detail = std::move(new_detail); + return *this; +} + +//============================================================================== +Activity::ConstModelPtr Sequence::Description::make_model( + rmf_task::State invariant_initial_state, + const Parameters& parameters) const +{ + return Activity::SequenceModel::make( + _pimpl->elements, + std::move(invariant_initial_state), + parameters); +} + +//============================================================================== +Header Sequence::Description::generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->generate_header(initial_state, parameters); +} + +} // namespace events +} // namespace rmf_task_sequence From 390f5a4cc3ac600329e8fb1793d219f8d6eb6ab1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 1 Nov 2021 22:10:36 +0800 Subject: [PATCH 55/85] Implementing Event Sequence Standby Signed-off-by: Michael X. Grey --- .../include/rmf_task_sequence/Event.hpp | 6 +- .../src/rmf_task_sequence/Event.cpp | 4 +- .../src/rmf_task_sequence/events/Sequence.cpp | 72 +++++++++++++++++++ 3 files changed, 77 insertions(+), 5 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp index 5c14ed1e..a0dd8e45 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -118,7 +118,7 @@ class Event::Initializer using Initialize = std::function< StandbyPtr( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Description& description, std::optional backup_state) @@ -146,10 +146,10 @@ class Event::Initializer /// /// \return an Event in a Standby state StandbyPtr initialize( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, - std::optional backup_state); + std::optional backup_state) const; class Implementation; private: diff --git a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp index 99ffafde..a4b58ffa 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp @@ -37,10 +37,10 @@ Event::Initializer::Initializer() //============================================================================== Event::StandbyPtr Event::Initializer::initialize( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, - std::optional backup_state) + std::optional backup_state) const { const auto& type = typeid(description); const auto it = _pimpl->initializers.find(type); diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index ab3aabce..c95fbc42 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -16,6 +16,7 @@ */ #include +#include #include @@ -175,5 +176,76 @@ Header Sequence::Description::generate_header( return _pimpl->generate_header(initial_state, parameters); } +//============================================================================== +class SequenceStandby : public Event::Standby +{ +public: + + static Event::StandbyPtr make_fresh( + const Event::ConstInitializerPtr& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Sequence::Description& description) + { + std::vector elements; + for (const auto& desc : description.elements()) + { + elements.push_back( + initializer->initialize(get_state, parameters, *desc, std::nullopt)); + } + + return std::make_shared(description, std::move(elements)); + } + + static Event::StandbyPtr make_restore( + const Event::ConstInitializerPtr& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Sequence::Description& description, + const std::string& backup) + { + + + std::vector elements; + + } + + + const Event::ConstStatePtr& state() const final + { + return _state; + } + + rmf_traffic::Duration duration_estimate() const final + { + + } + + Event::ActivePtr begin(std::function update, std::function checkpoint) final + { + + } + + SequenceStandby( + const Sequence::Description& desc, + std::vector elements) + : _elements(std::move(elements)) + { + std::vector element_states; + for (const auto& element : _elements) + element_states.emplace_back(element->state()); + + _state = rmf_task::events::SimpleEvent::make( + desc.category().value_or("Sequence"), + desc.detail().value_or(""), + rmf_task::Event::Status::Standby, + std::move(element_states)); + } + +private: + std::vector _elements; + Event::ConstStatePtr _state; +}; + } // namespace events } // namespace rmf_task_sequence From 54765af3f81bdef31afc864e1ae31777959bf747 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 3 Nov 2021 18:33:32 +0800 Subject: [PATCH 56/85] Implementing the Event Sequence class Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Event.hpp | 39 ++- .../include/rmf_task/events/SimpleEvent.hpp | 5 + .../src/rmf_task/{Condition.cpp => Event.cpp} | 39 +++ rmf_task/src/rmf_task/events/SimpleEvent.cpp | 7 + .../src/rmf_task/phases/RestoreBackup.cpp | 2 +- .../include/rmf_task_sequence/Event.hpp | 158 ++++++++- .../rmf_task_sequence/detail/impl_Event.hpp | 42 ++- .../backup_PhaseSequenceTask_v0_1.schema.json | 6 +- .../src/rmf_task_sequence/Event.cpp | 38 ++- .../src/rmf_task_sequence/events/Sequence.cpp | 309 ++++++++++++++++-- 10 files changed, 563 insertions(+), 82 deletions(-) rename rmf_task/src/rmf_task/{Condition.cpp => Event.cpp} (74%) diff --git a/rmf_task/include/rmf_task/Event.hpp b/rmf_task/include/rmf_task/Event.hpp index 6f56d911..0ebddf00 100644 --- a/rmf_task/include/rmf_task/Event.hpp +++ b/rmf_task/include/rmf_task/Event.hpp @@ -45,6 +45,19 @@ class Event /// that should not generally be used. Uninitialized = 0, + /// The event is underway but it has been blocked. The blockage may + /// require manual intervention to fix. + Blocked, + + /// An error has occurred that the Task implementation does not know how to + /// deal with. Manual intervention is needed to get the task back on track. + Error, + + /// The event cannot ever finish correctly, even with manual intervention. + /// This may mean that the Task cannot be completed if it does not have + /// an automated way to recover from this failure state. + Failed, + /// The event is on standby. It cannot be started yet, and that is its /// expected status. Standby, @@ -55,14 +68,6 @@ class Event /// The event is underway but it has been temporarily delayed. Delayed, - /// The event is underway but it has been blocked. The blockage may - /// require manual intervention to fix. - Blocked, - - /// An error has occurred that the Task implementation does not know how to - /// deal with. Manual intervention is needed to get the task back on track. - Error, - /// An operator has instructed this event to be skipped Skipped, @@ -72,15 +77,14 @@ class Event /// An operator has instructed this event to be killed Killed, - /// The event cannot ever finish correctly, even with manual intervention. - /// This may mean that the Task cannot be completed if it does not have - /// an automated way to recover from this failure state. - Failed, - - /// The event is finished. - Finished + /// The event has completed. + Completed, }; + /// Given the status of two events that are in sequence with each other, + /// return the overall status of the sequence. + static Status sequence_status(Status earlier, Status later); + class State; using ConstStatePtr = std::shared_ptr; @@ -101,8 +105,9 @@ class Event::State /// The current Status of this event virtual Status status() const = 0; - /// Simple wrapper for identifying when an event is finished - inline bool finished() const { return status() == Status::Finished; } + /// A convenience function which returns true if the event's status is any of + /// Skipped, Canceled, Killed, or Completed. + bool finished() const; /// The "name" of this event. Ideally a short, simple piece of text that /// helps a human being intuit what this event is expecting at a glance. diff --git a/rmf_task/include/rmf_task/events/SimpleEvent.hpp b/rmf_task/include/rmf_task/events/SimpleEvent.hpp index 96364868..75da02d9 100644 --- a/rmf_task/include/rmf_task/events/SimpleEvent.hpp +++ b/rmf_task/include/rmf_task/events/SimpleEvent.hpp @@ -73,12 +73,17 @@ class SimpleEvent : public Event::State SimpleEvent& update_dependencies( std::vector new_dependencies); + /// Add one dependency to the state + SimpleEvent& add_dependency(ConstStatePtr new_dependency); + class Implementation; private: SimpleEvent(); rmf_utils::unique_impl_ptr _pimpl; }; +using SimpleEventPtr = std::shared_ptr; + } // namespace events } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Condition.cpp b/rmf_task/src/rmf_task/Event.cpp similarity index 74% rename from rmf_task/src/rmf_task/Condition.cpp rename to rmf_task/src/rmf_task/Event.cpp index 542dbd14..2436f988 100644 --- a/rmf_task/src/rmf_task/Condition.cpp +++ b/rmf_task/src/rmf_task/Event.cpp @@ -37,6 +37,45 @@ std::vector snapshot_dependencies( } } // anonymous namespace +//============================================================================== +Event::Status Event::sequence_status(Status earlier, Status later) +{ + // If either status "needs attention" then we elevate that status, in order + // of criticality. + for (const auto& s : + {Status::Failed, Status::Error, Status::Blocked, Status::Uninitialized}) + { + if (earlier == s || later == s) + return s; + } + + // If the earlier status is "finished" then we use the later status + for (const auto& s : + {Status::Completed, Status::Killed, Status::Canceled, Status::Skipped}) + { + if (earlier == s) + return later; + } + + // If the earlier status is not finished, then we use the earlier status + return earlier; +} + +//============================================================================== +bool Event::State::finished() const +{ + switch (status()) + { + case Status::Skipped: + case Status::Canceled: + case Status::Killed: + case Status::Completed: + return true; + default: + return false; + } +} + //============================================================================== class Event::Snapshot::Implementation { diff --git a/rmf_task/src/rmf_task/events/SimpleEvent.cpp b/rmf_task/src/rmf_task/events/SimpleEvent.cpp index 205d1f28..6b48e406 100644 --- a/rmf_task/src/rmf_task/events/SimpleEvent.cpp +++ b/rmf_task/src/rmf_task/events/SimpleEvent.cpp @@ -117,6 +117,13 @@ SimpleEvent& SimpleEvent::update_dependencies( return *this; } +//============================================================================== +SimpleEvent& SimpleEvent::add_dependency(ConstStatePtr new_dependency) +{ + _pimpl->dependencies.push_back(new_dependency); + return *this; +} + //============================================================================== SimpleEvent::SimpleEvent() { diff --git a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp index d1a54c1d..a786c88f 100644 --- a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -105,7 +105,7 @@ void RestoreBackup::Active::restoration_failed(const std::string& error_message) //============================================================================== void RestoreBackup::Active::restoration_succeeded() { - _pimpl->event->update_status(Event::Status::Finished); + _pimpl->event->update_status(Event::Status::Completed); } //============================================================================== diff --git a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp index a0dd8e45..edb20950 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -27,6 +27,8 @@ #include +#include + namespace rmf_task_sequence { //============================================================================== @@ -35,7 +37,7 @@ class Event : public rmf_task::Event public: // Event::Active simply uses the Activity::Active API - using Active = Activity::Active; + class Active; using ActivePtr = std::shared_ptr; // Event::Description simply uses the Activity::Description API @@ -59,8 +61,10 @@ class Event::Standby { public: - /// Get the state of this Standby event - virtual const ConstStatePtr& state() const = 0; + /// Get the state of this event. The state object returned by this function + /// must always be the same state object, and it must remain the relevant + /// state object for this Event after begin(~) has been called. + virtual ConstStatePtr state() const = 0; /// Estimate how long this event will take once it has started virtual rmf_traffic::Duration duration_estimate() const = 0; @@ -68,21 +72,39 @@ class Event::Standby /// Tell this event to begin. This function should be implemented to always /// return the same Event::Active instance if it gets called more than once. /// - /// \param[in] update - /// A callback that will be triggered when a notable change happens for this - /// event. - /// /// \param[in] checkpoint /// A callback that will be triggered when the event reaches a "checkpoint" /// meaning that the task state should be backed up. + /// + /// \param[in] finished + /// A callback that will be triggered when the event reaches a Finished + /// state virtual ActivePtr begin( - std::function update, - std::function checkpoint) = 0; + std::function checkpoint, + std::function finished) = 0; // Virtual destructor virtual ~Standby() = default; }; +//============================================================================== +class Event::Active : public Activity::Active +{ +public: + + /// Get the state of this event. The state object returned by this function + /// must always be the same state object, and it must remain the same state + /// object that would have been provided by the Event::Standby that kicked off + /// this Event::Active. + virtual ConstStatePtr state() const = 0; + + /// Estimate how much longer this event will take to complete + virtual rmf_traffic::Duration remaining_time_estimate() const = 0; + + // Virtual destructor + virtual ~Active() = default; +}; + //============================================================================== /// The Event::Initializer class is the Event equivalent to the /// rmf_task::Activator class. It consumes an Event::Description and produces @@ -113,6 +135,13 @@ class Event::Initializer /// from a crash or disconnection. If the Event is not being restored, a /// std::nullopt will be passed in here. /// + /// \param[in] update + /// A callback that will be triggered when a notable change happens for this + /// event. \warning The update function must not be triggered during + /// initialization because upstream event handlers will not be ready to + /// handle it. The event state will always be checked right after + /// initialization is finished anyway, so there is no need to trigger this. + /// /// \return an Event in a Standby state template using Initialize = @@ -121,12 +150,60 @@ class Event::Initializer const std::function& get_state, const ConstParametersPtr& parameters, const Description& description, - std::optional backup_state) + std::function update) + >; + + /// Signature for restoring an Event + /// + /// \tparam Description + /// A class that implements the Event::Description interface + /// + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] description + /// The down-casted description of the event + /// + /// \param[in] backup_state + /// The backup state of the Event. + /// + /// \param[in] update + /// A callback that will be triggered when a notable change happens for this + /// event. \warning The update function must not be triggered during + /// initialization because upstream event handlers will not be ready to + /// handle it. The event state will always be checked right after + /// initialization is finished anyway, so there is no need to trigger this. + /// + /// \param[in] checkpoint + /// A callback that will be triggered when the event reaches a "checkpoint" + /// meaning that the task state should be backed up. + /// + /// \param[in] finished + /// A callback that will be triggered when the event reaches a Finished + /// state + /// + /// \return a restored Event in an Active state + template + using Restore = + std::function< + ActivePtr( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Description& description, + const nlohmann::json& backup_state, + std::function update, + std::function checkpoint, + std::function finished) >; /// Add a callback to convert from a Description to an event in standby mode template - void add_initializer(Initialize initializer); + void add( + Initialize initializer, + Restore restorer); /// Initialize an event /// @@ -139,24 +216,69 @@ class Event::Initializer /// \param[in] description /// The description of the event /// - /// \param[in] backup_state - /// The serialized backup state of the Event, if the Event is being restored - /// from a crash or disconnection. If the Event is not being restored, a - /// std::nullopt will be passed in here. + /// \param[in] update + /// A callback that will be triggered when a notable change happens for this + /// event. \warning The update function must not be triggered during + /// initialization because upstream event handlers will not be ready to + /// handle it. The event state will always be checked right after + /// initialization is finished anyway, so there is no need to trigger this. /// /// \return an Event in a Standby state StandbyPtr initialize( const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, - std::optional backup_state) const; + std::function update) const; + + /// Signature for restoring an Event + /// + /// \tparam Description + /// A class that implements the Event::Description interface + /// + /// \param[in] get_state + /// A callback for retrieving the current state of the robot + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] description + /// The down-casted description of the event + /// + /// \param[in] backup_state + /// The backup state of the Event. + /// + /// \param[in] update + /// A callback that will be triggered when a notable change happens for this + /// event. \warning The update function must not be triggered during + /// initialization because upstream event handlers will not be ready to + /// handle it. The event state will always be checked right after + /// initialization is finished anyway, so there is no need to trigger this. + /// + /// \param[in] checkpoint + /// A callback that will be triggered when the event reaches a "checkpoint" + /// meaning that the task state should be backed up. + /// + /// \param[in] finished + /// A callback that will be triggered when the event reaches a Finished + /// state + /// + /// \return a restored Event in an Active state + ActivePtr restore( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + const nlohmann::json& backup, + std::function update, + std::function checkpoint, + std::function finished) const; class Implementation; private: /// \private - void _add_initializer( + void _add( std::type_index type, - Initialize initializer); + Initialize initializer, + Restore restorer); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp index ee6a5494..444f4ffb 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp @@ -24,22 +24,42 @@ namespace rmf_task_sequence { //============================================================================== template -void Event::Initializer::add_initializer(Initialize initializer) +void Event::Initializer::add( + Initialize initializer, + Restore restorer) { - _add_initializer( + _add( typeid(Desc), [initializer]( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, - std::optional backup_state) - { - return initializer( - std::move(get_state), - parameters, - static_cast(description), - std::move(backup_state)); - }); + std::function update) + { + return initializer( + get_state, + parameters, + static_cast(description), + std::move(update)); + }, + [restorer]( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + const nlohmann::json& backup, + std::function update, + std::function checkpoint, + std::function finished) + { + return restorer( + get_state, + parameters, + static_cast(description), + backup, + std::move(update), + std::move(checkpoint), + std::move(finished)); + }); } } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json b/rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json index d3641177..5ca948d7 100644 --- a/rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json +++ b/rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json @@ -1,11 +1,11 @@ { "$schema": "https://json-schema.org/draft/2020-12/schema", "$id": "https://open-rmf.org/rmf_task_sequence/backup_PhaseSequenceTask/0.1", - "title": "Phase Sequence Task", - "description": "A task which is defined by a sequence of specified phases", + "title": "Phase Sequence Task Backup", + "description": "A backup state for a task which is defined by a fixed sequence of phases", "properties": { "schema_version": { - "description": "The version of the PhaseSequenceTask schema being used", + "description": "The version of the Phase Sequence Task Backup schema being used", "const": "0.1" }, "current_phase": { diff --git a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp index a4b58ffa..f2cfada9 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp @@ -25,6 +25,7 @@ class Event::Initializer::Implementation public: std::unordered_map> initializers; + std::unordered_map> restorers; }; @@ -40,7 +41,7 @@ Event::StandbyPtr Event::Initializer::initialize( const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, - std::optional backup_state) const + std::function update) const { const auto& type = typeid(description); const auto it = _pimpl->initializers.find(type); @@ -48,18 +49,45 @@ Event::StandbyPtr Event::Initializer::initialize( return nullptr; return it->second( - std::move(get_state), + get_state, parameters, description, - std::move(backup_state)); + std::move(update)); } //============================================================================== -void Event::Initializer::_add_initializer( +Event::ActivePtr Event::Initializer::restore( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + const nlohmann::json& backup, + std::function update, + std::function checkpoint, + std::function finished) const +{ + const auto& type = typeid(description); + const auto it = _pimpl->restorers.find(type); + if (it == _pimpl->restorers.end()) + return nullptr; + + return it->second( + get_state, + parameters, + description, + backup, + std::move(update), + std::move(checkpoint), + std::move(finished)); +} + +//============================================================================== +void Event::Initializer::_add( std::type_index type, - Initialize initializer) + Initialize initializer, + Restore restorer) { _pimpl->initializers.insert_or_assign(type, std::move(initializer)); + _pimpl->restorers.insert_or_assign(type, std::move(restorer)); } } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index c95fbc42..e3924707 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -19,6 +19,12 @@ #include #include +#include + +#include +#include + +#include namespace rmf_task_sequence { namespace events { @@ -176,76 +182,325 @@ Header Sequence::Description::generate_header( return _pimpl->generate_header(initial_state, parameters); } +//============================================================================== +class BoolGuard +{ +public: + BoolGuard(bool& value) + : _value(value) + { + _value = true; + } + + ~BoolGuard() + { + _value = false; + } + +private: + bool& _value; +}; + +//============================================================================== +class SequenceActive; + //============================================================================== class SequenceStandby : public Event::Standby { public: - static Event::StandbyPtr make_fresh( + static Event::StandbyPtr initiate( const Event::ConstInitializerPtr& initializer, const std::function& get_state, const ConstParametersPtr& parameters, - const Sequence::Description& description) + const Sequence::Description& description, + std::function parent_update) { + auto state = make_state(description); + const auto update = [parent_update = std::move(parent_update), state]() + { + update_status(*state); + parent_update(); + }; + std::vector elements; for (const auto& desc : description.elements()) { - elements.push_back( - initializer->initialize(get_state, parameters, *desc, std::nullopt)); + auto element = initializer->initialize( + get_state, parameters, *desc, update); + + state->add_dependency(element->state()); + + elements.emplace_back(std::move(element)); } - return std::make_shared(description, std::move(elements)); + std::reverse(elements.begin(), elements.end()); + + update_status(*state); + return std::make_shared( + std::move(elements), std::move(state), std::move(parent_update)); + } + + Event::ConstStatePtr state() const final + { + return _state; + } + + rmf_traffic::Duration duration_estimate() const final + { + auto estimate = rmf_traffic::Duration(0); + for (const auto& element : _elements) + estimate += element->duration_estimate(); + + return estimate; + } + + Event::ActivePtr begin( + std::function checkpoint, + std::function finish) final; + + SequenceStandby( + std::vector elements, + rmf_task::events::SimpleEventPtr state, + std::function parent_update) + : _elements(std::move(elements)), + _state(std::move(state)), + _parent_update(std::move(parent_update)) + { + // Do nothing + } + + static rmf_task::events::SimpleEventPtr make_state( + const Sequence::Description& description) + { + return rmf_task::events::SimpleEvent::make( + description.category().value_or("Sequence"), + description.detail().value_or(""), + rmf_task::Event::Status::Standby); + } + + static void update_status(rmf_task::events::SimpleEvent& state) + { + Event::Status status = Event::Status::Completed; + for (const auto& dep : state.dependencies()) + status = Event::sequence_status(status, dep->status()); + + state.update_status(status); } - static Event::StandbyPtr make_restore( +private: + + std::vector _elements; + rmf_task::events::SimpleEventPtr _state; + std::function _parent_update; + std::shared_ptr _active; +}; + +//============================================================================== +class SequenceActive + : public Event::Active, + public std::enable_shared_from_this +{ +public: + + static Event::ActivePtr restore( const Event::ConstInitializerPtr& initializer, const std::function& get_state, const ConstParametersPtr& parameters, const Sequence::Description& description, - const std::string& backup) + const std::string& backup, + std::function parent_update, + std::function checkpoint, + std::function finished) { - + auto state = SequenceStandby::make_state(description); + const auto update = [parent_update = std::move(parent_update), state]() + { + SequenceStandby::update_status(*state); + parent_update(); + }; std::vector elements; - } + const auto backup_state = nlohmann::json::parse(backup); + if (const auto result = + schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) + { + state->update_log().error( + "Parsing failed while restoring backup: " + result->message + + "\nOriginal backup state:\n```" + backup + "\n```"); + state->update_status(Event::Status::Error); + return std::make_shared( + elements, std::move(state), nullptr, nullptr, nullptr); + } + + const auto& current_event_json = backup_state["current_event"]; + const auto current_event_index = + current_event_json["index"].get(); + + const auto& element_descriptions = description.elements(); + if (element_descriptions.size() <= current_event_index) + { + state->update_log().error( + "Failed to restore backup. Index [" + + std::to_string(current_event_index) + "] is too high for [" + + std::to_string(description.elements().size()) + "] event elements. " + "Original text:\n```\n" + backup + "\n```"); + state->update_status(Event::Status::Error); + return std::make_shared( + elements, std::move(state), nullptr, nullptr, nullptr); + } + + auto active = std::make_shared( + elements, std::move(state), std::move(checkpoint), std::move(finished)); + const auto event_finished = [me = active->weak_from_this()]() + { + if (const auto self = me.lock()) + self->next(); + }; + + const auto& current_event_state = current_event_json["state"]; + active->_current = initializer->restore( + get_state, + parameters, + *element_descriptions.at(current_event_index), + current_event_state, + update, + checkpoint, + event_finished); + state->add_dependency(active->state()); + + for (std::size_t i = current_event_index+1; i < element_descriptions.size(); ++i) + { + const auto& desc = element_descriptions[i]; + auto element = initializer->initialize( + get_state, parameters, *desc, update); + + active->_state->add_dependency(element->state()); + active->_remaining.emplace_back(std::move(element)); + } + + std::reverse(active->_remaining.begin(), active->_remaining.end()); + + BoolGuard lock(active->_inside_next); + while (active->_current->state()->finished()) + { + if (active->_remaining.empty()) + { + SequenceStandby::update_status(*active->_state); + return active; + } + + const auto next_event = active->_remaining.back(); + active->_remaining.pop_back(); + active->_current = next_event->begin(active->_checkpoint, event_finished); + } + + return active; + } - const Event::ConstStatePtr& state() const final + Event::ConstStatePtr state() const final { return _state; } - rmf_traffic::Duration duration_estimate() const final + rmf_traffic::Duration remaining_time_estimate() const final { + auto estimate = rmf_traffic::Duration(0); + if (_current) + estimate += _current->remaining_time_estimate(); + + for (const auto& element : _remaining) + estimate += element->duration_estimate(); + return estimate; } - Event::ActivePtr begin(std::function update, std::function checkpoint) final + SequenceActive( + std::vector elements, + rmf_task::events::SimpleEventPtr state, + std::function parent_update, + std::function checkpoint, + std::function finished) + : _current(nullptr), + _remaining(elements), + _state(std::move(state)), + _parent_update(std::move(parent_update)), + _checkpoint(std::move(checkpoint)), + _sequence_finished(std::move(finished)) { - + // Do nothing } - SequenceStandby( - const Sequence::Description& desc, - std::vector elements) - : _elements(std::move(elements)) + void next() { - std::vector element_states; - for (const auto& element : _elements) - element_states.emplace_back(element->state()); + if (_inside_next) + return; + + BoolGuard lock(_inside_next); + + const auto event_finished = [me = weak_from_this()]() + { + if (const auto self = me.lock()) + self->next(); + }; + + do + { + if (_remaining.empty()) + { + SequenceStandby::update_status(*_state); + _sequence_finished(); + return; + } + + const auto next_event = _remaining.back(); + _remaining.pop_back(); + _current = next_event->begin(_checkpoint, event_finished); + } while (_current->state()->finished()); - _state = rmf_task::events::SimpleEvent::make( - desc.category().value_or("Sequence"), - desc.detail().value_or(""), - rmf_task::Event::Status::Standby, - std::move(element_states)); + SequenceStandby::update_status(*_state); + _parent_update(); } private: - std::vector _elements; - Event::ConstStatePtr _state; + + static const nlohmann::json_schema::json_validator backup_schema_validator; + + Event::ActivePtr _current; + std::vector _remaining; + rmf_task::events::SimpleEventPtr _state; + std::function _parent_update; + std::function _checkpoint; + std::function _sequence_finished; + + // We need to make sure that next() never gets called recursively by events + // that finish as soon as they are activated + bool _inside_next = false; }; +//============================================================================== +Event::ActivePtr SequenceStandby::begin( + std::function checkpoint, + std::function finish) +{ + if (_active) + return _active; + + _active = std::make_shared( + std::move(_elements), _state, _parent_update, + std::move(checkpoint), std::move(finish)); + + _active->next(); + return _active; +} + +//============================================================================== +const nlohmann::json_schema::json_validator +SequenceActive::backup_schema_validator = + nlohmann::json_schema::json_validator( + schemas::backup_EventSequence_v0_1); + } // namespace events } // namespace rmf_task_sequence From 036867b899e2d507433ed2911e985715456c1620 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 3 Nov 2021 18:33:56 +0800 Subject: [PATCH 57/85] Define the schema for event sequence backups Signed-off-by: Michael X. Grey --- .../backup_EventSequence_v0_1.schema.json | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 rmf_task_sequence/schemas/backup_EventSequence_v0_1.schema.json diff --git a/rmf_task_sequence/schemas/backup_EventSequence_v0_1.schema.json b/rmf_task_sequence/schemas/backup_EventSequence_v0_1.schema.json new file mode 100644 index 00000000..c98d2ac8 --- /dev/null +++ b/rmf_task_sequence/schemas/backup_EventSequence_v0_1.schema.json @@ -0,0 +1,27 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://open-rmf.org/rmf_task_sequence/backup_EventSequence/0.1", + "title": "Event Sequence Backup", + "description": "A backup state for a sequence of events", + "properties": { + "schema_version": { + "description": "The version of the Event Sequence schema being used", + "const": "0.1" + }, + "current_event": { + "description": "The current event in the sequence when the backup occurred", + "properties": { + "index": { + "description": "The index of the current phase within the sequence", + "type": "integer", + "minimum": 0 + }, + "state": { + "description": "The serialized state of the backed up current event" + } + }, + "required": [ "index", "state" ] + } + }, + "required": [ "schema_version", "current_event" ] +} From 6f6eb029d9ec1752a83f87dba2ddbeb93d2a013a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 5 Nov 2021 16:02:57 +0800 Subject: [PATCH 58/85] Added Repeat event Signed-off-by: Yadunund --- .../rmf_task_sequence/events/Repeat.hpp | 86 +++++++++++++ .../src/rmf_task_sequence/events/Repeat.cpp | 118 ++++++++++++++++++ 2 files changed, 204 insertions(+) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/Repeat.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Repeat.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Repeat.hpp new file mode 100644 index 00000000..f2939f18 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Repeat.hpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__REPEAT_HPP +#define RMF_TASK_SEQUENCE__EVENTS__REPEAT_HPP + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// An event that repeats for a number of iterations +class Repeat +{ +public: + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class Repeat::Description : public Event::Description +{ +public: + + /// Make a Repeat description. + /// + /// \param[in] event + /// The event that will be repeated + /// + /// \param[in] repetitions + /// The number of times the event will be repeated + static DescriptionPtr make( + Event::ConstDescriptionPtr event, + std::size_t repetitions); + + /// Get the event + const Event::ConstDescriptionPtr event() const; + + /// Set the event + Description& event(Event::ConstDescriptionPtr new_event); + + /// Get the repetitions + std::size_t repetitions() const; + + /// Set the repetitions + Description& repetitions(std::size_t new_repetitions); + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__REPEAT_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp new file mode 100644 index 00000000..9cfcfa33 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2021 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 +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== + +// No definition of Model class. Description::make_mode() will return an +// Activity::SequenceModel + +//============================================================================== +class Repeat::Description::Implementation +{ +public: + + Event::ConstDescriptionPtr event; + std::size_t repetitions; +}; + +//============================================================================== +auto Repeat::Description::make( + Event::ConstDescriptionPtr event, + std::size_t repetitions) -> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = + rmf_utils::make_impl( + Implementation{ + std::move(event), + repetitions}); + + return output; +} + +//============================================================================== +Repeat::Description::Description() +{ + // Do nothing +} + +//============================================================================== +auto Repeat::Description::event() const -> const Event::ConstDescriptionPtr +{ + return _pimpl->event; +} + +//============================================================================== +auto Repeat::Description::event( + Event::ConstDescriptionPtr new_event)-> Description& +{ + _pimpl->event = std::move(new_event); + return *this; +} + +//============================================================================== +std::size_t Repeat::Description::repetitions() const +{ + return _pimpl->repetitions; +} + +//============================================================================== +auto Repeat::Description::repetitions( + std::size_t new_repetitions)-> Description& +{ + _pimpl->repetitions = new_repetitions; + return *this; +} + +//============================================================================== +Activity::ConstModelPtr Repeat::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + std::vector descriptions = {}; + for (std::size_t i = 0; i < _pimpl->repetitions; ++i) + descriptions.push_back(_pimpl->event); + + return Activity::SequenceModel::make() + descriptions, + invariant_initial_state, + parameters); +} + +//============================================================================== +Header Repeat::Description::generate_header( + const State&, const Parameters&) const +{ + const auto& original_header = _pimpl->event->generate_header(); + const std::string& original_category = original_header.category(); + const auto& estimate = original_header.original_duration_estimate(); + + const std::string& category = "Repeating [" + original_category + " ]"; + const std::string& detail = + " " + std::to_string(_pimpl->repetitions) + " times"; + + return Header( + category, + detail, + _pimpl->repetitions * estimate); +} From a6dd97be19af68eb6e2e5981181e9730a8146975 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 5 Nov 2021 18:03:29 +0800 Subject: [PATCH 59/85] Added While event Signed-off-by: Yadunund --- .../rmf_task_sequence/events/While.hpp | 98 +++++++++++ .../src/rmf_task_sequence/events/Repeat.cpp | 32 +++- .../src/rmf_task_sequence/events/While.cpp | 154 ++++++++++++++++++ 3 files changed, 275 insertions(+), 9 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/While.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/While.cpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/While.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/While.hpp new file mode 100644 index 00000000..949012ab --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/While.hpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__WHILE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__WHILE_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// An event that remains active indefinitely until a condition is fullfiled +class While +{ +public: + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class While::Description : public Event::Description +{ +public: + + /// Make a While description. + /// + /// \param[in] event + /// The event that will be repeated indefinitely + /// + /// \param[in] completed + /// A callback that will return true when the event is completed + /// + /// \param[in] while_duration_estimate + /// An estimate of how long the event will take to complete + static DescriptionPtr make( + Event::ConstDescriptionPtr event, + std::function completed, + rmf_traffic::Duration while_duration_estimate); + + /// Get the event + const Event::ConstDescriptionPtr event() const; + + /// Set the event + Description& event(Event::ConstDescriptionPtr new_event); + + /// Get the completed condition + const std::function completed() const; + + /// Set the completed condition + Description& completed(std::function new_completed); + + /// Get the duration estimate + const rmf_traffic::Duration while_duration_estimate() const; + + /// Set the duration estimate + Description& while_duration_estimate(rmf_traffic::Duration new_duration); + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__WHILE_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp index 9cfcfa33..3a2be474 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp @@ -22,9 +22,11 @@ namespace rmf_task_sequence { namespace events { //============================================================================== - -// No definition of Model class. Description::make_mode() will return an -// Activity::SequenceModel +class Model +{ + // No definition of Model class. Description::make_mode() will return an + // Activity::SequenceModel +}; //============================================================================== class Repeat::Description::Implementation @@ -40,6 +42,14 @@ auto Repeat::Description::make( Event::ConstDescriptionPtr event, std::size_t repetitions) -> DescriptionPtr { + if (repetitions == 0) + { + /* INDENT-OFF */ + throw std::invalid_argument( + "The number of repetitions has to be greater than zero"); + /* INDENT-ON */ + } + auto output = std::shared_ptr(new Description); output->_pimpl = rmf_utils::make_impl( @@ -89,11 +99,11 @@ Activity::ConstModelPtr Repeat::Description::make_model( State invariant_initial_state, const Parameters& parameters) const { - std::vector descriptions = {}; + std::vector descriptions = {}; for (std::size_t i = 0; i < _pimpl->repetitions; ++i) descriptions.push_back(_pimpl->event); - return Activity::SequenceModel::make() + return Activity::SequenceModel::make( descriptions, invariant_initial_state, parameters); @@ -101,18 +111,22 @@ Activity::ConstModelPtr Repeat::Description::make_model( //============================================================================== Header Repeat::Description::generate_header( - const State&, const Parameters&) const + const State& state, const Parameters& parameters) const { - const auto& original_header = _pimpl->event->generate_header(); + const auto& original_header = + _pimpl->event->generate_header(state, parameters); const std::string& original_category = original_header.category(); const auto& estimate = original_header.original_duration_estimate(); const std::string& category = "Repeating [" + original_category + " ]"; - const std::string& detail = - " " + std::to_string(_pimpl->repetitions) + " times"; + const std::string& detail = category + " " + + std::to_string(_pimpl->repetitions) + " times"; return Header( category, detail, _pimpl->repetitions * estimate); } + +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp new file mode 100644 index 00000000..c0cf5256 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2021 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 +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class Model +{ + // No definition of Model class. Description::make_mode() will return a + // Repeat::Model +}; + +//============================================================================== +class While::Description::Implementation +{ +public: + + Event::ConstDescriptionPtr event; + std::function completed; + rmf_traffic::Duration while_duration_estimate; +}; + +//============================================================================== +auto While::Description::make( + Event::ConstDescriptionPtr event, + std::function completed, + rmf_traffic::Duration while_duration_estimate) -> DescriptionPtr +{ + auto output = std::shared_ptr(new Description); + output->_pimpl = + rmf_utils::make_impl( + Implementation{ + std::move(event), + std::move(completed), + while_duration_estimate}); + + return output; +} + +//============================================================================== +While::Description::Description() +{ + // Do nothing +} + +//============================================================================== +auto While::Description::event() const -> const Event::ConstDescriptionPtr +{ + return _pimpl->event; +} + +//============================================================================== +auto While::Description::event( + Event::ConstDescriptionPtr new_event)-> Description& +{ + _pimpl->event = std::move(new_event); + return *this; +} + +//============================================================================== +const std::function While::Description::completed() const +{ + return _pimpl->completed; +} + +//============================================================================== +auto While::Description::completed( + std::function new_completed)-> Description& +{ + _pimpl->completed = std::move(new_completed); + return *this; +} + +//============================================================================== +const rmf_traffic::Duration While::Description::while_duration_estimate() const +{ + return _pimpl->while_duration_estimate; +} + +//============================================================================== +auto While::Description::while_duration_estimate( + rmf_traffic::Duration new_duration)-> Description& +{ + _pimpl->while_duration_estimate = new_duration; + return *this; +} + +//============================================================================== +Activity::ConstModelPtr While::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + const auto& event_header = _pimpl->event->generate_header( + invariant_initial_state, parameters); + const auto& event_estimate = event_header.original_duration_estimate(); + + // TODO(YV) Warn users if while_duration_estimate is smaller than + // event estimate + const std::size_t repetitions = std::max((std::size_t)1, static_cast< + std::size_t>(_pimpl->while_duration_estimate / event_estimate)); + + const auto repeat_event = Repeat::Description::make( + _pimpl->event, + repetitions); + + if (repeat_event) + { + return repeat_event->make_model( + invariant_initial_state, + parameters); + } + + return nullptr; +} + +//============================================================================== +Header While::Description::generate_header( + const State& state, const Parameters& parameters) const +{ + const auto& original_header = + _pimpl->event->generate_header(state, parameters); + const std::string& original_category = original_header.category(); + const auto& estimate = original_header.original_duration_estimate(); + + const std::string& category = "While [" + original_category + " ]"; + const std::string& detail = category + " indefinitely"; + + return Header( + category, + detail, + _pimpl->while_duration_estimate); +} + +} // namespace events +} // namespace rmf_task_sequence From eaf95eaee0dee0f074ec6cf499da8bb6e5235a54 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 6 Nov 2021 16:41:55 +0800 Subject: [PATCH 60/85] Finished implementing event sequence Signed-off-by: Michael X. Grey --- .../rmf_task_sequence/events/Sequence.hpp | 23 ++- .../src/rmf_task_sequence/events/Sequence.cpp | 144 +++++++++++++++--- 2 files changed, 146 insertions(+), 21 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp index d21f2ad5..49db7490 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp @@ -31,12 +31,27 @@ class Sequence : public Event { public: - /// Make an initializer for Event Sequences + /// Give an initializer the ability to initialize event sequences /// /// \param[in] initializer - /// The Initializer that should be used to initialize other events - static Initializer::Initialize - make_initializer(ConstInitializerPtr initializer); + /// The Initializer that should be used to initialize other events, and + /// also will be given the ability to initialize event sequences. This is + /// equivalent to the overload of this function, but where add_to and + /// initialize_from are the same initializer. + static void add(const Event::InitializerPtr& initializer); + + /// Give an initializer the ability to initialize event sequences + /// + /// \param[in] add_to + /// This Initializer will be given the ability to initialize event sequences + /// + /// \param[in] initialize_from + /// This Initializer will be used by the Event Sequence to initialize the + /// events that it depends on. This may be a different initializer than the + /// one that the event sequence is added to. + static void add( + Event::Initializer& add_to, + const Event::ConstInitializerPtr& initialize_from); class Description; }; diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index e3924707..8e266842 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -210,7 +210,7 @@ class SequenceStandby : public Event::Standby public: static Event::StandbyPtr initiate( - const Event::ConstInitializerPtr& initializer, + const Event::Initializer& initializer, const std::function& get_state, const ConstParametersPtr& parameters, const Sequence::Description& description, @@ -226,7 +226,7 @@ class SequenceStandby : public Event::Standby std::vector elements; for (const auto& desc : description.elements()) { - auto element = initializer->initialize( + auto element = initializer.initialize( get_state, parameters, *desc, update); state->add_dependency(element->state()); @@ -281,6 +281,11 @@ class SequenceStandby : public Event::Standby static void update_status(rmf_task::events::SimpleEvent& state) { + if (state.status() == Event::Status::Canceled + || state.status() == Event::Status::Killed + || state.status() == Event::Status::Skipped) + return; + Event::Status status = Event::Status::Completed; for (const auto& dep : state.dependencies()) status = Event::sequence_status(status, dep->status()); @@ -304,7 +309,7 @@ class SequenceActive public: static Event::ActivePtr restore( - const Event::ConstInitializerPtr& initializer, + const Event::Initializer& initializer, const std::function& get_state, const ConstParametersPtr& parameters, const Sequence::Description& description, @@ -352,7 +357,11 @@ class SequenceActive } auto active = std::make_shared( - elements, std::move(state), std::move(checkpoint), std::move(finished)); + current_event_index, + std::move(state), + parent_update, + std::move(checkpoint), + std::move(finished)); const auto event_finished = [me = active->weak_from_this()]() { @@ -361,7 +370,7 @@ class SequenceActive }; const auto& current_event_state = current_event_json["state"]; - active->_current = initializer->restore( + active->_current = initializer.restore( get_state, parameters, *element_descriptions.at(current_event_index), @@ -374,29 +383,33 @@ class SequenceActive for (std::size_t i = current_event_index+1; i < element_descriptions.size(); ++i) { const auto& desc = element_descriptions[i]; - auto element = initializer->initialize( + auto element = initializer.initialize( get_state, parameters, *desc, update); active->_state->add_dependency(element->state()); - active->_remaining.emplace_back(std::move(element)); + elements.emplace_back(std::move(element)); } - std::reverse(active->_remaining.begin(), active->_remaining.end()); + std::reverse(elements.begin(), elements.end()); + + active->_reverse_remaining = std::move(elements); BoolGuard lock(active->_inside_next); while (active->_current->state()->finished()) { - if (active->_remaining.empty()) + if (active->_reverse_remaining.empty()) { SequenceStandby::update_status(*active->_state); return active; } - const auto next_event = active->_remaining.back(); - active->_remaining.pop_back(); + ++active->_current_event_index_plus_one; + const auto next_event = active->_reverse_remaining.back(); + active->_reverse_remaining.pop_back(); active->_current = next_event->begin(active->_checkpoint, event_finished); } + SequenceStandby::update_status(*active->_state); return active; } @@ -411,12 +424,44 @@ class SequenceActive if (_current) estimate += _current->remaining_time_estimate(); - for (const auto& element : _remaining) + for (const auto& element : _reverse_remaining) estimate += element->duration_estimate(); return estimate; } + Backup backup() const final + { + nlohmann::json current_event_json; + current_event_json["index"] = _current_event_index_plus_one - 1; + current_event_json["state"] = _current->backup().state(); + + nlohmann::json backup_json; + backup_json["schema_version"] = "0.1"; + backup_json["current_event"] = std::move(current_event_json); + + return Backup::make(_next_backup_sequence_number++, backup_json); + } + + Resume interrupt(std::function task_is_interrupted) final + { + return _current->interrupt(std::move(task_is_interrupted)); + } + + void cancel() + { + _reverse_remaining.clear(); + _state->update_status(Event::Status::Canceled); + _current->cancel(); + } + + void kill() + { + _reverse_remaining.clear(); + _state->update_status(Event::Status::Killed); + _current->kill(); + } + SequenceActive( std::vector elements, rmf_task::events::SimpleEventPtr state, @@ -424,7 +469,23 @@ class SequenceActive std::function checkpoint, std::function finished) : _current(nullptr), - _remaining(elements), + _reverse_remaining(elements), + _state(std::move(state)), + _parent_update(std::move(parent_update)), + _checkpoint(std::move(checkpoint)), + _sequence_finished(std::move(finished)) + { + // Do nothing + } + + SequenceActive( + uint64_t current_event_index, + rmf_task::events::SimpleEventPtr state, + std::function parent_update, + std::function checkpoint, + std::function finished) + : _current(nullptr), + _current_event_index_plus_one(current_event_index+1), _state(std::move(state)), _parent_update(std::move(parent_update)), _checkpoint(std::move(checkpoint)), @@ -448,15 +509,16 @@ class SequenceActive do { - if (_remaining.empty()) + if (_reverse_remaining.empty()) { SequenceStandby::update_status(*_state); _sequence_finished(); return; } - const auto next_event = _remaining.back(); - _remaining.pop_back(); + ++_current_event_index_plus_one; + const auto next_event = _reverse_remaining.back(); + _reverse_remaining.pop_back(); _current = next_event->begin(_checkpoint, event_finished); } while (_current->state()->finished()); @@ -469,7 +531,8 @@ class SequenceActive static const nlohmann::json_schema::json_validator backup_schema_validator; Event::ActivePtr _current; - std::vector _remaining; + uint64_t _current_event_index_plus_one = 0; + std::vector _reverse_remaining; rmf_task::events::SimpleEventPtr _state; std::function _parent_update; std::function _checkpoint; @@ -478,6 +541,7 @@ class SequenceActive // We need to make sure that next() never gets called recursively by events // that finish as soon as they are activated bool _inside_next = false; + mutable uint64_t _next_backup_sequence_number = 0; }; //============================================================================== @@ -502,5 +566,51 @@ SequenceActive::backup_schema_validator = nlohmann::json_schema::json_validator( schemas::backup_EventSequence_v0_1); +//============================================================================== +void Sequence::add(const Event::InitializerPtr& initializer) +{ + add(*initializer, initializer); +} + +//============================================================================== +void Sequence::add( + Event::Initializer& add_to, + const Event::ConstInitializerPtr& initialize_from) +{ + add_to.add( + [initialize_from]( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Sequence::Description& description, + std::function update) + { + return SequenceStandby::initiate( + *initialize_from, + get_state, + parameters, + description, + std::move(update)); + }, + [initialize_from]( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Sequence::Description& description, + const nlohmann::json& backup_state, + std::function update, + std::function checkpoint, + std::function finished) + { + return SequenceActive::restore( + *initialize_from, + get_state, + parameters, + description, + backup_state, + std::move(update), + std::move(checkpoint), + std::move(finished)); + }); +} + } // namespace events } // namespace rmf_task_sequence From 56ec2a224fa6adab7514a0b2dc7d9ad2b9628a2b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 6 Nov 2021 17:15:26 +0800 Subject: [PATCH 61/85] Refactoring Sequence into a more generic Bundle Signed-off-by: Michael X. Grey --- .../events/{Sequence.hpp => Bundle.hpp} | 58 +++++--- .../events/{Sequence.cpp => Bundle.cpp} | 135 +++++++++++------- 2 files changed, 126 insertions(+), 67 deletions(-) rename rmf_task_sequence/include/rmf_task_sequence/events/{Sequence.hpp => Bundle.hpp} (66%) rename rmf_task_sequence/src/rmf_task_sequence/events/{Sequence.cpp => Bundle.cpp} (80%) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp similarity index 66% rename from rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp rename to rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp index 49db7490..60cfb301 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/Sequence.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK_SEQUENCE__EVENTS__SEQUENCE_HPP -#define RMF_TASK_SEQUENCE__EVENTS__SEQUENCE_HPP +#ifndef RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP #include @@ -27,11 +27,27 @@ namespace rmf_task_sequence { namespace events { //============================================================================== -class Sequence : public Event +class Bundle : public Event { public: - /// Give an initializer the ability to initialize event sequences + enum Type + { + /// The bundle's dependencies will be executed one-by-one in sequence. The + /// bundle will finished when the last of its events reaches a finished + /// status. + Sequence, + + /// The bundle will execute its dependencies in parallel and will finish + /// when all of its dependencies are finished. + ParallelAll, + + /// The bundle will execute its dependencies in parallel and will finished + /// when any (one or more) of its dependencies finishes. + ParallelAny + }; + + /// Give an initializer the ability to initialize event bundles /// /// \param[in] initializer /// The Initializer that should be used to initialize other events, and @@ -57,36 +73,44 @@ class Sequence : public Event }; //============================================================================== -class Sequence::Description : public Event::Description +class Bundle::Description : public Event::Description { public: - using Elements = std::vector; + using Dependencies = std::vector; /// Construct a Sequence Description /// - /// \param[in] elements - /// These are the events that the sequence will run through. Each event in - /// the sequence will be in Standby mode until the previous one reaches a - /// finished status. + /// \param[in] dependencies + /// These are the events that the bundle will depend on. + /// + /// \param[in] type + /// The type of the bundle, which determines its behavior. /// /// \param[in] category - /// Optionally give a category to this sequence. If left unspecified, the - /// category will simply be "Sequence". + /// Optionally give a category to this bundle. If left unspecified, the + /// category will be based on its type. /// /// \param[in] detail - /// Optionally give some detail to this sequence. If left unspecified, the + /// Optionally give some detail to this bundle. If left unspecified, the /// detail will simply aggregate the details of the dependencies. Description( - Elements elements, + Dependencies dependencies, + Type type, std::optional category = std::nullopt, std::optional detail = std::nullopt); /// Get the elements of the sequence - const Elements& elements() const; + const Dependencies& dependencies() const; /// Change the elements of the sequence - Description& elements(Elements new_elements); + Description& dependencies(Dependencies new_elements); + + /// Get the type of bundle this is + Type type() const; + + /// Change the type of bundle that this is + Description& type(Type new_type); /// Get the category settings const std::optional& category() const; @@ -119,4 +143,4 @@ class Sequence::Description : public Event::Description } // namespace rmf_task_sequence -#endif // RMF_TASK_SEQUENCE__EVENTS__SEQUENCE_HPP +#endif // RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp similarity index 80% rename from rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp rename to rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index 8e266842..5a764408 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -15,7 +15,7 @@ * */ -#include +#include #include #include @@ -48,11 +48,12 @@ nlohmann::json convert_to_json(const std::string& input) } // anonymous namespace //============================================================================== -class Sequence::Description::Implementation +class Bundle::Description::Implementation { public: - Elements elements; + Dependencies dependencies; + Type type; std::optional category; std::optional detail; @@ -61,7 +62,38 @@ class Sequence::Description::Implementation if (category.has_value()) return *category; - return "Sequence"; + switch (type) + { + case Type::Sequence: + return "Sequence"; + case Type::ParallelAll: + return "Parallel All"; + case Type::ParallelAny: + return "One of"; + } + + return ""; + } + + rmf_traffic::Duration adjust_estimate( + std::optional current_estimate, + rmf_traffic::Duration next_dependency_estimate) const + { + if (current_estimate.has_value()) + { + if (Type::ParallelAll == type) + return std::max(*current_estimate, next_dependency_estimate); + else if (Type::ParallelAny == type) + return std::min(*current_estimate, next_dependency_estimate); + } + else + { + if (Type::ParallelAll == type || Type::ParallelAny == type) + return next_dependency_estimate; + } + + return current_estimate.value_or(rmf_traffic::Duration(0)) + + next_dependency_estimate; } Header generate_header( @@ -72,14 +104,15 @@ class Sequence::Description::Implementation if (!detail.has_value()) detail_json = std::vector(); - rmf_traffic::Duration duration_estimate = rmf_traffic::Duration(0); + std::optional duration_estimate; - for (const auto& element : elements) + for (const auto& element : dependencies) { const auto element_header = element->generate_header(initial_state, parameters); - duration_estimate += element_header.original_duration_estimate(); + duration_estimate = adjust_estimate( + duration_estimate, element_header.original_duration_estimate()); initial_state = element->make_model(initial_state, parameters) @@ -103,18 +136,20 @@ class Sequence::Description::Implementation return Header( generate_category(), std::move(output_detail), - duration_estimate); + duration_estimate.value_or(rmf_traffic::Duration(0))); } }; //============================================================================== -Sequence::Description::Description( - Elements elements, +Bundle::Description::Description( + Dependencies dependencies, + Type type, std::optional category, std::optional detail) : _pimpl(rmf_utils::make_impl( Implementation{ - std::move(elements), + std::move(dependencies), + type, std::move(category), std::move(detail) })) @@ -123,26 +158,26 @@ Sequence::Description::Description( } //============================================================================== -auto Sequence::Description::elements() const -> const Elements& +auto Bundle::Description::dependencies() const -> const Dependencies& { - return _pimpl->elements; + return _pimpl->dependencies; } //============================================================================== -auto Sequence::Description::elements(Elements new_elements) -> Description& +auto Bundle::Description::dependencies(Dependencies new_dependencies) -> Description& { - _pimpl->elements = std::move(new_elements); + _pimpl->dependencies = std::move(new_dependencies); return *this; } //============================================================================== -const std::optional& Sequence::Description::category() const +const std::optional& Bundle::Description::category() const { return _pimpl->category; } //============================================================================== -auto Sequence::Description::category(std::optional new_category) +auto Bundle::Description::category(std::optional new_category) -> Description& { _pimpl->category = std::move(new_category); @@ -150,13 +185,13 @@ auto Sequence::Description::category(std::optional new_category) } //============================================================================== -const std::optional& Sequence::Description::detail() const +const std::optional& Bundle::Description::detail() const { return _pimpl->detail; } //============================================================================== -auto Sequence::Description::detail(std::optional new_detail) +auto Bundle::Description::detail(std::optional new_detail) -> Description& { _pimpl->detail = std::move(new_detail); @@ -164,18 +199,18 @@ auto Sequence::Description::detail(std::optional new_detail) } //============================================================================== -Activity::ConstModelPtr Sequence::Description::make_model( +Activity::ConstModelPtr Bundle::Description::make_model( rmf_task::State invariant_initial_state, const Parameters& parameters) const { return Activity::SequenceModel::make( - _pimpl->elements, + _pimpl->dependencies, std::move(invariant_initial_state), parameters); } //============================================================================== -Header Sequence::Description::generate_header( +Header Bundle::Description::generate_header( const rmf_task::State& initial_state, const Parameters& parameters) const { @@ -213,7 +248,7 @@ class SequenceStandby : public Event::Standby const Event::Initializer& initializer, const std::function& get_state, const ConstParametersPtr& parameters, - const Sequence::Description& description, + const Bundle::Description& description, std::function parent_update) { auto state = make_state(description); @@ -223,22 +258,22 @@ class SequenceStandby : public Event::Standby parent_update(); }; - std::vector elements; - for (const auto& desc : description.elements()) + std::vector dependencies; + for (const auto& desc : description.dependencies()) { auto element = initializer.initialize( get_state, parameters, *desc, update); state->add_dependency(element->state()); - elements.emplace_back(std::move(element)); + dependencies.emplace_back(std::move(element)); } - std::reverse(elements.begin(), elements.end()); + std::reverse(dependencies.begin(), dependencies.end()); update_status(*state); return std::make_shared( - std::move(elements), std::move(state), std::move(parent_update)); + std::move(dependencies), std::move(state), std::move(parent_update)); } Event::ConstStatePtr state() const final @@ -249,7 +284,7 @@ class SequenceStandby : public Event::Standby rmf_traffic::Duration duration_estimate() const final { auto estimate = rmf_traffic::Duration(0); - for (const auto& element : _elements) + for (const auto& element : _dependencies) estimate += element->duration_estimate(); return estimate; @@ -260,10 +295,10 @@ class SequenceStandby : public Event::Standby std::function finish) final; SequenceStandby( - std::vector elements, + std::vector dependencies, rmf_task::events::SimpleEventPtr state, std::function parent_update) - : _elements(std::move(elements)), + : _dependencies(std::move(dependencies)), _state(std::move(state)), _parent_update(std::move(parent_update)) { @@ -271,7 +306,7 @@ class SequenceStandby : public Event::Standby } static rmf_task::events::SimpleEventPtr make_state( - const Sequence::Description& description) + const Bundle::Description& description) { return rmf_task::events::SimpleEvent::make( description.category().value_or("Sequence"), @@ -295,7 +330,7 @@ class SequenceStandby : public Event::Standby private: - std::vector _elements; + std::vector _dependencies; rmf_task::events::SimpleEventPtr _state; std::function _parent_update; std::shared_ptr _active; @@ -312,7 +347,7 @@ class SequenceActive const Event::Initializer& initializer, const std::function& get_state, const ConstParametersPtr& parameters, - const Sequence::Description& description, + const Bundle::Description& description, const std::string& backup, std::function parent_update, std::function checkpoint, @@ -325,7 +360,7 @@ class SequenceActive parent_update(); }; - std::vector elements; + std::vector dependencies; const auto backup_state = nlohmann::json::parse(backup); if (const auto result = @@ -336,24 +371,24 @@ class SequenceActive + "\nOriginal backup state:\n```" + backup + "\n```"); state->update_status(Event::Status::Error); return std::make_shared( - elements, std::move(state), nullptr, nullptr, nullptr); + dependencies, std::move(state), nullptr, nullptr, nullptr); } const auto& current_event_json = backup_state["current_event"]; const auto current_event_index = current_event_json["index"].get(); - const auto& element_descriptions = description.elements(); + const auto& element_descriptions = description.dependencies(); if (element_descriptions.size() <= current_event_index) { state->update_log().error( "Failed to restore backup. Index [" + std::to_string(current_event_index) + "] is too high for [" - + std::to_string(description.elements().size()) + "] event elements. " + + std::to_string(description.dependencies().size()) + "] event dependencies. " "Original text:\n```\n" + backup + "\n```"); state->update_status(Event::Status::Error); return std::make_shared( - elements, std::move(state), nullptr, nullptr, nullptr); + dependencies, std::move(state), nullptr, nullptr, nullptr); } auto active = std::make_shared( @@ -387,12 +422,12 @@ class SequenceActive get_state, parameters, *desc, update); active->_state->add_dependency(element->state()); - elements.emplace_back(std::move(element)); + dependencies.emplace_back(std::move(element)); } - std::reverse(elements.begin(), elements.end()); + std::reverse(dependencies.begin(), dependencies.end()); - active->_reverse_remaining = std::move(elements); + active->_reverse_remaining = std::move(dependencies); BoolGuard lock(active->_inside_next); while (active->_current->state()->finished()) @@ -463,13 +498,13 @@ class SequenceActive } SequenceActive( - std::vector elements, + std::vector dependencies, rmf_task::events::SimpleEventPtr state, std::function parent_update, std::function checkpoint, std::function finished) : _current(nullptr), - _reverse_remaining(elements), + _reverse_remaining(dependencies), _state(std::move(state)), _parent_update(std::move(parent_update)), _checkpoint(std::move(checkpoint)), @@ -553,7 +588,7 @@ Event::ActivePtr SequenceStandby::begin( return _active; _active = std::make_shared( - std::move(_elements), _state, _parent_update, + std::move(_dependencies), _state, _parent_update, std::move(checkpoint), std::move(finish)); _active->next(); @@ -567,21 +602,21 @@ SequenceActive::backup_schema_validator = schemas::backup_EventSequence_v0_1); //============================================================================== -void Sequence::add(const Event::InitializerPtr& initializer) +void Bundle::add(const Event::InitializerPtr& initializer) { add(*initializer, initializer); } //============================================================================== -void Sequence::add( +void Bundle::add( Event::Initializer& add_to, const Event::ConstInitializerPtr& initialize_from) { - add_to.add( + add_to.add( [initialize_from]( const std::function& get_state, const ConstParametersPtr& parameters, - const Sequence::Description& description, + const Bundle::Description& description, std::function update) { return SequenceStandby::initiate( @@ -594,7 +629,7 @@ void Sequence::add( [initialize_from]( const std::function& get_state, const ConstParametersPtr& parameters, - const Sequence::Description& description, + const Bundle::Description& description, const nlohmann::json& backup_state, std::function update, std::function checkpoint, From cb5619cd2f417cbcc7247e107977bf8479b1560a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 10 Nov 2021 17:47:09 +0800 Subject: [PATCH 62/85] Migrating schema header generation to rmf_api_msgs Signed-off-by: Michael X. Grey --- rmf_task_sequence/CMakeLists.txt | 3 +- .../cmake/rmf_task_sequence-config.cmake.in | 4 +- rmf_task_sequence/package.xml | 1 + rmf_task_sequence/schemas/CMakeLists.txt | 21 ++++++---- .../events/internal_Sequence.hpp | 40 +++++++++++++++++++ 5 files changed, 58 insertions(+), 11 deletions(-) create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index bb360087..38cb74fd 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -20,6 +20,7 @@ endif() include(GNUInstallDirs) +find_package(rmf_api_msgs REQUIRED) find_package(rmf_task REQUIRED) find_package(nlohmann_json REQUIRED) find_package(nlohmann_json_schema_validator_vendor REQUIRED) @@ -47,7 +48,7 @@ target_include_directories(rmf_task_sequence PUBLIC $ $ - $ # for auto-generated headers + $ # for auto-generated schema headers ) if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) diff --git a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in index 4b671c2f..d4cd585a 100644 --- a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in +++ b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in @@ -4,8 +4,8 @@ get_filename_component(rmf_task_sequence_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" include(CMakeFindDependencyMacro) -find_dependency(rmf_task REQUIRED) -find_dependency(nlohmann-json-dev REQUIRED) +find_dependency(rmf_task) +find_dependency(nlohmann_json) if(NOT TARGET rmf_task_sequence::rmf_task_sequence) include("${rmf_task_sequence_CMAKE_DIR}/rmf_task_sequence-targets.cmake") diff --git a/rmf_task_sequence/package.xml b/rmf_task_sequence/package.xml index f8c0889f..6f194213 100644 --- a/rmf_task_sequence/package.xml +++ b/rmf_task_sequence/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 Grey + rmf_api_msgs rmf_task nlohmann-json3-dev nlohmann_json_schema_validator_vendor diff --git a/rmf_task_sequence/schemas/CMakeLists.txt b/rmf_task_sequence/schemas/CMakeLists.txt index ec81d1d1..97421707 100644 --- a/rmf_task_sequence/schemas/CMakeLists.txt +++ b/rmf_task_sequence/schemas/CMakeLists.txt @@ -1,12 +1,17 @@ -include(../cmake/generate_schema_header.cmake) +#include(../cmake/generate_schema_header.cmake) -file(GLOB schema_files "*.schema.json") +#file(GLOB schema_files "*.schema.json") -foreach(schema_file ${schema_files}) - generate_schema_header(${schema_file}) -endforeach() +#foreach(schema_file ${schema_files}) +# generate_schema_header(${schema_file}) +#endforeach() -install( - DIRECTORY ${CMAKE_BINARY_DIR}/include/ - DESTINATION include +#install( +# DIRECTORY ${CMAKE_BINARY_DIR}/include/ +# DESTINATION include +#) + +rmf_api_generate_schema_headers( + PACKAGE rmf_task_sequence + SCHEMAS_DIR ${CMAKE_CURRENT_LIST_DIR} ) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp new file mode 100644 index 00000000..2d5b8bf8 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2021 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__EVENTS__INTERNAL_SEQUENCE_HPP +#define SRC__RMF_TASK_SEQUENCE__EVENTS__INTERNAL_SEQUENCE_HPP + +#include + +namespace rmf_task_sequence { +namespace events { +namespace internal { + +class Sequence +{ +public: + + class Standby; + class Active; + +}; + +} // namespace internal +} // namespace events +} // namespace rmf_task_sequence + +#endif // SRC__RMF_TASK_SEQUENCE__EVENTS__INTERNAL_SEQUENCE_HPP From df57b7ff9a9beece1b4499526fc052fddc860d5a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 10 Nov 2021 20:48:15 +0800 Subject: [PATCH 63/85] Add some conceptual sample schemas for task descriptions Signed-off-by: Michael X. Grey --- .../samples/description_Clean.schema.json | 17 +++++++++++ .../samples/description_PickUp.schema.json | 29 +++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 rmf_task_sequence/samples/description_Clean.schema.json create mode 100644 rmf_task_sequence/samples/description_PickUp.schema.json diff --git a/rmf_task_sequence/samples/description_Clean.schema.json b/rmf_task_sequence/samples/description_Clean.schema.json new file mode 100644 index 00000000..02736166 --- /dev/null +++ b/rmf_task_sequence/samples/description_Clean.schema.json @@ -0,0 +1,17 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://open-rmf.org/rmf_task_sequence/description_Clean/0.1", + "title": "Pick Up Description", + "description": "The description of a Pick Up task", + "properties": { + "zone": { + "type": "string", + "enum": ["pharmacy", "ward_0", "ward_1", "ward_2", "kitchen"] + }, + "type": { + "type": "string", + "enum": ["vacuum", "mop"] + } + }, + "required": ["zone", "type"] +} diff --git a/rmf_task_sequence/samples/description_PickUp.schema.json b/rmf_task_sequence/samples/description_PickUp.schema.json new file mode 100644 index 00000000..b14199ee --- /dev/null +++ b/rmf_task_sequence/samples/description_PickUp.schema.json @@ -0,0 +1,29 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://open-rmf.org/rmf_task_sequence/description_PickUp/0.1", + "title": "Pick Up Description", + "description": "The description of a Pick Up task", + "properties": { + "pickup_location": { + "type": "string", + "enum": ["pharmacy", "vending_machines", "kitchen"] + }, + "dispenser": { + "type": "string", + "enum": ["pharmacist", "soda_dispenser", "candy_dispenser", "chef"] + }, + "payload": { + "type": "array", + "items": { + "type": "object", + "properties": { + "sku": { "type": "string" }, + "quantity": { "type": "integer" }, + "compartment": { "type": "string" } + }, + "required": ["sku", "quantity"] + } + } + }, + "required": ["pickup_location", "dispenser", "payload"] +} From 4d6f0af80b4878b13dd51a639cc95fd207eba902 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 12 Nov 2021 15:37:35 +0800 Subject: [PATCH 64/85] Uncrustify Signed-off-by: Yadunund --- .../src/rmf_task_sequence/detail/ContactCard.cpp | 14 +++++++------- .../src/rmf_task_sequence/events/Call.cpp | 6 +++--- .../src/rmf_task_sequence/events/PerformAction.cpp | 14 +++++++------- .../src/rmf_task_sequence/events/Repeat.cpp | 8 ++++---- .../src/rmf_task_sequence/events/SMS.cpp | 8 ++++---- .../src/rmf_task_sequence/events/While.cpp | 10 +++++----- .../src/rmf_task_sequence/events/utils.hpp | 1 - 7 files changed, 30 insertions(+), 31 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp b/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp index d7cf8394..6f03a10b 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp @@ -18,7 +18,7 @@ #include namespace rmf_task_sequence { -namespace detail{ +namespace detail { //============================================================================== class ContactCard::Implementation @@ -38,12 +38,12 @@ ContactCard::ContactCard( const std::string& email, PhoneNumber number) : _pimpl(rmf_utils::make_impl( - Implementation{ - name, - address, - email, - std::move(number) - })) + Implementation{ + name, + address, + email, + std::move(number) + })) { // Do nothing } diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp index c0e19646..4bbaba89 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Call.cpp @@ -109,9 +109,9 @@ auto Call::Description::make( auto output = std::shared_ptr(new Description); output->_pimpl = rmf_utils::make_impl( - Implementation{ - std::move(contact), - wait_duration}); + Implementation{ + std::move(contact), + wait_duration}); return output; } diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp index bdb2aacb..2980063f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -61,13 +61,13 @@ PerformAction::Model::Model( { _invariant_battery_drain = parameters.ambient_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(_invariant_duration)); + rmf_traffic::time::to_seconds(_invariant_duration)); if (_use_tool_sink && parameters.tool_sink() != nullptr) { _invariant_battery_drain += parameters.tool_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(_invariant_duration)); + rmf_traffic::time::to_seconds(_invariant_duration)); } } @@ -209,11 +209,11 @@ Activity::ConstModelPtr PerformAction::Description::make_model( if (_pimpl->expected_finish_location.has_value()) { const auto& goal = _pimpl->expected_finish_location.value(); - invariant_finish_state.waypoint(goal.waypoint()); - if (goal.orientation() != nullptr) - { - invariant_finish_state.orientation(*goal.orientation()); - } + invariant_finish_state.waypoint(goal.waypoint()); + if (goal.orientation() != nullptr) + { + invariant_finish_state.orientation(*goal.orientation()); + } } return std::make_shared( diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp index 3a2be474..536fb77f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Repeat.cpp @@ -46,16 +46,16 @@ auto Repeat::Description::make( { /* INDENT-OFF */ throw std::invalid_argument( - "The number of repetitions has to be greater than zero"); + "The number of repetitions has to be greater than zero"); /* INDENT-ON */ } auto output = std::shared_ptr(new Description); output->_pimpl = rmf_utils::make_impl( - Implementation{ - std::move(event), - repetitions}); + Implementation{ + std::move(event), + repetitions}); return output; } diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp index cb5221da..32d2f9f0 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/SMS.cpp @@ -111,10 +111,10 @@ auto SMS::Description::make( auto output = std::shared_ptr(new Description); output->_pimpl = rmf_utils::make_impl( - Implementation{ - message, - std::move(contact), - wait_duration}); + Implementation{ + message, + std::move(contact), + wait_duration}); return output; } diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp index c0cf5256..ef39e0c7 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/While.cpp @@ -48,10 +48,10 @@ auto While::Description::make( auto output = std::shared_ptr(new Description); output->_pimpl = rmf_utils::make_impl( - Implementation{ - std::move(event), - std::move(completed), - while_duration_estimate}); + Implementation{ + std::move(event), + std::move(completed), + while_duration_estimate}); return output; } @@ -116,7 +116,7 @@ Activity::ConstModelPtr While::Description::make_model( // TODO(YV) Warn users if while_duration_estimate is smaller than // event estimate const std::size_t repetitions = std::max((std::size_t)1, static_cast< - std::size_t>(_pimpl->while_duration_estimate / event_estimate)); + std::size_t>(_pimpl->while_duration_estimate / event_estimate)); const auto repeat_event = Repeat::Description::make( _pimpl->event, diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp index 9259cfca..3e80ff7e 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp @@ -1,4 +1,3 @@ - /* * Copyright (C) 2021 Open Source Robotics Foundation * From 6d6d7c6ecc0851617a736a20e6241195f65a21a3 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 12 Nov 2021 18:20:56 +0800 Subject: [PATCH 65/85] Fix some of the linker errors Signed-off-by: Yadunund --- rmf_task_sequence/CMakeLists.txt | 9 +-- .../rmf_task_sequence/detail/Backup.hpp | 1 + .../rmf_task_sequence/detail/ContactCard.hpp | 19 +++-- rmf_task_sequence/package.xml | 2 +- .../src/rmf_task_sequence/Task.cpp | 78 ++++++++++++++++++- .../src/rmf_task_sequence/detail/Backup.cpp | 63 +++++++++++++++ .../rmf_task_sequence/detail/ContactCard.cpp | 4 +- .../test/unit/events/test_ContactCard.cpp | 33 ++++++++ 8 files changed, 189 insertions(+), 20 deletions(-) create mode 100644 rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_ContactCard.cpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 0fcfdf6f..2adf559a 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -25,13 +25,11 @@ find_package(nlohmann_json REQUIRED) find_package(nlohmann_json_schema_validator_vendor REQUIRED) find_package(ament_cmake_catch2 QUIET) -find_package(rmf_cmake_uncrustify QUIET) +find_package(ament_cmake_uncrustify QUIET) # ===== RMF Task Sequence Library file(GLOB_RECURSE lib_srcs "src/rmf_task_sequence/*.cpp" - "src/rmf_task_sequence/phases/*.cpp" - "src/rmf_task_sequence/events/*.cpp" ) add_library(rmf_task_sequence SHARED @@ -52,7 +50,7 @@ target_include_directories(rmf_task_sequence $ # for auto-generated headers ) -if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) +if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND ament_cmake_uncrustify_FOUND) file(GLOB_RECURSE unit_test_srcs "test/*.cpp") ament_add_catch2( @@ -60,6 +58,7 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) TIMEOUT 300) target_link_libraries(test_rmf_task_sequence rmf_task_sequence + rmf_task::rmf_task ) target_include_directories(test_rmf_task_sequence @@ -71,7 +70,7 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - rmf_uncrustify( + ament_uncrustify( ARGN include src test CONFIG_FILE ${uncrustify_config_file} MAX_LINE_LENGTH 80 diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp index 8f000aef..0d719726 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp @@ -57,6 +57,7 @@ class Backup class Implementation; private: + Backup(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp index 9c56c92b..c55c04ee 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/ContactCard.hpp @@ -23,22 +23,21 @@ namespace rmf_task_sequence { namespace detail { -//============================================================================== -struct PhoneNumber -{ - /// The country code without "+"" - uint32_t country_code; - - /// The phone number - uint32_t number; -}; - //============================================================================== /// Store contact details for telecommunication class ContactCard { public: + struct PhoneNumber + { + /// The country code without "+"" + uint32_t country_code; + + /// The phone number + uint32_t number; + }; + /// Constructor /// /// \param[in] name diff --git a/rmf_task_sequence/package.xml b/rmf_task_sequence/package.xml index f8c0889f..810556d5 100644 --- a/rmf_task_sequence/package.xml +++ b/rmf_task_sequence/package.xml @@ -14,7 +14,7 @@ nlohmann_json_schema_validator_vendor ament_cmake_catch2 - rmf_cmake_uncrustify + ament_cmake_uncrustify cmake diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 0b897c8e..3d7d9c21 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -145,7 +145,7 @@ class Task::Active void rewind(uint64_t phase_id) final; /// Get a weak reference to this object - std::weak_ptr weak_from_this() const; + // std::weak_ptr weak_from_this() const; static const nlohmann::json_schema::json_validator backup_schema_validator; @@ -198,7 +198,16 @@ class Task::Active _pending_stages(Description::Implementation::get_stages(description)), _cancel_sequence_initial_id(_pending_stages.size()+1) { - // Do nothing + const auto model = description.make_model( + std::chrono::steady_clock::now(), + *_parameters); + // TODO: fix + const auto header = Header( + "", + "", + model->invariant_duration()); + + _tag = std::make_shared(_booking, header); } Phase::ConstActivatorPtr _phase_activator; @@ -206,6 +215,7 @@ class Task::Active std::function _get_state; ConstParametersPtr _parameters; ConstBookingPtr _booking; + ConstTagPtr _tag; std::function _update; std::function _checkpoint; std::function _phase_finished; @@ -256,6 +266,39 @@ auto Task::Builder::add_phase( return *this; } +//============================================================================== +const std::vector& +Task::Active::completed_phases() const +{ + return _completed_phases; +} + +//============================================================================== +Phase::ConstActivePtr Task::Active::active_phase() const +{ + return _active_phase; +} + +//============================================================================== +const std::vector& Task::Active::pending_phases() const +{ + return _pending_phases; +} + +//============================================================================== +const Task::ConstTagPtr& Task::Active::tag() const +{ + return _tag; +} + +//============================================================================== +rmf_traffic::Time Task::Active::estimate_finish_time() const +{ + // TODO fix + const auto now = _clock(); + return now; +} + //============================================================================== auto Task::Active::backup() const -> Backup { @@ -264,6 +307,37 @@ auto Task::Active::backup() const -> Backup _active_phase->backup()); } +//============================================================================== +Task::Active::Resume Task::Active::interrupt( + std::function task_is_interrupted) +{ + return rmf_task::Task::Active::make_resumer(task_is_interrupted); +} + +//============================================================================== +void Task::Active::cancel() +{ + // TODO +} + +//============================================================================== +void Task::Active::kill() +{ + // TODO +} + +//============================================================================== +void Task::Active::skip(uint64_t phase_id, bool value) +{ + // TODO +} + +//============================================================================== +void Task::Active::rewind(uint64_t phase_id) +{ + // TODO +} + //============================================================================== void Task::Active::_load_backup(std::string backup_state_str) { diff --git a/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp b/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp new file mode 100644 index 00000000..8d172910 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { +namespace detail { + +//============================================================================== +class Backup::Implementation +{ +public: + + uint64_t seq; + nlohmann::json state; +}; + +//============================================================================== +Backup::Backup() +{ + // Do nothing +} + +//============================================================================== +Backup Backup::make(uint64_t seq, nlohmann::json state) +{ + Backup backup; + backup._pimpl = rmf_utils::make_impl( + Implementation{seq, state}); + return backup; +} + +//============================================================================== +uint64_t Backup::sequence() const +{ + return _pimpl->seq; +} + +//============================================================================== +const nlohmann::json& Backup::state() const +{ + return _pimpl->state; +} + + + + +} // namespace detail +} // namespace rmf_task_sequence \ No newline at end of file diff --git a/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp b/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp index 6f03a10b..48bc2f77 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/detail/ContactCard.cpp @@ -88,13 +88,13 @@ ContactCard& ContactCard::email(const std::string& new_email) } //============================================================================== -const PhoneNumber& ContactCard::number() const +const ContactCard::PhoneNumber& ContactCard::number() const { return _pimpl->number; } //============================================================================== -ContactCard& ContactCard::number(PhoneNumber new_number) +ContactCard& ContactCard::number(ContactCard::PhoneNumber new_number) { _pimpl->number = std::move(new_number); return *this; diff --git a/rmf_task_sequence/test/unit/events/test_ContactCard.cpp b/rmf_task_sequence/test/unit/events/test_ContactCard.cpp new file mode 100644 index 00000000..8840bdd3 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_ContactCard.cpp @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2021 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 + +#include + +SCENARIO("Test ContactCard") +{ + using ContactCard = rmf_task_sequence::detail::ContactCard; + using PhoneNumber = ContactCard::PhoneNumber; + + auto contact = ContactCard{ + "foo", + "bar", + "baz", + PhoneNumber{42, 11311} + }; +} \ No newline at end of file From 92f911b3c38a6bfc64de121da2f5356649bc1a85 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 12 Nov 2021 18:27:50 +0800 Subject: [PATCH 66/85] Temporary fix for linker problems Signed-off-by: Yadunund --- rmf_task_sequence/src/rmf_task_sequence/Task.cpp | 8 ++++---- rmf_task_sequence/test/unit/events/test_ContactCard.cpp | 6 ++++++ 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 3d7d9c21..6dc09a2e 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -198,14 +198,14 @@ class Task::Active _pending_stages(Description::Implementation::get_stages(description)), _cancel_sequence_initial_id(_pending_stages.size()+1) { - const auto model = description.make_model( - std::chrono::steady_clock::now(), - *_parameters); + // const auto model = description.make_model( + // std::chrono::steady_clock::now(), + // *_parameters); // TODO: fix const auto header = Header( "", "", - model->invariant_duration()); + rmf_traffic::Duration{0}); _tag = std::make_shared(_booking, header); } diff --git a/rmf_task_sequence/test/unit/events/test_ContactCard.cpp b/rmf_task_sequence/test/unit/events/test_ContactCard.cpp index 8840bdd3..aa43c552 100644 --- a/rmf_task_sequence/test/unit/events/test_ContactCard.cpp +++ b/rmf_task_sequence/test/unit/events/test_ContactCard.cpp @@ -30,4 +30,10 @@ SCENARIO("Test ContactCard") "baz", PhoneNumber{42, 11311} }; + CHECK(contact.name() == "foo"); + CHECK(contact.address() == "bar"); + CHECK(contact.email() == "baz"); + CHECK(contact.number().country_code == 42); + CHECK(contact.number().number == 11311); + } \ No newline at end of file From 7207547d740907087ef891fdd9c81f4e2603d269 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 17 Nov 2021 13:43:40 +0800 Subject: [PATCH 67/85] Added test for Call Signed-off-by: Yadunund --- rmf_task_sequence/CMakeLists.txt | 2 - .../src/rmf_task_sequence/detail/Backup.cpp | 4 +- .../test/unit/events/test_Call.cpp | 101 ++++++++ .../test/unit/events/test_ContactCard.cpp | 27 ++- rmf_task_sequence/test/unit/utils.hpp | 216 ++++++++++++++++++ 5 files changed, 344 insertions(+), 6 deletions(-) create mode 100644 rmf_task_sequence/test/unit/events/test_Call.cpp create mode 100644 rmf_task_sequence/test/unit/utils.hpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 2adf559a..258238fd 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -130,5 +130,3 @@ export( FILE ${CMAKE_CURRENT_BINARY_DIR}/rmf_task_sequence-targets.cmake NAMESPACE rmf_task_sequence:: ) - -export(PACKAGE rmf_task_sequence) diff --git a/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp b/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp index 8d172910..91a8a2a0 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp @@ -40,7 +40,7 @@ Backup Backup::make(uint64_t seq, nlohmann::json state) { Backup backup; backup._pimpl = rmf_utils::make_impl( - Implementation{seq, state}); + Implementation{seq, state}); return backup; } @@ -57,7 +57,5 @@ const nlohmann::json& Backup::state() const } - - } // namespace detail } // namespace rmf_task_sequence \ No newline at end of file diff --git a/rmf_task_sequence/test/unit/events/test_Call.cpp b/rmf_task_sequence/test/unit/events/test_Call.cpp new file mode 100644 index 00000000..0a973cfb --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_Call.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2021 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 + +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test Call") +{ + using ContactCard = rmf_task_sequence::detail::ContactCard; + using PhoneNumber = ContactCard::PhoneNumber; + using Call = rmf_task_sequence::events::Call; + + const auto number = PhoneNumber{42, 11311}; + const auto contact = ContactCard{ + "foo", + "bar", + "baz", + number + }; + + const auto duration = 10s; + auto description = Call::Description::make(contact, duration); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK_CONTACT(description->contact(), "foo", "bar", "baz", number); + CHECK(description->call_duration_estimate() == duration); + } + + WHEN("Testing setters") + { + const auto new_number = PhoneNumber{11311, 42}; + description->contact( + ContactCard{"FOO", "BAR", "BAZ", new_number}); + CHECK_CONTACT(description->contact(), "FOO", "BAR", "BAZ", new_number); + + description->call_duration_estimate(20s); + CHECK(description->call_duration_estimate() == 20s); + } + + WHEN("Testing model") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(initial_state.time().has_value()); + expected_finish_state.time(initial_state.time().value() + duration); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state, + duration); + } + + WHEN("Testing header") + { + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() - duration < 100ms); + } +} diff --git a/rmf_task_sequence/test/unit/events/test_ContactCard.cpp b/rmf_task_sequence/test/unit/events/test_ContactCard.cpp index aa43c552..e8e24912 100644 --- a/rmf_task_sequence/test/unit/events/test_ContactCard.cpp +++ b/rmf_task_sequence/test/unit/events/test_ContactCard.cpp @@ -36,4 +36,29 @@ SCENARIO("Test ContactCard") CHECK(contact.number().country_code == 42); CHECK(contact.number().number == 11311); -} \ No newline at end of file + WHEN("Setting name") + { + contact.name("FOO"); + CHECK(contact.name() == "FOO"); + } + + WHEN("Setting address") + { + contact.address("BAR"); + CHECK(contact.address() == "BAR"); + } + + WHEN("Setting email") + { + contact.email("BAZ"); + CHECK(contact.email() == "BAZ"); + } + + WHEN("Setting number") + { + contact.number(PhoneNumber{11311, 42}); + CHECK(contact.number().country_code == 11311); + CHECK(contact.number().number == 42); + } + +} diff --git a/rmf_task_sequence/test/unit/utils.hpp b/rmf_task_sequence/test/unit/utils.hpp new file mode 100644 index 00000000..0454fd1a --- /dev/null +++ b/rmf_task_sequence/test/unit/utils.hpp @@ -0,0 +1,216 @@ +/* + * Copyright (C) 2021 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 TEST__UNIT__UTILS_HPP +#define TEST__UNIT__UTILS_HPP + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace { +using namespace std::chrono_literals; +//============================================================================== +void CHECK_CONTACT( + const rmf_task_sequence::detail::ContactCard& contact, + const std::string& name, + const std::string& address, + const std::string& email, + const rmf_task_sequence::detail::ContactCard::PhoneNumber& number) +{ + CHECK(contact.name() == name); + CHECK(contact.address() == address); + CHECK(contact.email() == email); + CHECK(contact.number().country_code == number.country_code); + CHECK(contact.number().number == number.number); +} + +//============================================================================== +std::shared_ptr make_test_constraints( + bool drain_battery = true) +{ + return std::make_shared( + 0.1, + 1.0, + drain_battery); +} + +//============================================================================== +std::shared_ptr make_test_parameters( + bool drain_battery = true) +{ + using BatterySystem = rmf_battery::agv::BatterySystem; + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + using PowerSystem = rmf_battery::agv::PowerSystem; + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + + const std::string test_map_name = "test_map"; + rmf_traffic::agv::Graph graph; + graph.add_waypoint(test_map_name, {-5, -5}).set_passthrough_point(true); // 0 + graph.add_waypoint(test_map_name, { 0, -5}).set_passthrough_point(true); // 1 + graph.add_waypoint(test_map_name, { 5, -5}).set_passthrough_point(true); // 2 + graph.add_waypoint(test_map_name, {10, -5}).set_passthrough_point(true); // 3 + graph.add_waypoint(test_map_name, {-5, 0}); // 4 + graph.add_waypoint(test_map_name, { 0, 0}); // 5 + graph.add_waypoint(test_map_name, { 5, 0}); // 6 + graph.add_waypoint(test_map_name, {10, 0}).set_passthrough_point(true); // 7 + graph.add_waypoint(test_map_name, {10, 4}).set_passthrough_point(true); // 8 + graph.add_waypoint(test_map_name, { 0, 8}).set_passthrough_point(true); // 9 + graph.add_waypoint(test_map_name, { 5, 8}).set_passthrough_point(true); // 10 + graph.add_waypoint(test_map_name, {10, 12}).set_passthrough_point(true); // 11 + graph.add_waypoint(test_map_name, {12, 12}).set_passthrough_point(true); // 12 + REQUIRE(graph.num_waypoints() == 13); + + auto add_bidir_lane = [&](const std::size_t w0, const std::size_t w1) + { + graph.add_lane(w0, w1); + graph.add_lane(w1, w0); + }; + + add_bidir_lane(0, 1); + add_bidir_lane(1, 2); + add_bidir_lane(2, 3); + add_bidir_lane(1, 5); + add_bidir_lane(3, 7); + add_bidir_lane(4, 5); + add_bidir_lane(6, 10); + add_bidir_lane(7, 8); + add_bidir_lane(9, 10); + add_bidir_lane(10, 11); + + const auto shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(1.0); + const rmf_traffic::Profile profile{shape, shape}; + const rmf_traffic::agv::VehicleTraits traits( + {1.0, 0.7}, {0.6, 0.5}, profile); + rmf_traffic::schedule::Database database; + const auto default_planner_options = rmf_traffic::agv::Planner::Options{ + nullptr}; + + auto planner = std::make_shared( + rmf_traffic::agv::Planner::Configuration{graph, traits}, + default_planner_options); + + auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); + REQUIRE(battery_system_optional); + BatterySystem& battery_system = *battery_system_optional; + auto mechanical_system_optional = MechanicalSystem::make(70.0, 40.0, 0.22); + REQUIRE(mechanical_system_optional); + MechanicalSystem& mechanical_system = *mechanical_system_optional; + auto power_system_optional = PowerSystem::make(20.0); + REQUIRE(power_system_optional); + PowerSystem& power_system_processor = *power_system_optional; + + std::shared_ptr motion_sink = + std::make_shared(battery_system, mechanical_system); + std::shared_ptr device_sink = + std::make_shared(battery_system, + power_system_processor); + + return std::make_shared( + planner, + battery_system, + motion_sink, + device_sink); + +} + +//============================================================================== +void CHECK_STATE( + const rmf_task::State& estimated_state, + const rmf_task::State& expected_state) +{ + if (expected_state.waypoint().has_value()) + { + REQUIRE(estimated_state.waypoint().has_value()); + CHECK( + expected_state.waypoint().value() == estimated_state.waypoint().value()); + } + + if (expected_state.orientation().has_value()) + { + REQUIRE(estimated_state.orientation().has_value()); + CHECK(std::abs(expected_state.orientation().value() - + estimated_state.orientation().value()) < 1e-3); + } + + if (expected_state.time().has_value()) + { + REQUIRE(estimated_state.time().has_value()); + CHECK(expected_state.time().value() - + estimated_state.time().value() < 100ms); + } + + if (expected_state.dedicated_charging_waypoint().has_value()) + { + REQUIRE(estimated_state.dedicated_charging_waypoint().has_value()); + CHECK(expected_state.dedicated_charging_waypoint().value() == + estimated_state.dedicated_charging_waypoint().value()); + } + + if (expected_state.battery_soc().has_value()) + { + REQUIRE(estimated_state.battery_soc().has_value()); + CHECK(estimated_state.battery_soc().value() <= + expected_state.battery_soc().value()); + } +} + +//============================================================================== +// TODO(YV): Also check for invariant_finish_state +void CHECK_MODEL( + const rmf_task_sequence::Activity::Model& model, + const rmf_task::State& initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator, + const rmf_task::State& expected_finish_state, + const rmf_traffic::Duration expected_invariant_duration) +{ + + const auto estimated_finish = model.estimate_finish( + initial_state, + constraints, + travel_estimator); + + REQUIRE(estimated_finish.has_value()); + + CHECK_STATE(estimated_finish.value(), expected_finish_state); + + CHECK(model.invariant_duration() - expected_invariant_duration < 100ms); +} + +} // anonymous namespace + +#endif // TEST__UNIT__UTILS_HPP From 0be7812fc2e0dbf4a75d1186c25ffba04ed5a1bf Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 17 Nov 2021 18:31:02 +0800 Subject: [PATCH 68/85] Added remaining tests Signed-off-by: Yadunund --- .../test/unit/events/test_Call.cpp | 7 +- .../test/unit/events/test_GoToPlace.cpp | 99 ++++++++++++++++ .../test/unit/events/test_PerformAction.cpp | 101 ++++++++++++++++ .../test/unit/events/test_Repeat.cpp | 94 +++++++++++++++ .../test/unit/events/test_SMS.cpp | 109 ++++++++++++++++++ .../test/unit/events/test_WaitFor.cpp | 84 ++++++++++++++ .../test/unit/events/test_While.cpp | 94 +++++++++++++++ rmf_task_sequence/test/unit/utils.hpp | 21 +++- 8 files changed, 602 insertions(+), 7 deletions(-) create mode 100644 rmf_task_sequence/test/unit/events/test_GoToPlace.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_PerformAction.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_Repeat.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_SMS.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_WaitFor.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_While.cpp diff --git a/rmf_task_sequence/test/unit/events/test_Call.cpp b/rmf_task_sequence/test/unit/events/test_Call.cpp index 0a973cfb..19a470d0 100644 --- a/rmf_task_sequence/test/unit/events/test_Call.cpp +++ b/rmf_task_sequence/test/unit/events/test_Call.cpp @@ -76,7 +76,7 @@ SCENARIO("Test Call") const auto travel_estimator = rmf_task::TravelEstimator(*parameters); rmf_task::State expected_finish_state = initial_state; - REQUIRE(initial_state.time().has_value()); + REQUIRE(expected_finish_state.time().has_value()); expected_finish_state.time(initial_state.time().value() + duration); CHECK_MODEL( @@ -84,8 +84,7 @@ SCENARIO("Test Call") initial_state, *constraints, travel_estimator, - expected_finish_state, - duration); + expected_finish_state); } WHEN("Testing header") @@ -96,6 +95,6 @@ SCENARIO("Test Call") CHECK(!header.category().empty()); CHECK(!header.detail().empty()); - CHECK(header.original_duration_estimate() - duration < 100ms); + CHECK(header.original_duration_estimate() == duration); } } diff --git a/rmf_task_sequence/test/unit/events/test_GoToPlace.cpp b/rmf_task_sequence/test/unit/events/test_GoToPlace.cpp new file mode 100644 index 00000000..67be12f1 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_GoToPlace.cpp @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2021 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 + +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test GoToPlace") +{ + using GoToPlace = rmf_task_sequence::events::GoToPlace; + using Goal = GoToPlace::Goal; + + const auto goal = Goal{1}; + auto description = GoToPlace::Description::make(goal); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->destination().waypoint() == goal.waypoint()); + CHECK_FALSE(description->destination().orientation()); + CHECK(description->destination_name(*parameters) == "#1"); + } + + WHEN("Testing setters") + { + description->destination(Goal{2, 1.57}); + CHECK(description->destination().waypoint() == 2); + REQUIRE(description->destination().orientation()); + CHECK(abs( + *description->destination().orientation() - 1.57) < 1e-3); + CHECK(description->destination_name(*parameters) == "#2"); + } + + WHEN("Testing model and header") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(expected_finish_state.time().has_value()); + const auto duration_opt = estimate_travel_duration( + parameters->planner(), + initial_state, + goal); + REQUIRE(duration_opt.has_value()); + expected_finish_state.waypoint(goal.waypoint()) + .time(initial_state.time().value() + duration_opt.value()); + + if (goal.orientation()) + expected_finish_state.orientation(*goal.orientation()); + else + expected_finish_state.erase(); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == duration_opt.value()); + } +} diff --git a/rmf_task_sequence/test/unit/events/test_PerformAction.cpp b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp new file mode 100644 index 00000000..379f24bc --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2021 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 + +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test PerformAction") +{ + using PerformAction = rmf_task_sequence::events::PerformAction; + const auto duration = 10s; + const std::string action_name = "test_action"; + const rmf_traffic::agv::Planner::Goal finish_location{1}; + + auto description = PerformAction::Description::make( + action_name, + duration, + false, + finish_location); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->action_name() == action_name); + CHECK(description->action_duration_estimate() == duration); + CHECK(description->use_tool_sink() == false); + REQUIRE(description->expected_finish_location().has_value()); + CHECK(description->expected_finish_location().value().waypoint() == 1); + } + + WHEN("Testing setters") + { + description->action_name("new_name"); + CHECK(description->action_name() == "new_name"); + description->action_duration_estimate(20s); + CHECK(description->action_duration_estimate() == 20s); + description->use_tool_sink(true); + CHECK(description->use_tool_sink() == true); + description->expected_finish_location(std::nullopt); + CHECK_FALSE(description->expected_finish_location().has_value()); + } + + WHEN("Testing model") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(initial_state.time().has_value()); + expected_finish_state.time(initial_state.time().value() + duration) + .waypoint(1); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + } + + WHEN("Testing header") + { + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == duration); + } +} diff --git a/rmf_task_sequence/test/unit/events/test_Repeat.cpp b/rmf_task_sequence/test/unit/events/test_Repeat.cpp new file mode 100644 index 00000000..d319f5d0 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_Repeat.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2021 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 + +#include +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test Repeat") +{ + using WaitFor = rmf_task_sequence::events::WaitFor; + using Repeat = rmf_task_sequence::events::Repeat; + + const auto duration = 60s; + const auto event = WaitFor::Description::make(duration); + const std::size_t repetitions = 5; + + auto description = Repeat::Description::make(event, repetitions); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->event() == event); + CHECK(description->repetitions() == repetitions); + } + + WHEN("Testing setters") + { + const auto new_event = WaitFor::Description::make(30s); + description->event(new_event); + // TODO(YV): More meaningful equality checks + CHECK(description->event() != event); + + description->repetitions(10); + CHECK(description->repetitions() == 10); + } + + WHEN("Testing model and header") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(expected_finish_state.time().has_value()); + const auto total_duration = repetitions * duration; + expected_finish_state + .time(initial_state.time().value() + total_duration); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == total_duration); + } +} diff --git a/rmf_task_sequence/test/unit/events/test_SMS.cpp b/rmf_task_sequence/test/unit/events/test_SMS.cpp new file mode 100644 index 00000000..d35f3d57 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_SMS.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2021 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 + +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test SMS") +{ + using ContactCard = rmf_task_sequence::detail::ContactCard; + using PhoneNumber = ContactCard::PhoneNumber; + using SMS = rmf_task_sequence::events::SMS; + + const std::string message = "hello world"; + const auto number = PhoneNumber{42, 11311}; + const auto contact = ContactCard{ + "foo", + "bar", + "baz", + number + }; + + const auto duration = 1s; + auto description = SMS::Description::make( + message, + contact, + duration); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->message() == message); + CHECK_CONTACT(description->contact(), "foo", "bar", "baz", number); + CHECK(description->sms_duration_estimate() == duration); + } + + WHEN("Testing setters") + { + const std::string new_message = "Hello World!!"; + description->message(new_message); + CHECK(description->message() == new_message); + + const auto new_number = PhoneNumber{11311, 42}; + description->contact( + ContactCard{"FOO", "BAR", "BAZ", new_number}); + CHECK_CONTACT(description->contact(), "FOO", "BAR", "BAZ", new_number); + + description->sms_duration_estimate(20s); + CHECK(description->sms_duration_estimate() == 20s); + } + + WHEN("Testing model") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(expected_finish_state.time().has_value()); + expected_finish_state.time(initial_state.time().value() + duration); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + } + + WHEN("Testing header") + { + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == duration); + } +} diff --git a/rmf_task_sequence/test/unit/events/test_WaitFor.cpp b/rmf_task_sequence/test/unit/events/test_WaitFor.cpp new file mode 100644 index 00000000..283376d7 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_WaitFor.cpp @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2021 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 + +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test WaitFor") +{ + using WaitFor = rmf_task_sequence::events::WaitFor; + + const auto duration = 10s; + auto description = WaitFor::Description::make(duration); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->duration() == duration); + } + + WHEN("Testing setters") + { + description->duration(20s); + CHECK(description->duration() == 20s); + } + + WHEN("Testing model") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(expected_finish_state.time().has_value()); + expected_finish_state.time(initial_state.time().value() + duration); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + } + + WHEN("Testing header") + { + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == duration); + } +} diff --git a/rmf_task_sequence/test/unit/events/test_While.cpp b/rmf_task_sequence/test/unit/events/test_While.cpp new file mode 100644 index 00000000..d98247e8 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_While.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2021 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 + +#include +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test While") +{ + using WaitFor = rmf_task_sequence::events::WaitFor; + using While = rmf_task_sequence::events::While; + + const auto duration = 300s; + const auto event = WaitFor::Description::make(10s); + const auto completed = []() -> bool {return true;}; + auto description = While::Description::make(event, completed, duration); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->event() == event); + CHECK(description->while_duration_estimate() == duration); + } + + WHEN("Testing setters") + { + const auto new_event = WaitFor::Description::make(30s); + description->event(new_event); + // TODO(YV): More meaningful equality checks + CHECK(description->event() != event); + + description->while_duration_estimate(60s); + CHECK(description->while_duration_estimate() == 60s); + } + + WHEN("Testing model and header") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(expected_finish_state.time().has_value()); + const auto repetitions = (int)(duration / event->duration()); + const auto total_duration = repetitions * event->duration(); + expected_finish_state + .time(initial_state.time().value() + total_duration); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == total_duration); + } +} diff --git a/rmf_task_sequence/test/unit/utils.hpp b/rmf_task_sequence/test/unit/utils.hpp index 0454fd1a..ece169fb 100644 --- a/rmf_task_sequence/test/unit/utils.hpp +++ b/rmf_task_sequence/test/unit/utils.hpp @@ -195,8 +195,7 @@ void CHECK_MODEL( const rmf_task::State& initial_state, const rmf_task::Constraints& constraints, const rmf_task::TravelEstimator& travel_estimator, - const rmf_task::State& expected_finish_state, - const rmf_traffic::Duration expected_invariant_duration) + const rmf_task::State& expected_finish_state) { const auto estimated_finish = model.estimate_finish( @@ -207,8 +206,24 @@ void CHECK_MODEL( REQUIRE(estimated_finish.has_value()); CHECK_STATE(estimated_finish.value(), expected_finish_state); +} + +//============================================================================== +std::optional estimate_travel_duration( + const std::shared_ptr& planner, + const rmf_task::State& initial_state, + const rmf_traffic::agv::Planner::Goal& goal) +{ + const auto result = + planner->setup(initial_state.project_plan_start().value(), goal); + + if (result.disconnected()) + return std::nullopt; + + if (!result.ideal_cost().has_value()) + return std::nullopt; - CHECK(model.invariant_duration() - expected_invariant_duration < 100ms); + return rmf_traffic::time::from_seconds(*result.ideal_cost()); } } // anonymous namespace From a0e39f7efb97693238a3e8fc05b80f1b5e0cadc2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 24 Nov 2021 14:03:46 +0800 Subject: [PATCH 69/85] Move schemas into subdirectory Signed-off-by: Michael X. Grey --- .../schemas/{ => backup}/backup_EventSequence_v0_1.schema.json | 0 .../{ => backup}/backup_PhaseSequenceTask_v0_1.schema.json | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename rmf_task_sequence/schemas/{ => backup}/backup_EventSequence_v0_1.schema.json (100%) rename rmf_task_sequence/schemas/{ => backup}/backup_PhaseSequenceTask_v0_1.schema.json (100%) diff --git a/rmf_task_sequence/schemas/backup_EventSequence_v0_1.schema.json b/rmf_task_sequence/schemas/backup/backup_EventSequence_v0_1.schema.json similarity index 100% rename from rmf_task_sequence/schemas/backup_EventSequence_v0_1.schema.json rename to rmf_task_sequence/schemas/backup/backup_EventSequence_v0_1.schema.json diff --git a/rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json b/rmf_task_sequence/schemas/backup/backup_PhaseSequenceTask_v0_1.schema.json similarity index 100% rename from rmf_task_sequence/schemas/backup_PhaseSequenceTask_v0_1.schema.json rename to rmf_task_sequence/schemas/backup/backup_PhaseSequenceTask_v0_1.schema.json From 3610da51615459c13dbd742ac1f080beb96ba565 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 30 Nov 2021 15:48:48 +0800 Subject: [PATCH 70/85] tweaks Signed-off-by: Michael X. Grey --- rmf_task_sequence/CMakeLists.txt | 6 +++++- rmf_task_sequence/schemas/CMakeLists.txt | 17 ----------------- .../src/rmf_task_sequence/events/Bundle.cpp | 7 ++++--- .../src/rmf_task_sequence/events/Sequence.cpp | 17 +++++++++++++++++ 4 files changed, 26 insertions(+), 21 deletions(-) delete mode 100644 rmf_task_sequence/schemas/CMakeLists.txt create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 38cb74fd..4a78b638 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -78,7 +78,11 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) endif() # Generate the schema headers -add_subdirectory(schemas) +rmf_api_generate_schema_headers( + PACKAGE rmf_task_sequence + SCHEMAS_DIR ${CMAKE_CURRENT_LIST_DIR}/schemas +) + # Create cmake config files include(CMakePackageConfigHelpers) diff --git a/rmf_task_sequence/schemas/CMakeLists.txt b/rmf_task_sequence/schemas/CMakeLists.txt deleted file mode 100644 index 97421707..00000000 --- a/rmf_task_sequence/schemas/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -#include(../cmake/generate_schema_header.cmake) - -#file(GLOB schema_files "*.schema.json") - -#foreach(schema_file ${schema_files}) -# generate_schema_header(${schema_file}) -#endforeach() - -#install( -# DIRECTORY ${CMAKE_BINARY_DIR}/include/ -# DESTINATION include -#) - -rmf_api_generate_schema_headers( - PACKAGE rmf_task_sequence - SCHEMAS_DIR ${CMAKE_CURRENT_LIST_DIR} -) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index 5a764408..fe4c37dd 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -67,12 +67,12 @@ class Bundle::Description::Implementation case Type::Sequence: return "Sequence"; case Type::ParallelAll: - return "Parallel All"; + return "All of"; case Type::ParallelAny: return "One of"; } - return ""; + return ""; } rmf_traffic::Duration adjust_estimate( @@ -164,7 +164,8 @@ auto Bundle::Description::dependencies() const -> const Dependencies& } //============================================================================== -auto Bundle::Description::dependencies(Dependencies new_dependencies) -> Description& +auto Bundle::Description::dependencies(Dependencies new_dependencies) +-> Description& { _pimpl->dependencies = std::move(new_dependencies); return *this; diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp new file mode 100644 index 00000000..feb743c5 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -0,0 +1,17 @@ +/* + * Copyright (C) 2021 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. + * +*/ + From 10fd78ab5772a3485b25ba4f5acf9ea4cca53d1d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 7 Dec 2021 13:41:32 +0800 Subject: [PATCH 71/85] Refactor Sequence into Bundle Signed-off-by: Michael X. Grey --- .../src/rmf_task_sequence/events/Bundle.cpp | 441 +++--------------- .../src/rmf_task_sequence/events/Sequence.cpp | 345 ++++++++++++++ .../events/internal_Sequence.hpp | 125 +++++ 3 files changed, 525 insertions(+), 386 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index fe4c37dd..868985ec 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -24,6 +24,8 @@ #include #include +#include "internal_Sequence.hpp" + #include namespace rmf_task_sequence { @@ -45,6 +47,57 @@ nlohmann::json convert_to_json(const std::string& input) return output; } + +//============================================================================== +Event::StandbyPtr initiate( + const Event::Initializer& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + std::function parent_update) +{ + if (description.type() == Bundle::Type::Sequence) + { + return internal::Sequence::Standby::initiate( + initializer, + get_state, + parameters, + description, + std::move(parent_update)); + } + + throw std::runtime_error( + "Bundle type not yet implemented: " + std::to_string(description.type())); +} + +//============================================================================== +Event::ActivePtr restore( + const Event::Initializer& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + const std::string& backup, + std::function parent_update, + std::function checkpoint, + std::function finished) +{ + if (description.type() == Bundle::Type::Sequence) + { + return internal::Sequence::Active::restore( + initializer, + get_state, + parameters, + description, + backup, + parent_update, + checkpoint, + finished); + } + + throw std::runtime_error( + "Bundle type not yet implemented: " + std::to_string(description.type())); +} + } // anonymous namespace //============================================================================== @@ -218,390 +271,6 @@ Header Bundle::Description::generate_header( return _pimpl->generate_header(initial_state, parameters); } -//============================================================================== -class BoolGuard -{ -public: - BoolGuard(bool& value) - : _value(value) - { - _value = true; - } - - ~BoolGuard() - { - _value = false; - } - -private: - bool& _value; -}; - -//============================================================================== -class SequenceActive; - -//============================================================================== -class SequenceStandby : public Event::Standby -{ -public: - - static Event::StandbyPtr initiate( - const Event::Initializer& initializer, - const std::function& get_state, - const ConstParametersPtr& parameters, - const Bundle::Description& description, - std::function parent_update) - { - auto state = make_state(description); - const auto update = [parent_update = std::move(parent_update), state]() - { - update_status(*state); - parent_update(); - }; - - std::vector dependencies; - for (const auto& desc : description.dependencies()) - { - auto element = initializer.initialize( - get_state, parameters, *desc, update); - - state->add_dependency(element->state()); - - dependencies.emplace_back(std::move(element)); - } - - std::reverse(dependencies.begin(), dependencies.end()); - - update_status(*state); - return std::make_shared( - std::move(dependencies), std::move(state), std::move(parent_update)); - } - - Event::ConstStatePtr state() const final - { - return _state; - } - - rmf_traffic::Duration duration_estimate() const final - { - auto estimate = rmf_traffic::Duration(0); - for (const auto& element : _dependencies) - estimate += element->duration_estimate(); - - return estimate; - } - - Event::ActivePtr begin( - std::function checkpoint, - std::function finish) final; - - SequenceStandby( - std::vector dependencies, - rmf_task::events::SimpleEventPtr state, - std::function parent_update) - : _dependencies(std::move(dependencies)), - _state(std::move(state)), - _parent_update(std::move(parent_update)) - { - // Do nothing - } - - static rmf_task::events::SimpleEventPtr make_state( - const Bundle::Description& description) - { - return rmf_task::events::SimpleEvent::make( - description.category().value_or("Sequence"), - description.detail().value_or(""), - rmf_task::Event::Status::Standby); - } - - static void update_status(rmf_task::events::SimpleEvent& state) - { - if (state.status() == Event::Status::Canceled - || state.status() == Event::Status::Killed - || state.status() == Event::Status::Skipped) - return; - - Event::Status status = Event::Status::Completed; - for (const auto& dep : state.dependencies()) - status = Event::sequence_status(status, dep->status()); - - state.update_status(status); - } - -private: - - std::vector _dependencies; - rmf_task::events::SimpleEventPtr _state; - std::function _parent_update; - std::shared_ptr _active; -}; - -//============================================================================== -class SequenceActive - : public Event::Active, - public std::enable_shared_from_this -{ -public: - - static Event::ActivePtr restore( - const Event::Initializer& initializer, - const std::function& get_state, - const ConstParametersPtr& parameters, - const Bundle::Description& description, - const std::string& backup, - std::function parent_update, - std::function checkpoint, - std::function finished) - { - auto state = SequenceStandby::make_state(description); - const auto update = [parent_update = std::move(parent_update), state]() - { - SequenceStandby::update_status(*state); - parent_update(); - }; - - std::vector dependencies; - - const auto backup_state = nlohmann::json::parse(backup); - if (const auto result = - schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) - { - state->update_log().error( - "Parsing failed while restoring backup: " + result->message - + "\nOriginal backup state:\n```" + backup + "\n```"); - state->update_status(Event::Status::Error); - return std::make_shared( - dependencies, std::move(state), nullptr, nullptr, nullptr); - } - - const auto& current_event_json = backup_state["current_event"]; - const auto current_event_index = - current_event_json["index"].get(); - - const auto& element_descriptions = description.dependencies(); - if (element_descriptions.size() <= current_event_index) - { - state->update_log().error( - "Failed to restore backup. Index [" - + std::to_string(current_event_index) + "] is too high for [" - + std::to_string(description.dependencies().size()) + "] event dependencies. " - "Original text:\n```\n" + backup + "\n```"); - state->update_status(Event::Status::Error); - return std::make_shared( - dependencies, std::move(state), nullptr, nullptr, nullptr); - } - - auto active = std::make_shared( - current_event_index, - std::move(state), - parent_update, - std::move(checkpoint), - std::move(finished)); - - const auto event_finished = [me = active->weak_from_this()]() - { - if (const auto self = me.lock()) - self->next(); - }; - - const auto& current_event_state = current_event_json["state"]; - active->_current = initializer.restore( - get_state, - parameters, - *element_descriptions.at(current_event_index), - current_event_state, - update, - checkpoint, - event_finished); - state->add_dependency(active->state()); - - for (std::size_t i = current_event_index+1; i < element_descriptions.size(); ++i) - { - const auto& desc = element_descriptions[i]; - auto element = initializer.initialize( - get_state, parameters, *desc, update); - - active->_state->add_dependency(element->state()); - dependencies.emplace_back(std::move(element)); - } - - std::reverse(dependencies.begin(), dependencies.end()); - - active->_reverse_remaining = std::move(dependencies); - - BoolGuard lock(active->_inside_next); - while (active->_current->state()->finished()) - { - if (active->_reverse_remaining.empty()) - { - SequenceStandby::update_status(*active->_state); - return active; - } - - ++active->_current_event_index_plus_one; - const auto next_event = active->_reverse_remaining.back(); - active->_reverse_remaining.pop_back(); - active->_current = next_event->begin(active->_checkpoint, event_finished); - } - - SequenceStandby::update_status(*active->_state); - return active; - } - - Event::ConstStatePtr state() const final - { - return _state; - } - - rmf_traffic::Duration remaining_time_estimate() const final - { - auto estimate = rmf_traffic::Duration(0); - if (_current) - estimate += _current->remaining_time_estimate(); - - for (const auto& element : _reverse_remaining) - estimate += element->duration_estimate(); - - return estimate; - } - - Backup backup() const final - { - nlohmann::json current_event_json; - current_event_json["index"] = _current_event_index_plus_one - 1; - current_event_json["state"] = _current->backup().state(); - - nlohmann::json backup_json; - backup_json["schema_version"] = "0.1"; - backup_json["current_event"] = std::move(current_event_json); - - return Backup::make(_next_backup_sequence_number++, backup_json); - } - - Resume interrupt(std::function task_is_interrupted) final - { - return _current->interrupt(std::move(task_is_interrupted)); - } - - void cancel() - { - _reverse_remaining.clear(); - _state->update_status(Event::Status::Canceled); - _current->cancel(); - } - - void kill() - { - _reverse_remaining.clear(); - _state->update_status(Event::Status::Killed); - _current->kill(); - } - - SequenceActive( - std::vector dependencies, - rmf_task::events::SimpleEventPtr state, - std::function parent_update, - std::function checkpoint, - std::function finished) - : _current(nullptr), - _reverse_remaining(dependencies), - _state(std::move(state)), - _parent_update(std::move(parent_update)), - _checkpoint(std::move(checkpoint)), - _sequence_finished(std::move(finished)) - { - // Do nothing - } - - SequenceActive( - uint64_t current_event_index, - rmf_task::events::SimpleEventPtr state, - std::function parent_update, - std::function checkpoint, - std::function finished) - : _current(nullptr), - _current_event_index_plus_one(current_event_index+1), - _state(std::move(state)), - _parent_update(std::move(parent_update)), - _checkpoint(std::move(checkpoint)), - _sequence_finished(std::move(finished)) - { - // Do nothing - } - - void next() - { - if (_inside_next) - return; - - BoolGuard lock(_inside_next); - - const auto event_finished = [me = weak_from_this()]() - { - if (const auto self = me.lock()) - self->next(); - }; - - do - { - if (_reverse_remaining.empty()) - { - SequenceStandby::update_status(*_state); - _sequence_finished(); - return; - } - - ++_current_event_index_plus_one; - const auto next_event = _reverse_remaining.back(); - _reverse_remaining.pop_back(); - _current = next_event->begin(_checkpoint, event_finished); - } while (_current->state()->finished()); - - SequenceStandby::update_status(*_state); - _parent_update(); - } - -private: - - static const nlohmann::json_schema::json_validator backup_schema_validator; - - Event::ActivePtr _current; - uint64_t _current_event_index_plus_one = 0; - std::vector _reverse_remaining; - rmf_task::events::SimpleEventPtr _state; - std::function _parent_update; - std::function _checkpoint; - std::function _sequence_finished; - - // We need to make sure that next() never gets called recursively by events - // that finish as soon as they are activated - bool _inside_next = false; - mutable uint64_t _next_backup_sequence_number = 0; -}; - -//============================================================================== -Event::ActivePtr SequenceStandby::begin( - std::function checkpoint, - std::function finish) -{ - if (_active) - return _active; - - _active = std::make_shared( - std::move(_dependencies), _state, _parent_update, - std::move(checkpoint), std::move(finish)); - - _active->next(); - return _active; -} - -//============================================================================== -const nlohmann::json_schema::json_validator -SequenceActive::backup_schema_validator = - nlohmann::json_schema::json_validator( - schemas::backup_EventSequence_v0_1); - //============================================================================== void Bundle::add(const Event::InitializerPtr& initializer) { @@ -620,7 +289,7 @@ void Bundle::add( const Bundle::Description& description, std::function update) { - return SequenceStandby::initiate( + return initiate( *initialize_from, get_state, parameters, @@ -636,7 +305,7 @@ void Bundle::add( std::function checkpoint, std::function finished) { - return SequenceActive::restore( + return restore( *initialize_from, get_state, parameters, diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index feb743c5..4656b090 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -15,3 +15,348 @@ * */ +#include "internal_Sequence.hpp" + +namespace rmf_task_sequence { +namespace events { +namespace internal { + +//============================================================================== +Event::StandbyPtr Sequence::Standby::initiate( + const Event::Initializer& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + std::function parent_update) +{ + auto state = make_state(description); + const auto update = [parent_update = std::move(parent_update), state]() + { + update_status(*state); + parent_update(); + }; + + std::vector dependencies; + for (const auto& desc : description.dependencies()) + { + auto element = initializer.initialize( + get_state, parameters, *desc, update); + + state->add_dependency(element->state()); + + dependencies.emplace_back(std::move(element)); + } + + std::reverse(dependencies.begin(), dependencies.end()); + + update_status(*state); + return std::make_shared( + std::move(dependencies), std::move(state), std::move(parent_update)); +} + +//============================================================================== +Event::ConstStatePtr Sequence::Standby::state() const +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration Sequence::Standby::duration_estimate() const +{ + auto estimate = rmf_traffic::Duration(0); + for (const auto& element : _dependencies) + estimate += element->duration_estimate(); + + return estimate; +} + +Sequence::Standby::Standby( + std::vector dependencies, + rmf_task::events::SimpleEventPtr state, + std::function parent_update) +: _dependencies(std::move(dependencies)), + _state(std::move(state)), + _parent_update(std::move(parent_update)) +{ + // Do nothing +} + +//============================================================================== +Event::ActivePtr Sequence::Standby::begin( + std::function checkpoint, + std::function finish) +{ + if (_active) + return _active; + + _active = std::make_shared( + std::move(_dependencies), _state, _parent_update, + std::move(checkpoint), std::move(finish)); + + _active->next(); + return _active; +} + +//============================================================================== +rmf_task::events::SimpleEventPtr Sequence::Standby::make_state( + const Bundle::Description& description) +{ + return rmf_task::events::SimpleEvent::make( + description.category().value_or("Sequence"), + description.detail().value_or(""), + rmf_task::Event::Status::Standby); +} + +//============================================================================== +void Sequence::Standby::update_status(rmf_task::events::SimpleEvent& state) +{ + if (state.status() == Event::Status::Canceled + || state.status() == Event::Status::Killed + || state.status() == Event::Status::Skipped) + return; + + Event::Status status = Event::Status::Completed; + for (const auto& dep : state.dependencies()) + status = Event::sequence_status(status, dep->status()); + + state.update_status(status); +} + +//============================================================================== +Event::ActivePtr Sequence::Active::restore( + const Event::Initializer& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + const std::string& backup, + std::function parent_update, + std::function checkpoint, + std::function finished) +{ + auto state = Sequence::Standby::make_state(description); + const auto update = [parent_update = std::move(parent_update), state]() + { + Sequence::Standby::update_status(*state); + parent_update(); + }; + + std::vector dependencies; + + const auto backup_state = nlohmann::json::parse(backup); + if (const auto result = + schemas::ErrorHandler::has_error(backup_schema_validator, backup_state)) + { + state->update_log().error( + "Parsing failed while restoring backup: " + result->message + + "\nOriginal backup state:\n```" + backup + "\n```"); + state->update_status(Event::Status::Error); + return std::make_shared( + dependencies, std::move(state), nullptr, nullptr, nullptr); + } + + const auto& current_event_json = backup_state["current_event"]; + const auto current_event_index = + current_event_json["index"].get(); + + const auto& element_descriptions = description.dependencies(); + if (element_descriptions.size() <= current_event_index) + { + state->update_log().error( + "Failed to restore backup. Index [" + + std::to_string(current_event_index) + "] is too high for [" + + std::to_string(description.dependencies().size()) + + "] event dependencies. Original text:\n```\n" + backup + "\n```"); + state->update_status(Event::Status::Error); + return std::make_shared( + dependencies, std::move(state), nullptr, nullptr, nullptr); + } + + auto active = std::make_shared( + current_event_index, + std::move(state), + parent_update, + std::move(checkpoint), + std::move(finished)); + + const auto event_finished = [me = active->weak_from_this()]() + { + if (const auto self = me.lock()) + self->next(); + }; + + const auto& current_event_state = current_event_json["state"]; + active->_current = initializer.restore( + get_state, + parameters, + *element_descriptions.at(current_event_index), + current_event_state, + update, + checkpoint, + event_finished); + state->add_dependency(active->state()); + + for (std::size_t i = current_event_index+1; i < element_descriptions.size(); ++i) + { + const auto& desc = element_descriptions[i]; + auto element = initializer.initialize( + get_state, parameters, *desc, update); + + active->_state->add_dependency(element->state()); + dependencies.emplace_back(std::move(element)); + } + + std::reverse(dependencies.begin(), dependencies.end()); + + active->_reverse_remaining = std::move(dependencies); + + BoolGuard lock(active->_inside_next); + while (active->_current->state()->finished()) + { + if (active->_reverse_remaining.empty()) + { + Sequence::Standby::update_status(*active->_state); + return active; + } + + ++active->_current_event_index_plus_one; + const auto next_event = active->_reverse_remaining.back(); + active->_reverse_remaining.pop_back(); + active->_current = next_event->begin(active->_checkpoint, event_finished); + } + + Sequence::Standby::update_status(*active->_state); + return active; +} + +//============================================================================== +Event::ConstStatePtr Sequence::Active::state() const +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration Sequence::Active::remaining_time_estimate() const +{ + auto estimate = rmf_traffic::Duration(0); + if (_current) + estimate += _current->remaining_time_estimate(); + + for (const auto& element : _reverse_remaining) + estimate += element->duration_estimate(); + + return estimate; +} + +//============================================================================== +Event::Active::Backup Sequence::Active::backup() const +{ + nlohmann::json current_event_json; + current_event_json["index"] = _current_event_index_plus_one - 1; + current_event_json["state"] = _current->backup().state(); + + nlohmann::json backup_json; + backup_json["schema_version"] = "0.1"; + backup_json["current_event"] = std::move(current_event_json); + + return Backup::make(_next_backup_sequence_number++, backup_json); +} + +//============================================================================== +Event::Active::Resume Sequence::Active::interrupt( + std::function task_is_interrupted) +{ + return _current->interrupt(std::move(task_is_interrupted)); +} + +//============================================================================== +void Sequence::Active::cancel() +{ + _reverse_remaining.clear(); + _state->update_status(Event::Status::Canceled); + _current->cancel(); +} + +//============================================================================== +void Sequence::Active::kill() +{ + _reverse_remaining.clear(); + _state->update_status(Event::Status::Killed); + _current->kill(); +} + +//============================================================================== +Sequence::Active::Active( + std::vector dependencies, + rmf_task::events::SimpleEventPtr state, + std::function parent_update, + std::function checkpoint, + std::function finished) +: _current(nullptr), + _reverse_remaining(dependencies), + _state(std::move(state)), + _parent_update(std::move(parent_update)), + _checkpoint(std::move(checkpoint)), + _sequence_finished(std::move(finished)) +{ + // Do nothing +} + +//============================================================================== +Sequence::Active::Active( + uint64_t current_event_index, + rmf_task::events::SimpleEventPtr state, + std::function parent_update, + std::function checkpoint, + std::function finished) +: _current(nullptr), + _current_event_index_plus_one(current_event_index+1), + _state(std::move(state)), + _parent_update(std::move(parent_update)), + _checkpoint(std::move(checkpoint)), + _sequence_finished(std::move(finished)) +{ + // Do nothing +} + +//============================================================================== +void Sequence::Active::next() +{ + if (_inside_next) + return; + + BoolGuard lock(_inside_next); + + const auto event_finished = [me = weak_from_this()]() + { + if (const auto self = me.lock()) + self->next(); + }; + + do + { + if (_reverse_remaining.empty()) + { + Sequence::Standby::update_status(*_state); + _sequence_finished(); + return; + } + + ++_current_event_index_plus_one; + const auto next_event = _reverse_remaining.back(); + _reverse_remaining.pop_back(); + _current = next_event->begin(_checkpoint, event_finished); + } while (_current->state()->finished()); + + Sequence::Standby::update_status(*_state); + _parent_update(); +} + +//============================================================================== +const nlohmann::json_schema::json_validator +Sequence::Active::backup_schema_validator = + nlohmann::json_schema::json_validator( + schemas::backup_EventSequence_v0_1); + +} // namespace internal +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp index 2d5b8bf8..5d7aa8ee 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp @@ -19,11 +19,35 @@ #define SRC__RMF_TASK_SEQUENCE__EVENTS__INTERNAL_SEQUENCE_HPP #include +#include + +#include +#include namespace rmf_task_sequence { namespace events { namespace internal { +//============================================================================== +class BoolGuard +{ +public: + BoolGuard(bool& value) + : _value(value) + { + _value = true; + } + + ~BoolGuard() + { + _value = false; + } + +private: + bool& _value; +}; + +//============================================================================== class Sequence { public: @@ -33,6 +57,107 @@ class Sequence }; +//============================================================================== +class Sequence::Standby : public Event::Standby +{ +public: + + static Event::StandbyPtr initiate( + const Event::Initializer& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + std::function parent_update); + + Event::ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + Event::ActivePtr begin( + std::function checkpoint, + std::function finish) final; + + Standby( + std::vector dependencies, + rmf_task::events::SimpleEventPtr state, + std::function parent_update); + + static rmf_task::events::SimpleEventPtr make_state( + const Bundle::Description& description); + + static void update_status(rmf_task::events::SimpleEvent& state); + +private: + + std::vector _dependencies; + rmf_task::events::SimpleEventPtr _state; + std::function _parent_update; + std::shared_ptr _active; +}; + +//============================================================================== +class Sequence::Active + : public Event::Active, + public std::enable_shared_from_this +{ +public: + + static Event::ActivePtr restore( + const Event::Initializer& initializer, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + const std::string& backup, + std::function parent_update, + std::function checkpoint, + std::function finished); + + Event::ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel(); + + void kill(); + + Active( + std::vector dependencies, + rmf_task::events::SimpleEventPtr state, + std::function parent_update, + std::function checkpoint, + std::function finished); + + Active( + uint64_t current_event_index, + rmf_task::events::SimpleEventPtr state, + std::function parent_update, + std::function checkpoint, + std::function finished); + + void next(); + +private: + + static const nlohmann::json_schema::json_validator backup_schema_validator; + + Event::ActivePtr _current; + uint64_t _current_event_index_plus_one = 0; + std::vector _reverse_remaining; + rmf_task::events::SimpleEventPtr _state; + std::function _parent_update; + std::function _checkpoint; + std::function _sequence_finished; + + // We need to make sure that next() never gets called recursively by events + // that finish as soon as they are activated + bool _inside_next = false; + mutable uint64_t _next_backup_sequence_number = 0; +}; + } // namespace internal } // namespace events } // namespace rmf_task_sequence From 2314e144fe6d26807af2c951443072f1720d72f0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 8 Dec 2021 23:14:35 +0800 Subject: [PATCH 72/85] Filling in implementation gaps and beginning task sequence test Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Event.hpp | 28 ++ rmf_task/include/rmf_task/Phase.hpp | 4 +- rmf_task/include/rmf_task/Task.hpp | 2 +- rmf_task/include/rmf_task/detail/Resume.hpp | 6 + .../{SimpleEvent.hpp => SimpleEventState.hpp} | 28 +- .../include/rmf_task/phases/RestoreBackup.hpp | 4 +- rmf_task/src/rmf_task/Event.cpp | 34 ++ rmf_task/src/rmf_task/Phase.cpp | 8 +- rmf_task/src/rmf_task/detail/Resume.cpp | 6 + .../{SimpleEvent.cpp => SimpleEventState.cpp} | 44 +- .../src/rmf_task/phases/RestoreBackup.cpp | 21 +- rmf_task/test/mock/MockEvent.cpp | 10 +- rmf_task/test/mock/MockEvent.hpp | 3 + rmf_task/test/mock/MockPhase.cpp | 6 +- rmf_task/test/mock/MockPhase.hpp | 2 +- rmf_task/test/mock/MockTask.cpp | 5 +- rmf_task/test/mock/MockTask.hpp | 2 +- rmf_task_sequence/CMakeLists.txt | 4 +- .../include/rmf_task_sequence/Activity.hpp | 6 +- .../include/rmf_task_sequence/Event.hpp | 17 +- .../include/rmf_task_sequence/Phase.hpp | 12 +- .../include/rmf_task_sequence/SimplePhase.hpp | 100 +++++ .../include/rmf_task_sequence/Task.hpp | 33 +- .../rmf_task_sequence/detail/Backup.hpp | 1 + .../rmf_task_sequence/detail/impl_Event.hpp | 4 + .../rmf_task_sequence/detail/impl_Phase.hpp | 6 +- .../include/rmf_task_sequence/typedefs.hpp | 2 + .../src/rmf_task_sequence/Activity.cpp | 27 +- .../src/rmf_task_sequence/Event.cpp | 4 + .../src/rmf_task_sequence/Phase.cpp | 6 +- .../src/rmf_task_sequence/SimplePhase.cpp | 250 +++++++++++ .../src/rmf_task_sequence/Task.cpp | 401 ++++++++++++++++-- .../src/rmf_task_sequence/detail/Backup.cpp | 61 +++ .../src/rmf_task_sequence/events/Bundle.cpp | 23 +- .../rmf_task_sequence/events/GoToPlace.cpp | 16 +- .../src/rmf_task_sequence/events/Sequence.cpp | 25 +- .../src/rmf_task_sequence/events/WaitFor.cpp | 8 +- .../events/internal_Sequence.hpp | 19 +- rmf_task_sequence/test/mock/MockActivity.cpp | 197 +++++++++ rmf_task_sequence/test/mock/MockActivity.hpp | 124 ++++++ rmf_task_sequence/test/unit/test_Sequence.cpp | 61 +++ 41 files changed, 1465 insertions(+), 155 deletions(-) rename rmf_task/include/rmf_task/events/{SimpleEvent.hpp => SimpleEventState.hpp} (76%) rename rmf_task/src/rmf_task/events/{SimpleEvent.cpp => SimpleEventState.cpp} (71%) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp create mode 100644 rmf_task_sequence/test/mock/MockActivity.cpp create mode 100644 rmf_task_sequence/test/mock/MockActivity.hpp create mode 100644 rmf_task_sequence/test/unit/test_Sequence.cpp diff --git a/rmf_task/include/rmf_task/Event.hpp b/rmf_task/include/rmf_task/Event.hpp index 0ebddf00..82a3e608 100644 --- a/rmf_task/include/rmf_task/Event.hpp +++ b/rmf_task/include/rmf_task/Event.hpp @@ -91,6 +91,8 @@ class Event class Snapshot; using ConstSnapshotPtr = std::shared_ptr; + class AssignID; + using AssignIDPtr = std::shared_ptr; }; //============================================================================== @@ -102,6 +104,9 @@ class Event::State using Status = Event::Status; using ConstStatePtr = Event::ConstStatePtr; + /// The ID of this event, which is unique within its phase + virtual uint64_t id() const = 0; + /// The current Status of this event virtual Status status() const = 0; @@ -138,6 +143,9 @@ class Event::Snapshot : public Event::State /// Make a snapshot of the current state of an Event static ConstSnapshotPtr make(const State& other); + // Documentation inherited + uint64_t id() const final; + // Documentation inherited Status status() const final; @@ -159,6 +167,26 @@ class Event::Snapshot : public Event::State rmf_utils::impl_ptr _pimpl; }; +//============================================================================== +/// A utility class that helps to assign unique IDs to events +class Event::AssignID +{ +public: + + /// Make a shared_ptr + static AssignIDPtr make(); + + /// Constructor + AssignID(); + + /// Get a new unique ID + uint64_t assign() const; + + class Implementation; +private: + rmf_utils::unique_impl_ptr _pimpl; +}; + } // namespace rmf_task #endif // RMF_TASK__EVENT_HPP diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp index 9eed8766..666ca1bf 100644 --- a/rmf_task/include/rmf_task/Phase.hpp +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -118,7 +118,7 @@ class Phase::Active virtual Event::ConstStatePtr final_event() const = 0; /// The estimated finish time for this phase - virtual rmf_traffic::Time estimate_finish_time() const = 0; + virtual rmf_traffic::Duration estimate_remaining_time() const = 0; // Virtual destructor virtual ~Active() = default; @@ -139,7 +139,7 @@ class Phase::Snapshot : public Phase::Active Event::ConstStatePtr final_event() const final; // Documentation inherited - rmf_traffic::Time estimate_finish_time() const final; + rmf_traffic::Duration estimate_remaining_time() const final; class Implementation; private: diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 4f3df5fb..a96f5b49 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -194,7 +194,7 @@ class Task::Active virtual const ConstTagPtr& tag() const = 0; /// Estimate the overall finishing time of the task - virtual rmf_traffic::Time estimate_finish_time() const = 0; + virtual rmf_traffic::Duration estimate_remaining_time() const = 0; /// Get a backup for this Task virtual Backup backup() const = 0; diff --git a/rmf_task/include/rmf_task/detail/Resume.hpp b/rmf_task/include/rmf_task/detail/Resume.hpp index 9c4fa73d..49571581 100644 --- a/rmf_task/include/rmf_task/detail/Resume.hpp +++ b/rmf_task/include/rmf_task/detail/Resume.hpp @@ -20,6 +20,8 @@ #include +#include + namespace rmf_task { namespace detail { @@ -28,6 +30,10 @@ class Resume { public: + /// Make a Resume object. The callback will be triggered when the user + /// triggers the Resume. + static Resume make(std::function callback); + /// Call this object to tell the Task to resume. void operator()() const; diff --git a/rmf_task/include/rmf_task/events/SimpleEvent.hpp b/rmf_task/include/rmf_task/events/SimpleEventState.hpp similarity index 76% rename from rmf_task/include/rmf_task/events/SimpleEvent.hpp rename to rmf_task/include/rmf_task/events/SimpleEventState.hpp index 75da02d9..24657bad 100644 --- a/rmf_task/include/rmf_task/events/SimpleEvent.hpp +++ b/rmf_task/include/rmf_task/events/SimpleEventState.hpp @@ -15,8 +15,8 @@ * */ -#ifndef RMF_TASK__EVENTS__SIMPLEEVENT_HPP -#define RMF_TASK__EVENTS__SIMPLEEVENT_HPP +#ifndef RMF_TASK__EVENTS__SIMPLEEVENTSTATE_HPP +#define RMF_TASK__EVENTS__SIMPLEEVENTSTATE_HPP #include @@ -32,33 +32,37 @@ namespace events { /// satisfy the Phase interface's finish_event() function. Your Phase /// implementation can create an instance of this class and then manage its /// fields directly. -class SimpleEvent : public Event::State +class SimpleEventState : public Event::State { public: - static std::shared_ptr make( + static std::shared_ptr make( + uint64_t id, std::string name, std::string detail, Status initial_status, std::vector dependencies = {}); + // Documentation inherited + uint64_t id() const final; + // Documentation inherited Status status() const final; /// Update the status of this event - SimpleEvent& update_status(Status new_status); + SimpleEventState& update_status(Status new_status); // Documentation inherited VersionedString::View name() const final; /// Update the name of this event - SimpleEvent& update_name(std::string new_name); + SimpleEventState& update_name(std::string new_name); // Documentation inherited VersionedString::View detail() const final; /// Update the detail of this event - SimpleEvent& update_detail(std::string new_detail); + SimpleEventState& update_detail(std::string new_detail); // Documentation inherited Log::View log() const final; @@ -70,21 +74,21 @@ class SimpleEvent : public Event::State std::vector dependencies() const final; /// Update the dependencies - SimpleEvent& update_dependencies( + SimpleEventState& update_dependencies( std::vector new_dependencies); /// Add one dependency to the state - SimpleEvent& add_dependency(ConstStatePtr new_dependency); + SimpleEventState& add_dependency(ConstStatePtr new_dependency); class Implementation; private: - SimpleEvent(); + SimpleEventState(); rmf_utils::unique_impl_ptr _pimpl; }; -using SimpleEventPtr = std::shared_ptr; +using SimpleEventStatePtr = std::shared_ptr; } // namespace events } // namespace rmf_task -#endif // RMF_TASK__EVENTS__SIMPLEEVENT_HPP +#endif // RMF_TASK__EVENTS__SIMPLEEVENTSTATE_HPP diff --git a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp index acfe61f1..125946a2 100644 --- a/rmf_task/include/rmf_task/phases/RestoreBackup.hpp +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.hpp @@ -44,7 +44,7 @@ class RestoreBackup::Active : public Phase::Active /// Make an active RestoreBackup phase static ActivePtr make( const std::string& backup_state_str, - rmf_traffic::Time estimated_finish_time); + rmf_traffic::Duration estimated_remaining_time); // Documentation inherited ConstTagPtr tag() const final; @@ -53,7 +53,7 @@ class RestoreBackup::Active : public Phase::Active Event::ConstStatePtr final_event() const final; // Documentation inherited - rmf_traffic::Time estimate_finish_time() const final; + rmf_traffic::Duration estimate_remaining_time() const final; /// Call this function if the parsing fails void parsing_failed(const std::string& error_message); diff --git a/rmf_task/src/rmf_task/Event.cpp b/rmf_task/src/rmf_task/Event.cpp index 2436f988..2cb42b8c 100644 --- a/rmf_task/src/rmf_task/Event.cpp +++ b/rmf_task/src/rmf_task/Event.cpp @@ -81,6 +81,7 @@ class Event::Snapshot::Implementation { public: + uint64_t id; Status status; VersionedString::View name; VersionedString::View detail; @@ -95,6 +96,7 @@ auto Event::Snapshot::make(const State& other) -> ConstSnapshotPtr Snapshot output; output._pimpl = rmf_utils::make_impl( Implementation{ + other.id(), other.status(), other.name(), other.detail(), @@ -105,6 +107,12 @@ auto Event::Snapshot::make(const State& other) -> ConstSnapshotPtr return std::make_shared(std::move(output)); } +//============================================================================== +uint64_t Event::Snapshot::id() const +{ + return _pimpl->id; +} + //============================================================================== auto Event::Snapshot::status() const -> Status { @@ -141,4 +149,30 @@ Event::Snapshot::Snapshot() // Do nothing } +//============================================================================== +class Event::AssignID::Implementation +{ +public: + mutable uint64_t next_id = 0; +}; + +//============================================================================== +Event::AssignIDPtr Event::AssignID::make() +{ + return std::make_shared(); +} + +//============================================================================== +Event::AssignID::AssignID() +: _pimpl(rmf_utils::make_unique_impl()) +{ + // Do nothing +} + +//============================================================================== +uint64_t Event::AssignID::assign() const +{ + return _pimpl->next_id++; +} + } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp index b86e7065..60c2c722 100644 --- a/rmf_task/src/rmf_task/Phase.cpp +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -103,7 +103,7 @@ class Phase::Snapshot::Implementation public: ConstTagPtr tag; Event::ConstStatePtr finish_event; - rmf_traffic::Time estimated_finish_time; + rmf_traffic::Duration estimated_remaining_time; }; //============================================================================== @@ -114,7 +114,7 @@ Phase::ConstSnapshotPtr Phase::Snapshot::make(const Active& active) Implementation{ active.tag(), Event::Snapshot::make(*active.final_event()), - active.estimate_finish_time() + active.estimate_remaining_time() }); return std::make_shared(std::move(output)); @@ -133,9 +133,9 @@ Event::ConstStatePtr Phase::Snapshot::final_event() const } //============================================================================== -rmf_traffic::Time Phase::Snapshot::estimate_finish_time() const +rmf_traffic::Duration Phase::Snapshot::estimate_remaining_time() const { - return _pimpl->estimated_finish_time; + return _pimpl->estimated_remaining_time; } //============================================================================== diff --git a/rmf_task/src/rmf_task/detail/Resume.cpp b/rmf_task/src/rmf_task/detail/Resume.cpp index 624899d8..9086dff8 100644 --- a/rmf_task/src/rmf_task/detail/Resume.cpp +++ b/rmf_task/src/rmf_task/detail/Resume.cpp @@ -20,6 +20,12 @@ namespace rmf_task { namespace detail { +//============================================================================== +Resume Resume::make(std::function callback) +{ + return Implementation::make(std::move(callback)); +} + //============================================================================== void Resume::operator()() const { diff --git a/rmf_task/src/rmf_task/events/SimpleEvent.cpp b/rmf_task/src/rmf_task/events/SimpleEventState.cpp similarity index 71% rename from rmf_task/src/rmf_task/events/SimpleEvent.cpp rename to rmf_task/src/rmf_task/events/SimpleEventState.cpp index 6b48e406..11be7ef2 100644 --- a/rmf_task/src/rmf_task/events/SimpleEvent.cpp +++ b/rmf_task/src/rmf_task/events/SimpleEventState.cpp @@ -15,16 +15,17 @@ * */ -#include +#include namespace rmf_task { namespace events { //============================================================================== -class SimpleEvent::Implementation +class SimpleEventState::Implementation { public: + uint64_t id; Status status; VersionedString name; VersionedString detail; @@ -34,14 +35,17 @@ class SimpleEvent::Implementation }; //============================================================================== -std::shared_ptr SimpleEvent::make(std::string name, +std::shared_ptr SimpleEventState::make( + uint64_t id, + std::string name, std::string detail, Event::Status initial_status, std::vector dependencies) { - SimpleEvent output; + SimpleEventState output; output._pimpl = rmf_utils::make_unique_impl( Implementation{ + id, initial_status, std::move(name), std::move(detail), @@ -49,68 +53,74 @@ std::shared_ptr SimpleEvent::make(std::string name, std::move(dependencies) }); - return std::make_shared(std::move(output)); + return std::make_shared(std::move(output)); } //============================================================================== -Event::Status SimpleEvent::status() const +uint64_t SimpleEventState::id() const +{ + return _pimpl->id; +} + +//============================================================================== +Event::Status SimpleEventState::status() const { return _pimpl->status; } //============================================================================== -SimpleEvent& SimpleEvent::update_status(Event::Status new_status) +SimpleEventState& SimpleEventState::update_status(Event::Status new_status) { _pimpl->status = new_status; return *this; } //============================================================================== -VersionedString::View SimpleEvent::name() const +VersionedString::View SimpleEventState::name() const { return _pimpl->name.view(); } //============================================================================== -SimpleEvent& SimpleEvent::update_name(std::string new_name) +SimpleEventState& SimpleEventState::update_name(std::string new_name) { _pimpl->name.update(std::move(new_name)); return *this; } //============================================================================== -VersionedString::View SimpleEvent::detail() const +VersionedString::View SimpleEventState::detail() const { return _pimpl->detail.view(); } //============================================================================== -SimpleEvent& SimpleEvent::update_detail(std::string new_detail) +SimpleEventState& SimpleEventState::update_detail(std::string new_detail) { _pimpl->detail.update(std::move(new_detail)); return *this; } //============================================================================== -Log::View SimpleEvent::log() const +Log::View SimpleEventState::log() const { return _pimpl->log.view(); } //============================================================================== -Log& SimpleEvent::update_log() +Log& SimpleEventState::update_log() { return _pimpl->log; } //============================================================================== -std::vector SimpleEvent::dependencies() const +std::vector SimpleEventState::dependencies() const { return _pimpl->dependencies; } //============================================================================== -SimpleEvent& SimpleEvent::update_dependencies( +SimpleEventState& SimpleEventState::update_dependencies( std::vector new_dependencies) { _pimpl->dependencies = new_dependencies; @@ -118,14 +128,14 @@ SimpleEvent& SimpleEvent::update_dependencies( } //============================================================================== -SimpleEvent& SimpleEvent::add_dependency(ConstStatePtr new_dependency) +SimpleEventState& SimpleEventState::add_dependency(ConstStatePtr new_dependency) { _pimpl->dependencies.push_back(new_dependency); return *this; } //============================================================================== -SimpleEvent::SimpleEvent() +SimpleEventState::SimpleEventState() { // Do nothing } diff --git a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp index a786c88f..7088bfe4 100644 --- a/rmf_task/src/rmf_task/phases/RestoreBackup.cpp +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include namespace rmf_task { namespace phases { @@ -38,32 +38,33 @@ class RestoreBackup::Active::Implementation Implementation( const std::string& backup_state_str, - rmf_traffic::Time estimated_finish_time_) + rmf_traffic::Duration estimated_remaining_time_) : tag(make_tag()), - event(events::SimpleEvent::make( + event(events::SimpleEventState::make( + 0, tag->header().category(), tag->header().detail(), rmf_task::Event::Status::Underway)), - estimated_finish_time(estimated_finish_time_) + estimated_remaining_time(estimated_remaining_time_) { event->update_log().info( "Parsing backup state:\n```\n" + backup_state_str + "\n```"); } ConstTagPtr tag; - std::shared_ptr event; - rmf_traffic::Time estimated_finish_time; + std::shared_ptr event; + rmf_traffic::Duration estimated_remaining_time; }; //============================================================================== auto RestoreBackup::Active::make( const std::string& backup_state_str, - rmf_traffic::Time estimated_finish_time) -> ActivePtr + rmf_traffic::Duration estimated_remaining_time) -> ActivePtr { Active output; output._pimpl = rmf_utils::make_unique_impl( - std::move(backup_state_str), estimated_finish_time); + std::move(backup_state_str), estimated_remaining_time); return std::make_shared(std::move(output)); } @@ -81,9 +82,9 @@ Event::ConstStatePtr RestoreBackup::Active::final_event() const } //============================================================================== -rmf_traffic::Time RestoreBackup::Active::estimate_finish_time() const +rmf_traffic::Duration RestoreBackup::Active::estimate_remaining_time() const { - return _pimpl->estimated_finish_time; + return _pimpl->estimated_remaining_time; } //============================================================================== diff --git a/rmf_task/test/mock/MockEvent.cpp b/rmf_task/test/mock/MockEvent.cpp index ba3f5e2f..81a1bfd9 100644 --- a/rmf_task/test/mock/MockEvent.cpp +++ b/rmf_task/test/mock/MockEvent.cpp @@ -21,16 +21,24 @@ namespace test_rmf_task { //============================================================================== MockEvent::MockEvent( + uint64_t id_, std::string name_, std::string detail_, Status initial_status) -: _status(initial_status), +: _id(id_), + _status(initial_status), _name(std::move(name_)), _detail(std::move(detail_)) { // Do nothing } +//============================================================================== +uint64_t MockEvent::id() const +{ + return _id; +} + //============================================================================== auto MockEvent::status() const -> Status { diff --git a/rmf_task/test/mock/MockEvent.hpp b/rmf_task/test/mock/MockEvent.hpp index 91d2e2da..c43f9418 100644 --- a/rmf_task/test/mock/MockEvent.hpp +++ b/rmf_task/test/mock/MockEvent.hpp @@ -28,11 +28,13 @@ class MockEvent : public rmf_task::Event::State public: MockEvent( + uint64_t id_, std::string name_, std::string detail_, Status initial_status = Status::Standby); // Interface + uint64_t id() const final; Status status() const final; rmf_task::VersionedString::View name() const final; rmf_task::VersionedString::View detail() const final; @@ -40,6 +42,7 @@ class MockEvent : public rmf_task::Event::State std::vector dependencies() const final; // Fields + uint64_t _id; Status _status; rmf_task::VersionedString _name; rmf_task::VersionedString _detail; diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp index e0d537b7..fe8a73f2 100644 --- a/rmf_task/test/mock/MockPhase.cpp +++ b/rmf_task/test/mock/MockPhase.cpp @@ -27,7 +27,7 @@ MockPhase::Active::Active( std::function phase_finished_) : _tag(std::move(tag_)), _event(std::make_shared( - "Mock condition", "This is a mocked up condition")), + 0, "Mock Event", "This is a mocked up event")), _start_time(start_time_), _update(std::move(update_)), _phase_finished(std::move(phase_finished_)) @@ -48,9 +48,9 @@ rmf_task::Event::ConstStatePtr MockPhase::Active::final_event() const } //============================================================================== -rmf_traffic::Time MockPhase::Active::estimate_finish_time() const +rmf_traffic::Duration MockPhase::Active::estimate_remaining_time() const { - return _start_time + _tag->header().original_duration_estimate(); + return _tag->header().original_duration_estimate(); } //============================================================================== diff --git a/rmf_task/test/mock/MockPhase.hpp b/rmf_task/test/mock/MockPhase.hpp index 85b59f5b..2f87baf0 100644 --- a/rmf_task/test/mock/MockPhase.hpp +++ b/rmf_task/test/mock/MockPhase.hpp @@ -42,7 +42,7 @@ class MockPhase : public rmf_task::Phase ConstTagPtr tag() const final; rmf_task::Event::ConstStatePtr final_event() const final; - rmf_traffic::Time estimate_finish_time() const final; + rmf_traffic::Duration estimate_remaining_time() const final; void send_update() const; diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index c5de8dfb..78068faa 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -46,10 +46,9 @@ auto MockTask::Active::tag() const -> const ConstTagPtr& } //============================================================================== -rmf_traffic::Time MockTask::Active::estimate_finish_time() const +rmf_traffic::Duration MockTask::Active::estimate_remaining_time() const { - return std::chrono::steady_clock::now() - + _tag->header().original_duration_estimate(); + return _tag->header().original_duration_estimate(); } //============================================================================== diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp index 96c89497..f4ad58aa 100644 --- a/rmf_task/test/mock/MockTask.hpp +++ b/rmf_task/test/mock/MockTask.hpp @@ -44,7 +44,7 @@ class MockTask : public rmf_task::Task const ConstTagPtr& tag() const override; - rmf_traffic::Time estimate_finish_time() const override; + rmf_traffic::Duration estimate_remaining_time() const override; Backup backup() const override; diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 4a78b638..5fb2b162 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -52,10 +52,10 @@ target_include_directories(rmf_task_sequence ) if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) - file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + file(GLOB_RECURSE test_srcs "test/*.cpp") ament_add_catch2( - test_rmf_task_sequence test/main.cpp ${unit_test_srcs} + test_rmf_task_sequence test/main.cpp ${test_srcs} TIMEOUT 300) target_link_libraries(test_rmf_task_sequence rmf_task_sequence diff --git a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp index f4ae3272..50b87907 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Activity.hpp @@ -148,8 +148,9 @@ class Activity::Model /// \param[in] /// /// \return an estimated state for the robot when the phase is finished. - virtual std::optional estimate_finish( + virtual std::optional estimate_finish( State initial_state, + rmf_traffic::Time earliest_arrival_time, const Constraints& constraints, const TravelEstimator& travel_estimator) const = 0; @@ -192,8 +193,9 @@ class Activity::SequenceModel : public Activity::Model const Parameters& parameters); // Documentation inherited - std::optional estimate_finish( + std::optional estimate_finish( State initial_state, + rmf_traffic::Time earliest_arrival_time, const Constraints& constraints, const TravelEstimator& travel_estimator) const final; diff --git a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp index edb20950..bb859120 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -121,6 +121,9 @@ class Event::Initializer /// \tparam Description /// A class that implements the Event::Description interface /// + /// \param[in] id + /// An object to help assign an ID to each event + /// /// \param[in] get_state /// A callback for retrieving the current state of the robot /// @@ -147,6 +150,7 @@ class Event::Initializer using Initialize = std::function< StandbyPtr( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Description& description, @@ -158,6 +162,9 @@ class Event::Initializer /// \tparam Description /// A class that implements the Event::Description interface /// + /// \param[in] id + /// An object to help assign an ID to each event + /// /// \param[in] get_state /// A callback for retrieving the current state of the robot /// @@ -190,6 +197,7 @@ class Event::Initializer using Restore = std::function< ActivePtr( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Description& description, @@ -207,6 +215,9 @@ class Event::Initializer /// Initialize an event /// + /// \param[in] id + /// An object to help assign an ID to each event + /// /// \param[in] get_state /// A callback for retrieving the current state of the robot /// @@ -225,6 +236,7 @@ class Event::Initializer /// /// \return an Event in a Standby state StandbyPtr initialize( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, @@ -232,8 +244,8 @@ class Event::Initializer /// Signature for restoring an Event /// - /// \tparam Description - /// A class that implements the Event::Description interface + /// \param[in] id + /// An object to help assign an ID to each event /// /// \param[in] get_state /// A callback for retrieving the current state of the robot @@ -264,6 +276,7 @@ class Event::Initializer /// /// \return a restored Event in an Active state ActivePtr restore( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, diff --git a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp index 9dddc20c..b0cc7e2a 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -66,6 +66,9 @@ class Phase::Activator /// \param[in] get_state /// A callback for getting the current state of the robot /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// /// \param[in] tag /// The tag of this phase /// @@ -95,7 +98,8 @@ class Phase::Activator using Activate = std::function< ActivePtr( - std::function get_state, + const std::function& get_state, + const ConstParametersPtr& parameters, ConstTagPtr tag, const Description& description, std::optional backup_state, @@ -116,6 +120,9 @@ class Phase::Activator /// \param[in] get_state /// A callback for getting the current state of the robot /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// /// \param[in] tag /// The tag of this phase /// @@ -139,7 +146,8 @@ class Phase::Activator /// /// \return an active, running instance of the described phase. ActivePtr activate( - std::function get_state, + const std::function& get_state, + const ConstParametersPtr& parameters, ConstTagPtr tag, const Description& description, std::optional backup_state, diff --git a/rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp b/rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp new file mode 100644 index 00000000..0cfabc26 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__SIMPLEPHASE_HPP +#define RMF_TASK_SEQUENCE__SIMPLEPHASE_HPP + +#include +#include + +namespace rmf_task_sequence { + +//============================================================================== +class SimplePhase : public Phase +{ +public: + + class Active; + + class Description; + using DescriptionPtr = std::shared_ptr; + + static void add( + Phase::Activator& phase_activator, + const Event::ConstInitializerPtr& event_initializer); + +}; + +//============================================================================== +class SimplePhase::Description : public Phase::Description +{ +public: + + /// Make a SimplePhase description. + /// + /// \param[in] final_event + /// This is the final event which determines when the phase is finished + /// + /// \param[in] category + /// Specify a custom category string. If this is null, then the phase will + /// borrow the category from the final_event. + /// + /// \param[in] detail + /// Specify a custom detail string for the phase. If this is null, then the + /// phase will borrow the detail from the final_event. + static DescriptionPtr make( + Event::ConstDescriptionPtr final_event, + std::optional category = std::nullopt, + std::optional detail = std::nullopt); + + /// Get the final event + const Event::ConstDescriptionPtr& final_event() const; + + /// Set the final event + Description& final_event(Event::ConstDescriptionPtr new_final_event); + + /// Get the category + const std::optional& category() const; + + /// Set the category + Description& category(std::optional new_category); + + /// Get the detail + const std::optional& detail() const; + + /// Set the detail + Description& detail(std::optional new_detail); + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__SIMPLEPHASE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index 1c3e0722..a187c56d 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -40,6 +40,7 @@ class Task : public rmf_task::Task // Declaration class Description; + using DescriptionPtr = std::shared_ptr; using ConstDescriptionPtr = std::shared_ptr; class Model; @@ -95,7 +96,7 @@ class Task::Builder /// /// \param[in] detail /// Any detailed information that will go into the Task::Tag - ConstDescriptionPtr build( + DescriptionPtr build( std::string category, std::string detail); @@ -114,28 +115,26 @@ class Task::Description : public rmf_task::Task::Description rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; + /// Get the category for this task + const std::string& category() const; -//============================================================================== -class Task::Model : public rmf_task::Task::Model -{ -public: + /// Change the category for this task + Description& category(std::string new_category); - // Documentation inherited - std::optional estimate_finish( - const State& initial_state, - const Constraints& task_planning_constraints, - const TravelEstimator& travel_estimator) const final; + /// Get the details for this task + const std::string& detail() const; - // Documentation inherited - rmf_traffic::Duration invariant_duration() const final; + /// Change the details for this task + Description& detail(std::string new_detail); + + Header generate_header( + const State& initial_state, + const Parameters& parameters) const; class Implementation; private: - rmf_utils::unique_impl_ptr _pimpl; + Description(); + rmf_utils::impl_ptr _pimpl; }; } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp index 8f000aef..0d719726 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp @@ -57,6 +57,7 @@ class Backup class Implementation; private: + Backup(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp index 444f4ffb..d84e4f90 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp @@ -31,18 +31,21 @@ void Event::Initializer::add( _add( typeid(Desc), [initializer]( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, std::function update) { return initializer( + id, get_state, parameters, static_cast(description), std::move(update)); }, [restorer]( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, @@ -52,6 +55,7 @@ void Event::Initializer::add( std::function finished) { return restorer( + id, get_state, parameters, static_cast(description), diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp index 4880334b..79b3053d 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp @@ -29,7 +29,8 @@ void Phase::Activator::add_activator(Activate activator) _add_activator( typeid(Desc), [activator]( - std::function get_state, + const std::function& get_state, + const ConstParametersPtr& parameters, ConstTagPtr tag, const Phase::Description& description, std::optional backup_state, @@ -38,7 +39,8 @@ void Phase::Activator::add_activator(Activate activator) std::function phase_finished) { return activator( - std::move(get_state), + get_state, + parameters, std::move(tag), static_cast(description), std::move(backup_state), diff --git a/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp index 9aa8f74e..f0b20ae6 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,7 @@ namespace rmf_task_sequence { using Header = rmf_task::Header; using State = rmf_task::State; +using Estimate = rmf_task::Estimate; using Parameters = rmf_task::Parameters; using ConstParametersPtr = rmf_task::ConstParametersPtr; using Constraints = rmf_task::Constraints; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp b/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp index 26511b02..eaf49517 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Activity.cpp @@ -58,22 +58,37 @@ Activity::ConstModelPtr Activity::SequenceModel::make( } //============================================================================== -std::optional Activity::SequenceModel::estimate_finish( +std::optional Activity::SequenceModel::estimate_finish( rmf_task::State initial_state, + rmf_traffic::Time earliest_arrival_time, const rmf_task::Constraints& constraints, const rmf_task::TravelEstimator& travel_estimator) const { - std::optional finish_state = std::move(initial_state); + rmf_task::State finish_state = std::move(initial_state); + std::optional wait_until; for (const auto& model : _pimpl->models) { - finish_state = model->estimate_finish( - std::move(*finish_state), constraints, travel_estimator); + const auto estimate = model->estimate_finish( + std::move(finish_state), + earliest_arrival_time, + constraints, + travel_estimator); - if (!finish_state.has_value()) + if (!estimate.has_value()) return std::nullopt; + + finish_state = estimate->finish_state(); + if (!wait_until.has_value()) + wait_until = estimate->wait_until(); + } + + if (!wait_until.has_value()) + { + // This means that the models are empty, which is probably an error... + wait_until = earliest_arrival_time; } - return *finish_state; + return Estimate(std::move(finish_state), *wait_until); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp index f2cfada9..4c63f4e8 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Event.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp @@ -38,6 +38,7 @@ Event::Initializer::Initializer() //============================================================================== Event::StandbyPtr Event::Initializer::initialize( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, @@ -49,6 +50,7 @@ Event::StandbyPtr Event::Initializer::initialize( return nullptr; return it->second( + id, get_state, parameters, description, @@ -57,6 +59,7 @@ Event::StandbyPtr Event::Initializer::initialize( //============================================================================== Event::ActivePtr Event::Initializer::restore( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Event::Description& description, @@ -71,6 +74,7 @@ Event::ActivePtr Event::Initializer::restore( return nullptr; return it->second( + id, get_state, parameters, description, diff --git a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp index c4ee1776..06b544d1 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -37,7 +37,8 @@ Phase::Activator::Activator() //============================================================================== Phase::ActivePtr Phase::Activator::activate( - std::function get_state, + const std::function& get_state, + const ConstParametersPtr& parameters, ConstTagPtr tag, const Description& description, std::optional backup_state, @@ -51,7 +52,8 @@ Phase::ActivePtr Phase::Activator::activate( return nullptr; return it->second( - std::move(get_state), + get_state, + parameters, std::move(tag), description, std::move(backup_state), diff --git a/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp b/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp new file mode 100644 index 00000000..a3a6520a --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp @@ -0,0 +1,250 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { + +//============================================================================== +class SimplePhase::Description::Implementation +{ +public: + + std::optional category; + std::optional detail; + Event::ConstDescriptionPtr final_event; + + Header generate_header( + const State& initial_state, + const Parameters& parameters) const + { + const auto duration = + final_event->make_model(initial_state, parameters)->invariant_duration(); + + if (category.has_value() && detail.has_value()) + return Header(*category, *detail, duration); + + auto event_header = final_event->generate_header(initial_state, parameters); + const std::string& c = category.has_value()? + *category : event_header.category(); + const std::string& d = detail.has_value()? + *detail : event_header.detail(); + + return Header(c, d, duration); + } +}; + +//============================================================================== +class SimplePhase::Active + : public Phase::Active, + public std::enable_shared_from_this +{ +public: + + ConstTagPtr tag() const final + { + return _tag; + } + + Event::ConstStatePtr final_event() const final + { + return _final_event->state(); + } + + rmf_traffic::Duration estimate_remaining_time() const final + { + return _final_event->remaining_time_estimate(); + } + + Backup backup() const final + { + return _final_event->backup(); + } + + Resume interrupt(std::function task_is_interrupted) final + { + return _final_event->interrupt(std::move(task_is_interrupted)); + } + + void cancel() final + { + _final_event->cancel(); + } + + void kill() final + { + _final_event->kill(); + } + + ConstTagPtr _tag; + Event::ActivePtr _final_event; +}; + +//============================================================================== +void SimplePhase::add( + Activator& phase_activator, + const Event::ConstInitializerPtr& event_initializer) +{ + phase_activator.add_activator( + [event_initializer]( + const std::function& get_state, + const ConstParametersPtr& parameters, + ConstTagPtr tag, + const SimplePhase::Description& desc, + std::optional backup_state, + std::function phase_update, + std::function phase_checkpoint, + std::function finished) -> ActivePtr + { + const auto phase = std::make_shared(); + phase->_tag = tag; + + std::function event_update = + [ + weak = phase->weak_from_this(), + phase_update = std::move(phase_update) + ]() + { + if (const auto phase = weak.lock()) + phase_update(Phase::Snapshot::make(*phase)); + }; + + std::function event_checkpoint = + [ + weak = phase->weak_from_this(), + phase_checkpoint = std::move(phase_checkpoint) + ]() + { + if (const auto phase = weak.lock()) + phase_checkpoint(phase->backup()); + }; + + const auto assign_id = Event::AssignID::make(); + + if (backup_state.has_value()) + { + phase->_final_event = event_initializer->restore( + assign_id, + get_state, + parameters, + *desc.final_event(), + *backup_state, + std::move(event_update), + std::move(event_checkpoint), + std::move(finished)); + + return phase; + } + + const auto init_event = event_initializer->initialize( + assign_id, + get_state, + parameters, + *desc.final_event(), + std::move(event_update)); + + phase->_final_event = + init_event->begin(std::move(event_checkpoint), std::move(finished)); + + return phase; + }); +} + +//============================================================================== +SimplePhase::DescriptionPtr SimplePhase::Description::make( + Event::ConstDescriptionPtr final_event, + std::optional category, + std::optional detail) +{ + Description desc; + desc._pimpl = rmf_utils::make_impl( + Implementation{ + std::move(category), + std::move(detail), + std::move(final_event) + }); + + return std::make_shared(std::move(desc)); +} + +//============================================================================== +const Event::ConstDescriptionPtr& SimplePhase::Description::final_event() const +{ + return _pimpl->final_event; +} + +//============================================================================== +SimplePhase::Description& SimplePhase::Description::final_event( + Event::ConstDescriptionPtr new_final_event) +{ + _pimpl->final_event = std::move(new_final_event); + return *this; +} + +//============================================================================== +const std::optional& SimplePhase::Description::category() const +{ + return _pimpl->category; +} + +//============================================================================== +SimplePhase::Description& SimplePhase::Description::category( + std::optional new_category) +{ + _pimpl->category = std::move(new_category); + return *this; +} + +//============================================================================== +const std::optional& SimplePhase::Description::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +SimplePhase::Description& SimplePhase::Description::detail( + std::optional new_detail) +{ + _pimpl->detail = std::move(new_detail); + return *this; +} + +//============================================================================== +Activity::ConstModelPtr SimplePhase::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return _pimpl->final_event->make_model( + std::move(invariant_initial_state), + parameters); +} + +//============================================================================== +Header SimplePhase::Description::generate_header( + const State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->generate_header(initial_state, parameters); +} + +//============================================================================== +SimplePhase::Description::Description() +{ + // Do nothing +} + +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 0b897c8e..7445628f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -55,6 +55,8 @@ class Task::Description::Implementation { public: + std::string category; + std::string detail; std::vector stages; static std::list get_stages(const Description& desc) @@ -64,6 +66,57 @@ class Task::Description::Implementation desc._pimpl->stages.end()); } + static DescriptionPtr make( + std::string category, + std::string detail, + std::vector stages) + { + Description desc; + desc._pimpl = rmf_utils::make_impl( + Implementation{ + std::move(category), + std::move(detail), + std::move(stages) + }); + + return std::make_shared(std::move(desc)); + } +}; + +//============================================================================== +class Task::Model : public rmf_task::Task::Model +{ +public: + + Model( + Activity::ConstModelPtr activity_model, + rmf_traffic::Time earliest_start_time) + : _activity_model(std::move(activity_model)), + _earliest_start_time(earliest_start_time) + { + // Do nothing + } + + std::optional estimate_finish( + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) const final + { + return _activity_model->estimate_finish( + initial_state, + _earliest_start_time, + task_planning_constraints, + travel_estimator); + } + + rmf_traffic::Duration invariant_duration() const final + { + return _activity_model->invariant_duration(); + } + +private: + Activity::ConstModelPtr _activity_model; + rmf_traffic::Time _earliest_start_time; }; //============================================================================== @@ -124,7 +177,7 @@ class Task::Active const ConstTagPtr& tag() const final; // Documentation inherited - rmf_traffic::Time estimate_finish_time() const final; + rmf_traffic::Duration estimate_remaining_time() const final; // Documentation inherited Backup backup() const final; @@ -144,9 +197,6 @@ class Task::Active // Documentation inherited void rewind(uint64_t phase_id) final; - /// Get a weak reference to this object - std::weak_ptr weak_from_this() const; - static const nlohmann::json_schema::json_validator backup_schema_validator; private: @@ -190,11 +240,14 @@ class Task::Active _clock(std::move(clock)), _get_state(std::move(get_state)), _parameters(parameters), - _booking(std::move(booking)), + _tag(std::make_shared( + booking, + description.generate_header(_get_state(), *parameters))), _update(std::move(update)), _checkpoint(std::move(checkpoint)), _phase_finished(std::move(phase_finished)), _task_finished(std::move(task_finished)), + _task_is_interrupted(nullptr), _pending_stages(Description::Implementation::get_stages(description)), _cancel_sequence_initial_id(_pending_stages.size()+1) { @@ -205,12 +258,15 @@ class Task::Active std::function _clock; std::function _get_state; ConstParametersPtr _parameters; - ConstBookingPtr _booking; + ConstTagPtr _tag; std::function _update; std::function _checkpoint; std::function _phase_finished; std::function _task_finished; + std::function _task_is_interrupted; + std::optional _resume_phase; + std::list _pending_stages; std::vector _pending_phases; @@ -237,6 +293,13 @@ Task::Active::backup_schema_validator = nlohmann::json_schema::json_validator( schemas::backup_PhaseSequenceTask_v0_1); +//============================================================================== +Task::Builder::Builder() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + //============================================================================== auto Task::Builder::add_phase( Phase::ConstDescriptionPtr description, @@ -256,6 +319,117 @@ auto Task::Builder::add_phase( return *this; } +//============================================================================== +Task::DescriptionPtr Task::Builder::build( + std::string category, + std::string detail) +{ + return Description::Implementation::make( + std::move(category), + std::move(detail), + _pimpl->stages); +} + +//============================================================================== +Task::ConstModelPtr Task::Description::make_model( + rmf_traffic::Time earliest_start_time, + const Parameters& parameters) const +{ + std::vector descriptions; + descriptions.reserve(_pimpl->stages.size()); + for (const auto& desc : _pimpl->stages) + descriptions.push_back(desc->description); + + return std::make_shared( + Activity::SequenceModel::make( + std::move(descriptions), + State(), + parameters), + earliest_start_time); +} + +//============================================================================== +const std::string& Task::Description::category() const +{ + return _pimpl->category; +} + +//============================================================================== +Task::Description& Task::Description::category(std::string new_category) +{ + _pimpl->category = std::move(new_category); + return *this; +} + +//============================================================================== +const std::string& Task::Description::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +Task::Description& Task::Description::detail(std::string new_category) +{ + _pimpl->detail = std::move(new_category); + return *this; +} + +//============================================================================== +Header Task::Description::generate_header( + const State& initial_state, + const Parameters& parameters) const +{ + const auto model = make_model( + initial_state.time().value(), + parameters); + + return Header( + _pimpl->category, + _pimpl->detail, + model->invariant_duration()); +} + +//============================================================================== +Task::Description::Description() +{ + // Do nothing +} + +//============================================================================== +const std::vector& +Task::Active::completed_phases() const +{ + return _completed_phases; +} + +//============================================================================== +Phase::ConstActivePtr Task::Active::active_phase() const +{ + return _active_phase; +} + +//============================================================================== +const std::vector& Task::Active::pending_phases() const +{ + return _pending_phases; +} + +//============================================================================== +const Task::ConstTagPtr& Task::Active::tag() const +{ + return _tag; +} + +//============================================================================== +rmf_traffic::Duration Task::Active::estimate_remaining_time() const +{ + auto remaining_time = _active_phase->estimate_remaining_time(); + for (const auto& p : _pending_phases) + remaining_time += p.tag()->header().original_duration_estimate(); + + return remaining_time; +} + //============================================================================== auto Task::Active::backup() const -> Backup { @@ -264,11 +438,126 @@ auto Task::Active::backup() const -> Backup _active_phase->backup()); } +//============================================================================== +auto Task::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + _task_is_interrupted = std::move(task_is_interrupted); + _resume_phase = _active_phase->interrupt(_task_is_interrupted); + + return Resume::make( + [self = weak_from_this()]() + { + const auto me = self.lock(); + if (!me) + return; + + me->_task_is_interrupted = nullptr; + if (me->_resume_phase.has_value()) + (*me->_resume_phase)(); + else + me->_begin_next_stage(); + }); +} + +//============================================================================== +void Task::Active::cancel() +{ + if (_cancelled_on_phase.has_value()) + { + // If this already has a value, then the task is already running through + // its cancellation sequence. + return; + } + + _cancelled_on_phase = _active_phase->tag()->id(); + + _pending_phases.clear(); + _pending_phases.reserve(_active_stage->cancellation_sequence.size()); + auto next_id = _cancel_sequence_initial_id; + auto state = _get_state(); + for (const auto& desc : _active_stage->cancellation_sequence) + { + _pending_phases.emplace_back( + std::make_shared( + ++next_id, + desc->generate_header(state, *_parameters)) + ); + + state = desc->make_model(state, *_parameters)->invariant_finish_state(); + } + + _active_phase->cancel(); +} + +//============================================================================== +void Task::Active::kill() +{ + _killed = true; + _active_phase->kill(); +} + +//============================================================================== +void Task::Active::skip(uint64_t phase_id, bool value) +{ + if (value && _active_phase->tag()->id() == phase_id) + { + // If we are being told to skip the active phase then we will simply tell + // it to cancel and then move on to the remaining phases. + _active_phase->cancel(); + return; + } + + for (auto& p : _pending_phases) + { + if (phase_id == p.tag()->id()) + { + p.will_be_skipped(value); + return; + } + } +} + +//============================================================================== +void Task::Active::rewind(uint64_t phase_id) +{ + assert(_completed_phases.size() == _completed_stages.size()); + std::size_t completed_index=0; + auto stage_it = _completed_stages.begin(); + while (stage_it != _completed_stages.end()) + { + if ((*stage_it)->id == phase_id) + break; + + ++completed_index; + } + + if (stage_it == _completed_stages.end()) + { + // TODO(MXG): Indicate to the user that they asked to rewind to a phase + // that hasn't been reached yet. + return; + } + + _pending_stages.insert( + _pending_stages.begin(), + stage_it, + _completed_stages.begin()); + + // The currently active stage should also be put back into pending + _pending_stages.push_back(_active_stage); + _generate_pending_phases(); + + // If we are supposed to rewind to an earlier stage, then we should cancel + // the currently active one. + _active_phase->cancel(); +} + //============================================================================== void Task::Active::_load_backup(std::string backup_state_str) { const auto restore_phase = rmf_task::phases::RestoreBackup::Active::make( - backup_state_str, std::chrono::steady_clock::now()); + backup_state_str, rmf_traffic::Duration(0)); // TODO(MXG): Allow users to specify a custom clock for the log const auto start_time = std::chrono::steady_clock::now(); @@ -392,6 +681,9 @@ void Task::Active::_generate_pending_phases() s->description->generate_header(state, *_parameters) ) ); + + state = s->description->make_model( + state, *_parameters)->invariant_finish_state(); } } @@ -416,42 +708,77 @@ void Task::Active::_finish_phase() //============================================================================== void Task::Active::_begin_next_stage(std::optional restore) { - if (_pending_stages.empty()) + if (_task_is_interrupted) + { + // If we currently expect the task to be interrupted but we reach this + // function, then the previous phase finished despite the fact that it + // should have been interrupted (maybe it's a phase that cannot be + // interrupted). So we will now signal that the task is interrupted and + // refuse to move on to the next phase. + _task_is_interrupted(); + return; + } + + if (_resume_phase.has_value()) + { + // If we were expecting to resume a phase but we wound up here instead, then + // the phase that we tried to interrupt finished instead of being + // interrupted. We will clear this field since it is no longer relevant. + _resume_phase = std::nullopt; + } + + if (_killed) return _finish_task(); - assert(!_pending_phases.empty()); - _active_stage = _pending_stages.front(); - assert(_active_stage->id == _pending_phases.front().tag()->id()); + while (true) + { + if (_pending_stages.empty()) + return _finish_task(); - _pending_stages.pop_front(); - auto tag = _pending_phases.front().tag(); - _pending_phases.erase(_pending_phases.begin()); + _active_stage = _pending_stages.front(); + assert(_active_stage->id == _pending_phases.front().tag()->id()); + const auto skip_phase = _pending_phases.front().will_be_skipped(); - // Reset our memory of phase backup sequence number - _last_phase_backup_sequence_number = std::nullopt; + _pending_stages.pop_front(); + auto tag = _pending_phases.front().tag(); + _pending_phases.erase(_pending_phases.begin()); - _current_phase_start_time = _clock(); - _active_phase = _phase_activator->activate( - _get_state, - 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 = _active_phase->tag()->id()]( - Phase::Active::Backup backup) - { - if (const auto self = me.lock()) - self->_issue_backup(id, std::move(backup)); - }, - [me = weak_from_this()]() + // Reset our memory of phase backup sequence number + _last_phase_backup_sequence_number = std::nullopt; + + if (skip_phase) { - if (const auto self = me.lock()) - self->_finish_phase(); - }); + // If we're supposed to skip this phase, then continue looping until we + // reach a phase that we're not supposed to skip. + continue; + } + + _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 = _active_phase->tag()->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(); + }); + + return; + } } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp b/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp new file mode 100644 index 00000000..91a8a2a0 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { +namespace detail { + +//============================================================================== +class Backup::Implementation +{ +public: + + uint64_t seq; + nlohmann::json state; +}; + +//============================================================================== +Backup::Backup() +{ + // Do nothing +} + +//============================================================================== +Backup Backup::make(uint64_t seq, nlohmann::json state) +{ + Backup backup; + backup._pimpl = rmf_utils::make_impl( + Implementation{seq, state}); + return backup; +} + +//============================================================================== +uint64_t Backup::sequence() const +{ + return _pimpl->seq; +} + +//============================================================================== +const nlohmann::json& Backup::state() const +{ + return _pimpl->state; +} + + +} // namespace detail +} // namespace rmf_task_sequence \ No newline at end of file diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index 868985ec..c3e95dc1 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include @@ -51,6 +51,7 @@ nlohmann::json convert_to_json(const std::string& input) //============================================================================== Event::StandbyPtr initiate( const Event::Initializer& initializer, + const Event::AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -60,6 +61,7 @@ Event::StandbyPtr initiate( { return internal::Sequence::Standby::initiate( initializer, + id, get_state, parameters, description, @@ -73,6 +75,7 @@ Event::StandbyPtr initiate( //============================================================================== Event::ActivePtr restore( const Event::Initializer& initializer, + const Event::AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -85,6 +88,7 @@ Event::ActivePtr restore( { return internal::Sequence::Active::restore( initializer, + id, get_state, parameters, description, @@ -224,6 +228,19 @@ auto Bundle::Description::dependencies(Dependencies new_dependencies) return *this; } +//============================================================================== +auto Bundle::Description::type() const -> Type +{ + return _pimpl->type; +} + +//============================================================================== +auto Bundle::Description::type(Type new_type) -> Description& +{ + _pimpl->type = new_type; + return *this; +} + //============================================================================== const std::optional& Bundle::Description::category() const { @@ -284,6 +301,7 @@ void Bundle::add( { add_to.add( [initialize_from]( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -291,12 +309,14 @@ void Bundle::add( { return initiate( *initialize_from, + id, get_state, parameters, description, std::move(update)); }, [initialize_from]( + const AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -307,6 +327,7 @@ void Bundle::add( { return restore( *initialize_from, + id, get_state, parameters, description, diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp index ef7c5a0f..b53529d8 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp @@ -52,8 +52,9 @@ class GoToPlace::Model : public Activity::Model Goal goal); // Documentation inherited - std::optional estimate_finish( + std::optional estimate_finish( State initial_state, + rmf_traffic::Time earliest_arrival_time, const Constraints& constraints, const TravelEstimator& estimator) const final; @@ -111,8 +112,9 @@ Activity::ConstModelPtr GoToPlace::Model::make( } //============================================================================== -std::optional GoToPlace::Model::estimate_finish( +std::optional GoToPlace::Model::estimate_finish( State initial_state, + rmf_traffic::Time earliest_arrival_time, const Constraints& constraints, const TravelEstimator& travel_estimator) const { @@ -126,7 +128,13 @@ std::optional GoToPlace::Model::estimate_finish( if (!travel.has_value()) return std::nullopt; - finish.time(finish.time().value() + travel->duration()); + const auto arrival_time = + std::max( + initial_state.time().value() + travel->duration(), + earliest_arrival_time); + + const auto wait_until_time = arrival_time - travel->duration(); + finish.time(wait_until_time + travel->duration()); auto battery_soc = finish.battery_soc().value(); if (constraints.drain_battery()) @@ -138,7 +146,7 @@ std::optional GoToPlace::Model::estimate_finish( if (battery_soc <= battery_threshold) return std::nullopt; - return finish; + return Estimate(finish, wait_until_time); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index 4656b090..556e8401 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -24,12 +24,13 @@ namespace internal { //============================================================================== Event::StandbyPtr Sequence::Standby::initiate( const Event::Initializer& initializer, + const Event::AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, std::function parent_update) { - auto state = make_state(description); + auto state = make_state(id, description); const auto update = [parent_update = std::move(parent_update), state]() { update_status(*state); @@ -40,7 +41,7 @@ Event::StandbyPtr Sequence::Standby::initiate( for (const auto& desc : description.dependencies()) { auto element = initializer.initialize( - get_state, parameters, *desc, update); + id, get_state, parameters, *desc, update); state->add_dependency(element->state()); @@ -72,7 +73,7 @@ rmf_traffic::Duration Sequence::Standby::duration_estimate() const Sequence::Standby::Standby( std::vector dependencies, - rmf_task::events::SimpleEventPtr state, + rmf_task::events::SimpleEventStatePtr state, std::function parent_update) : _dependencies(std::move(dependencies)), _state(std::move(state)), @@ -98,17 +99,19 @@ Event::ActivePtr Sequence::Standby::begin( } //============================================================================== -rmf_task::events::SimpleEventPtr Sequence::Standby::make_state( +rmf_task::events::SimpleEventStatePtr Sequence::Standby::make_state( + const Event::AssignIDPtr& id, const Bundle::Description& description) { - return rmf_task::events::SimpleEvent::make( + return rmf_task::events::SimpleEventState::make( + id->assign(), description.category().value_or("Sequence"), description.detail().value_or(""), rmf_task::Event::Status::Standby); } //============================================================================== -void Sequence::Standby::update_status(rmf_task::events::SimpleEvent& state) +void Sequence::Standby::update_status(rmf_task::events::SimpleEventState& state) { if (state.status() == Event::Status::Canceled || state.status() == Event::Status::Killed @@ -125,6 +128,7 @@ void Sequence::Standby::update_status(rmf_task::events::SimpleEvent& state) //============================================================================== Event::ActivePtr Sequence::Active::restore( const Event::Initializer& initializer, + const Event::AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -133,7 +137,7 @@ Event::ActivePtr Sequence::Active::restore( std::function checkpoint, std::function finished) { - auto state = Sequence::Standby::make_state(description); + auto state = Sequence::Standby::make_state(id, description); const auto update = [parent_update = std::move(parent_update), state]() { Sequence::Standby::update_status(*state); @@ -186,6 +190,7 @@ Event::ActivePtr Sequence::Active::restore( const auto& current_event_state = current_event_json["state"]; active->_current = initializer.restore( + id, get_state, parameters, *element_descriptions.at(current_event_index), @@ -199,7 +204,7 @@ Event::ActivePtr Sequence::Active::restore( { const auto& desc = element_descriptions[i]; auto element = initializer.initialize( - get_state, parameters, *desc, update); + id, get_state, parameters, *desc, update); active->_state->add_dependency(element->state()); dependencies.emplace_back(std::move(element)); @@ -287,7 +292,7 @@ void Sequence::Active::kill() //============================================================================== Sequence::Active::Active( std::vector dependencies, - rmf_task::events::SimpleEventPtr state, + rmf_task::events::SimpleEventStatePtr state, std::function parent_update, std::function checkpoint, std::function finished) @@ -304,7 +309,7 @@ Sequence::Active::Active( //============================================================================== Sequence::Active::Active( uint64_t current_event_index, - rmf_task::events::SimpleEventPtr state, + rmf_task::events::SimpleEventStatePtr state, std::function parent_update, std::function checkpoint, std::function finished) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp index 4897dd5b..ca277335 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp @@ -30,8 +30,9 @@ class WaitFor::Model : public Activity::Model rmf_traffic::Duration duration, const rmf_task::Parameters& parameters); - std::optional estimate_finish( + std::optional estimate_finish( rmf_task::State initial_state, + rmf_traffic::Time earliest_arrival_time, const Constraints& constraints, const TravelEstimator& travel_estimator) const final; @@ -121,8 +122,9 @@ WaitFor::Model::Model( } //============================================================================== -std::optional WaitFor::Model::estimate_finish( +std::optional WaitFor::Model::estimate_finish( State state, + rmf_traffic::Time earliest_arrival_time, const Constraints& constraints, const TravelEstimator&) const { @@ -134,7 +136,7 @@ std::optional WaitFor::Model::estimate_finish( if (state.battery_soc().value() <= _invariant_battery_drain) return std::nullopt; - return state; + return Estimate(state, earliest_arrival_time); } //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp index 5d7aa8ee..444fa7e2 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp @@ -19,7 +19,7 @@ #define SRC__RMF_TASK_SEQUENCE__EVENTS__INTERNAL_SEQUENCE_HPP #include -#include +#include #include #include @@ -64,6 +64,7 @@ class Sequence::Standby : public Event::Standby static Event::StandbyPtr initiate( const Event::Initializer& initializer, + const Event::AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -79,18 +80,19 @@ class Sequence::Standby : public Event::Standby Standby( std::vector dependencies, - rmf_task::events::SimpleEventPtr state, + rmf_task::events::SimpleEventStatePtr state, std::function parent_update); - static rmf_task::events::SimpleEventPtr make_state( + static rmf_task::events::SimpleEventStatePtr make_state( + const Event::AssignIDPtr& id, const Bundle::Description& description); - static void update_status(rmf_task::events::SimpleEvent& state); + static void update_status(rmf_task::events::SimpleEventState& state); private: std::vector _dependencies; - rmf_task::events::SimpleEventPtr _state; + rmf_task::events::SimpleEventStatePtr _state; std::function _parent_update; std::shared_ptr _active; }; @@ -104,6 +106,7 @@ class Sequence::Active static Event::ActivePtr restore( const Event::Initializer& initializer, + const Event::AssignIDPtr& id, const std::function& get_state, const ConstParametersPtr& parameters, const Bundle::Description& description, @@ -126,14 +129,14 @@ class Sequence::Active Active( std::vector dependencies, - rmf_task::events::SimpleEventPtr state, + rmf_task::events::SimpleEventStatePtr state, std::function parent_update, std::function checkpoint, std::function finished); Active( uint64_t current_event_index, - rmf_task::events::SimpleEventPtr state, + rmf_task::events::SimpleEventStatePtr state, std::function parent_update, std::function checkpoint, std::function finished); @@ -147,7 +150,7 @@ class Sequence::Active Event::ActivePtr _current; uint64_t _current_event_index_plus_one = 0; std::vector _reverse_remaining; - rmf_task::events::SimpleEventPtr _state; + rmf_task::events::SimpleEventStatePtr _state; std::function _parent_update; std::function _checkpoint; std::function _sequence_finished; diff --git a/rmf_task_sequence/test/mock/MockActivity.cpp b/rmf_task_sequence/test/mock/MockActivity.cpp new file mode 100644 index 00000000..eca513cd --- /dev/null +++ b/rmf_task_sequence/test/mock/MockActivity.cpp @@ -0,0 +1,197 @@ +/* + * Copyright (C) 2021 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 + +#include "MockActivity.hpp" + +namespace test_rmf_task_sequence { + +//============================================================================== +MockActivity::Description::Description(std::shared_ptr ctrl_) +: ctrl(std::move(ctrl_)) +{ + // Do nothing +} + +//============================================================================== +rmf_task_sequence::Activity::ConstModelPtr +MockActivity::Description::make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const +{ + return rmf_task_sequence::events::WaitFor::Description::make( + rmf_traffic::Duration(0))->make_model( + std::move(invariant_initial_state), parameters); +} + +//============================================================================== +rmf_task::Header MockActivity::Description::generate_header( + const rmf_task::State&, + const rmf_task::Parameters&) const +{ + return rmf_task::Header( + "Mock Activity", "Mocking an activity", rmf_traffic::Duration(0)); +} + +//============================================================================== +auto MockActivity::Standby::make( + std::shared_ptr ctrl, + const rmf_task::Event::AssignIDPtr& id, + std::function update) -> std::shared_ptr +{ + auto standby = std::make_shared(); + standby->_ctrl = std::move(ctrl); + standby->_update = std::move(update); + standby->_state_data = rmf_task::events::SimpleEventState::make( + id->assign(), "Mock Activity", "Mocking an activity", + rmf_task::Event::Status::Standby); + + return standby; +} + +//============================================================================== +rmf_task::Event::ConstStatePtr MockActivity::Standby::state() const +{ + return _state_data; +} + +//============================================================================== +rmf_traffic::Duration MockActivity::Standby::duration_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +rmf_task_sequence::Event::ActivePtr MockActivity::Standby::begin( + std::function checkpoint, + std::function finished) +{ + return Active::make( + _ctrl, + Signals{ + _update, + std::move(checkpoint), + std::move(finished) + }, + nullptr, + _state_data); +} + +//============================================================================== +auto MockActivity::Active::make( + std::shared_ptr ctrl, + Signals signals, + const rmf_task::Event::AssignIDPtr& id, + rmf_task::events::SimpleEventStatePtr event) -> std::shared_ptr +{ + auto output = std::make_shared(); + output->signals = std::move(signals); + output->state_data = std::move(event); + ctrl->active = output; + + if (!output->state_data) + { + output->state_data = rmf_task::events::SimpleEventState::make( + id->assign(), "Mock Activity", "Mocking an activity", + rmf_task::Event::Status::Underway); + } + + return output; +} + +//============================================================================== +rmf_task::Event::ConstStatePtr MockActivity::Active::state() const +{ + return state_data; +} + +//============================================================================== +rmf_traffic::Duration MockActivity::Active::remaining_time_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto MockActivity::Active::backup() const -> Backup +{ + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto MockActivity::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + state_data->update_status(rmf_task::Event::Status::Standby); + task_is_interrupted(); + return Resume::make( + [state_data = state_data]() + { + state_data->update_status(rmf_task::Event::Status::Underway); + }); +} + +//============================================================================== +void MockActivity::Active::cancel() +{ + state_data->update_status(rmf_task::Event::Status::Canceled); + signals.finished(); +} + +//============================================================================== +void MockActivity::Active::kill() +{ + state_data->update_status(rmf_task::Event::Status::Killed); + signals.finished(); +} + +//============================================================================== +void MockActivity::add( + const rmf_task_sequence::Event::InitializerPtr& initializer) +{ + initializer->add( + []( + const rmf_task::Event::AssignIDPtr& id, + const std::function&, + const rmf_task::ConstParametersPtr&, + const MockActivity::Description& description, + std::function update) + { + return Standby::make(description.ctrl, id, std::move(update)); + }, + []( + const rmf_task::Event::AssignIDPtr& id, + const std::function&, + const rmf_task::ConstParametersPtr&, + const MockActivity::Description& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) + { + return Active::make( + description.ctrl, + Signals{ + std::move(update), + std::move(checkpoint), + std::move(finished) + }, + id); + }); +} + +} // namespace test_rmf_task_sequence diff --git a/rmf_task_sequence/test/mock/MockActivity.hpp b/rmf_task_sequence/test/mock/MockActivity.hpp new file mode 100644 index 00000000..98c66720 --- /dev/null +++ b/rmf_task_sequence/test/mock/MockActivity.hpp @@ -0,0 +1,124 @@ +/* + * Copyright (C) 2021 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 TEST__MOCK__MOCKACTIVITY_HPP +#define TEST__MOCK__MOCKACTIVITY_HPP + +#include +#include + +namespace test_rmf_task_sequence { + +//============================================================================== +class MockActivity +{ +public: + + class Active; + class Standby; + + /// This class gives the test access to the active event that will be + /// generated by the initializer. + class Controller + { + public: + + std::shared_ptr standby; + std::shared_ptr active; + + }; + + /// The mock event description simply passes along the controller object + class Description : public rmf_task_sequence::Event::Description + { + public: + + Description(std::shared_ptr ctrl); + + rmf_task_sequence::Activity::ConstModelPtr make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const final; + + rmf_task::Header generate_header( + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const final; + + std::shared_ptr ctrl; + }; + + struct Signals + { + std::function update; + std::function checkpoint; + std::function finished; + }; + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + + static std::shared_ptr make( + std::shared_ptr ctrl, + const rmf_task::Event::AssignIDPtr& id, + std::function update); + + rmf_task::Event::ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + rmf_task_sequence::Event::ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + std::shared_ptr _ctrl; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state_data; + }; + + class Active : public rmf_task_sequence::Event::Active + { + public: + + static std::shared_ptr make( + std::shared_ptr ctrl, + Signals sigs, + const rmf_task::Event::AssignIDPtr& id, + rmf_task::events::SimpleEventStatePtr event = nullptr); + + rmf_task::Event::ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + Signals signals; + rmf_task::events::SimpleEventStatePtr state_data; + }; + + static void add(const rmf_task_sequence::Event::InitializerPtr& initializer); +}; + +} // namespace test_rmf_task_sequence + +#endif // TEST__MOCK__MOCKACTIVITY_HPP diff --git a/rmf_task_sequence/test/unit/test_Sequence.cpp b/rmf_task_sequence/test/unit/test_Sequence.cpp new file mode 100644 index 00000000..06921fd2 --- /dev/null +++ b/rmf_task_sequence/test/unit/test_Sequence.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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 + +#include +#include +#include +#include + +#include + +#include "../mock/MockActivity.hpp" + +using namespace test_rmf_task_sequence; + +SCENARIO("Test Event Sequences") +{ + const auto event_initializer = + std::make_shared(); + + // Add the Bundle event to the initializer + rmf_task_sequence::events::Bundle::add(event_initializer); + + MockActivity::add(event_initializer); + auto ctrl_1_0 = std::make_shared(); + auto ctrl_1_1 = std::make_shared(); + auto ctrl_1_2 = std::make_shared(); + auto ctrl_1_3 = std::make_shared(); + + auto ctrl_2_0 = std::make_shared(); + + auto ctrl_3_0 = std::make_shared(); + auto ctrl_3_1 = std::make_shared(); + + auto cancel_ctrl_0 = std::make_shared(); + auto cancel_ctrl_1 = std::make_shared(); + + rmf_task_sequence::Task::Builder builder; + + + rmf_task::Activator task_activator; + const auto phase_activator = + std::make_shared(); + rmf_task_sequence::SimplePhase::add(*phase_activator, event_initializer); + +} From 1dd16599661c242611a4ab13b6ef782c2e9daad4 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 10 Dec 2021 00:54:51 +0800 Subject: [PATCH 73/85] Fix implementation bugs and begin unit tests for task sequences Signed-off-by: Michael X. Grey --- rmf_task_sequence/CMakeLists.txt | 1 + .../include/rmf_task_sequence/Task.hpp | 20 + .../src/rmf_task_sequence/SimplePhase.cpp | 11 +- .../src/rmf_task_sequence/Task.cpp | 20 +- .../src/rmf_task_sequence/events/Sequence.cpp | 3 +- rmf_task_sequence/test/mock/MockActivity.cpp | 22 ++ rmf_task_sequence/test/mock/MockActivity.hpp | 4 + rmf_task_sequence/test/unit/test_Sequence.cpp | 368 +++++++++++++++++- 8 files changed, 438 insertions(+), 11 deletions(-) diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 5fb2b162..8decc4d7 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -52,6 +52,7 @@ target_include_directories(rmf_task_sequence ) if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) + file(GLOB_RECURSE test_srcs "test/*.cpp") ament_add_catch2( diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index a187c56d..2730520b 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -66,6 +66,26 @@ class Task : public rmf_task::Task Phase::ConstActivatorPtr phase_activator, std::function clock); + /// Add this task type to an Activator. This is an alternative to using + /// make_activator(~). + /// + /// \param[in] activator + /// The task activator to add this task type to. + /// + /// \param[in] phase_activator + /// A phase activator. It is recommended to fully initialize this phase + /// activator (add all supported phases) before passing it to this function. + /// The task activator will keep a reference to this phase activator, so + /// modifying it while a task is activating a phase could cause data races + /// and therefore undefined behavior. + /// + /// \param[in] clock + /// A callback that gives the current time when called. + static void add( + rmf_task::Activator& activator, + Phase::ConstActivatorPtr phase_activator, + std::function clock); + }; //============================================================================== diff --git a/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp b/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp index a3a6520a..2ee8f707 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp @@ -111,6 +111,7 @@ void SimplePhase::add( std::function finished) -> ActivePtr { const auto phase = std::make_shared(); + assert(tag != nullptr); phase->_tag = tag; std::function event_update = @@ -120,7 +121,10 @@ void SimplePhase::add( ]() { if (const auto phase = weak.lock()) - phase_update(Phase::Snapshot::make(*phase)); + { + if (phase->_final_event) + phase_update(Phase::Snapshot::make(*phase)); + } }; std::function event_checkpoint = @@ -130,7 +134,10 @@ void SimplePhase::add( ]() { if (const auto phase = weak.lock()) - phase_checkpoint(phase->backup()); + { + if (phase->_final_event) + phase_checkpoint(phase->backup()); + } }; const auto assign_id = Event::AssignID::make(); diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 7445628f..3259a821 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -559,8 +559,7 @@ void Task::Active::_load_backup(std::string backup_state_str) const auto restore_phase = rmf_task::phases::RestoreBackup::Active::make( backup_state_str, rmf_traffic::Duration(0)); - // TODO(MXG): Allow users to specify a custom clock for the log - const auto start_time = std::chrono::steady_clock::now(); + const auto start_time = _clock(); const auto failed_to_restore = [&]() -> void { @@ -569,7 +568,7 @@ void Task::Active::_load_backup(std::string backup_state_str) std::make_shared( rmf_task::Phase::Snapshot::make(*restore_phase), start_time, - std::chrono::steady_clock::now())); + _clock())); _finish_task(); }; @@ -753,6 +752,7 @@ void Task::Active::_begin_next_stage(std::optional restore) continue; } + const auto phase_id = tag->id(); _current_phase_start_time = _clock(); _active_phase = _phase_activator->activate( _get_state, @@ -765,7 +765,7 @@ void Task::Active::_begin_next_stage(std::optional restore) if (const auto self = me.lock()) self->_update(snapshot); }, - [me = weak_from_this(), id = _active_phase->tag()->id()]( + [me = weak_from_this(), id = phase_id]( Phase::Active::Backup backup) { if (const auto self = me.lock()) @@ -777,6 +777,8 @@ void Task::Active::_begin_next_stage(std::optional restore) self->_finish_phase(); }); + _update(Phase::Snapshot::make(*_active_phase)); + _issue_backup(phase_id, _active_phase->backup()); return; } } @@ -897,4 +899,14 @@ auto Task::make_activator( }; } +//============================================================================== +void Task::add( + rmf_task::Activator& activator, + Phase::ConstActivatorPtr phase_activator, + std::function clock) +{ + activator.add_activator( + make_activator(std::move(phase_activator), std::move(clock))); +} + } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index 556e8401..399b22d4 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -31,7 +31,7 @@ Event::StandbyPtr Sequence::Standby::initiate( std::function parent_update) { auto state = make_state(id, description); - const auto update = [parent_update = std::move(parent_update), state]() + const auto update = [parent_update, state]() { update_status(*state); parent_update(); @@ -354,6 +354,7 @@ void Sequence::Active::next() Sequence::Standby::update_status(*_state); _parent_update(); + _checkpoint(); } //============================================================================== diff --git a/rmf_task_sequence/test/mock/MockActivity.cpp b/rmf_task_sequence/test/mock/MockActivity.cpp index eca513cd..06696154 100644 --- a/rmf_task_sequence/test/mock/MockActivity.cpp +++ b/rmf_task_sequence/test/mock/MockActivity.cpp @@ -110,6 +110,10 @@ auto MockActivity::Active::make( id->assign(), "Mock Activity", "Mocking an activity", rmf_task::Event::Status::Underway); } + else + { + output->state_data->update_status(rmf_task::Event::Status::Underway); + } return output; } @@ -159,6 +163,24 @@ void MockActivity::Active::kill() signals.finished(); } +//============================================================================== +void MockActivity::Active::update( + rmf_task::Event::Status status, + std::string text) +{ + state_data->update_status(status); + state_data->update_log().info(std::move(text)); + signals.update(); +} + +//============================================================================== +void MockActivity::Active::complete() +{ + state_data->update_status(rmf_task::Event::Status::Completed); + state_data->update_log().info("Completed"); + signals.finished(); +} + //============================================================================== void MockActivity::add( const rmf_task_sequence::Event::InitializerPtr& initializer) diff --git a/rmf_task_sequence/test/mock/MockActivity.hpp b/rmf_task_sequence/test/mock/MockActivity.hpp index 98c66720..b223268c 100644 --- a/rmf_task_sequence/test/mock/MockActivity.hpp +++ b/rmf_task_sequence/test/mock/MockActivity.hpp @@ -112,6 +112,10 @@ class MockActivity void kill() final; + void update(rmf_task::Event::Status status, std::string text); + + void complete(); + Signals signals; rmf_task::events::SimpleEventStatePtr state_data; }; diff --git a/rmf_task_sequence/test/unit/test_Sequence.cpp b/rmf_task_sequence/test/unit/test_Sequence.cpp index 06921fd2..9efca8f1 100644 --- a/rmf_task_sequence/test/unit/test_Sequence.cpp +++ b/rmf_task_sequence/test/unit/test_Sequence.cpp @@ -21,13 +21,84 @@ #include #include #include +#include + +#include +#include +#include +#include +#include #include #include "../mock/MockActivity.hpp" +#include + using namespace test_rmf_task_sequence; +//============================================================================== +rmf_task_sequence::Phase::ConstDescriptionPtr make_sequence_for_phase( + const std::vector>& ctrls) +{ + std::vector descs; + for (const auto& c : ctrls) + descs.push_back(std::make_shared(c)); + + auto sequence = + std::make_shared( + descs, rmf_task_sequence::events::Bundle::Type::Sequence); + + return rmf_task_sequence::SimplePhase::Description::make(sequence); +} + +//============================================================================== +rmf_task_sequence::Phase::ConstDescriptionPtr make_phase( + std::shared_ptr ctrl) +{ + return rmf_task_sequence::SimplePhase::Description::make( + std::make_shared(std::move(ctrl))); +} + +//============================================================================== +void check_active( + const std::vector>& ctrls) +{ + for (const auto& c : ctrls) + CHECK(c->active); +} + +//============================================================================== +void check_status( + const std::vector>& ctrls, + rmf_task::Event::Status status) +{ + for (const auto& c : ctrls) + { + REQUIRE(c->active); + CHECK(c->active->state()->status() == status); + } +} + +//============================================================================== +void check_statuses( + const std::vector& states, + const std::vector& statuses) +{ + REQUIRE(states.size() == statuses.size()); + for (std::size_t i=0; i < states.size(); ++i) + CHECK(states[i]->status() == statuses[i]); +} + +//============================================================================== +void check_inactive( + const std::vector>& ctrls) +{ + for (const auto& c : ctrls) + CHECK_FALSE(c->active); +} + +//============================================================================== SCENARIO("Test Event Sequences") { const auto event_initializer = @@ -50,12 +121,301 @@ SCENARIO("Test Event Sequences") auto cancel_ctrl_0 = std::make_shared(); auto cancel_ctrl_1 = std::make_shared(); - rmf_task_sequence::Task::Builder builder; - - - rmf_task::Activator task_activator; const auto phase_activator = std::make_shared(); rmf_task_sequence::SimplePhase::add(*phase_activator, event_initializer); + rmf_task::Activator task_activator; + rmf_task_sequence::Task::add( + task_activator, + phase_activator, + [](){return std::chrono::steady_clock::now();}); + + rmf_task_sequence::Task::Builder builder; + builder.add_phase( + make_sequence_for_phase({ctrl_1_0, ctrl_1_1, ctrl_1_2, ctrl_1_3}), + {make_phase(cancel_ctrl_0), make_phase(cancel_ctrl_1)}); + + builder.add_phase(make_sequence_for_phase({ctrl_2_0}), {}); + + builder.add_phase( + make_sequence_for_phase({ctrl_3_0, ctrl_3_1}), + {make_phase(cancel_ctrl_0)}); + + auto battery_system_optional = + rmf_battery::agv::BatterySystem::make(24.0, 40.0, 8.8); + auto mechanical_system_optional = + rmf_battery::agv::MechanicalSystem::make(70.0, 40.0, 0.22); + auto power_system_optional = + rmf_battery::agv::PowerSystem::make(20.0); + + auto motion_sink = + std::make_shared( + *battery_system_optional, *mechanical_system_optional); + auto device_sink = + std::make_shared( + *battery_system_optional, *power_system_optional); + + const auto params = std::make_shared( + nullptr, + rmf_battery::agv::BatterySystem::make(1.0, 1.0, 1.0).value(), + motion_sink, + device_sink); + + rmf_task::Phase::ConstSnapshotPtr last_snapshot; + std::optional last_backup; + rmf_task::Phase::ConstCompletedPtr last_finished_phase; + std::size_t task_finished_counter = 0; + + auto task = task_activator.activate( + [](){ return rmf_task::State().time(std::chrono::steady_clock::now()); }, + params, + rmf_task::Request( + "mock_request_01", + std::chrono::steady_clock::now(), + nullptr, + builder.build("Mock Task", "Mocking a task")), + [&last_snapshot](rmf_task::Phase::ConstSnapshotPtr snapshot) + { + last_snapshot = std::move(snapshot); + }, + [&last_backup](rmf_task::Task::Active::Backup backup) + { + last_backup = std::move(backup); + }, + [&last_finished_phase](rmf_task::Phase::ConstCompletedPtr finished_phase) + { + last_finished_phase = std::move(finished_phase); + }, + [&task_finished_counter]() + { + ++task_finished_counter; + } + ); + + WHEN("Run through whole task") + { + check_active({ctrl_1_0}); + check_inactive({ctrl_1_1, ctrl_1_2, ctrl_1_3, ctrl_2_0, ctrl_3_0, ctrl_3_1}); + REQUIRE(last_snapshot); + CHECK(last_snapshot->tag()->id() == 1); + CHECK(last_snapshot->final_event()->dependencies().size() == 4); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Underway, + rmf_task::Event::Status::Standby, + rmf_task::Event::Status::Standby, + rmf_task::Event::Status::Standby + }); + + CHECK(task->completed_phases().size() == 0); + CHECK(task->pending_phases().size() == 2); + last_snapshot = nullptr; + + ctrl_1_0->active->complete(); + check_status({ctrl_1_0}, rmf_task::Event::Status::Completed); + check_active({ctrl_1_1}); + check_inactive({ctrl_1_2, ctrl_1_3, ctrl_2_0, ctrl_3_0, ctrl_3_1}); + REQUIRE(last_snapshot); + CHECK(last_snapshot->tag()->id() == 1); + CHECK(last_snapshot->final_event()->dependencies().size() == 4); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Underway, + rmf_task::Event::Status::Standby, + rmf_task::Event::Status::Standby + }); + + CHECK(last_backup.has_value()); + + CHECK(task->completed_phases().size() == 0); + CHECK(task->pending_phases().size() == 2); + last_snapshot = nullptr; + last_backup = std::nullopt; + + ctrl_1_1->active->complete(); + check_status({ctrl_1_0, ctrl_1_1}, rmf_task::Event::Status::Completed); + check_active({ctrl_1_2}); + check_inactive({ctrl_1_3, ctrl_2_0, ctrl_3_0, ctrl_3_1}); + REQUIRE(last_snapshot); + CHECK(last_snapshot->tag()->id() == 1); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Underway, + rmf_task::Event::Status::Standby + }); + + CHECK(last_backup.has_value()); + + CHECK(task->completed_phases().size() == 0); + CHECK(task->pending_phases().size() == 2); + last_snapshot = nullptr; + last_backup = std::nullopt; + + ctrl_1_2->active->complete(); + check_status( + {ctrl_1_0, ctrl_1_1, ctrl_1_2}, + rmf_task::Event::Status::Completed); + check_active({ctrl_1_3}); + check_inactive({ctrl_2_0, ctrl_3_0, ctrl_3_1}); + REQUIRE(last_snapshot); + CHECK(last_snapshot->tag()->id() == 1); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Underway + }); + + CHECK(last_backup.has_value()); + + CHECK(task->completed_phases().size() == 0); + CHECK(task->pending_phases().size() == 2); + last_snapshot = nullptr; + last_backup = std::nullopt; + + ctrl_1_3->active->signals.checkpoint(); + CHECK(last_backup.has_value()); + + CHECK(!last_finished_phase); + + ctrl_1_3->active->complete(); + check_status( + {ctrl_1_0, ctrl_1_1, ctrl_1_2, ctrl_1_3}, + rmf_task::Event::Status::Completed); + check_active({ctrl_2_0}); + check_inactive({ctrl_3_0, ctrl_3_1}); + REQUIRE(last_snapshot); + CHECK(last_snapshot->tag()->id() == 2); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Underway + }); + + REQUIRE(last_finished_phase); + CHECK(last_finished_phase->snapshot()->tag()->id() == 1); + check_statuses( + {last_finished_phase->snapshot()->final_event()}, + {rmf_task::Event::Status::Completed}); + check_statuses( + last_finished_phase->snapshot()->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed + }); + check_statuses( + {last_finished_phase->snapshot()->final_event()}, + {rmf_task::Event::Status::Completed}); + CHECK(last_backup.has_value()); + + CHECK(task->completed_phases().size() == 1); + CHECK(task->pending_phases().size() == 1); + last_snapshot = nullptr; + last_finished_phase = nullptr; + last_backup = std::nullopt; + + ctrl_2_0->active->complete(); + check_status( + {ctrl_1_0, ctrl_1_1, ctrl_1_2, ctrl_1_3, ctrl_2_0}, + rmf_task::Event::Status::Completed); + check_active({ctrl_3_0}); + check_inactive({ctrl_3_1}); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Underway, + rmf_task::Event::Status::Standby + }); + + REQUIRE(last_finished_phase); + CHECK(last_finished_phase->snapshot()->tag()->id() == 2); + check_statuses( + {last_finished_phase->snapshot()->final_event()}, + {rmf_task::Event::Status::Completed}); + check_statuses( + last_finished_phase->snapshot()->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed + }); + + CHECK(task->completed_phases().size() == 2); + CHECK(task->pending_phases().size() == 0); + last_snapshot = nullptr; + last_finished_phase = nullptr; + last_backup = std::nullopt; + + ctrl_3_0->active->complete(); + check_status( + {ctrl_1_0, ctrl_1_1, ctrl_1_2, ctrl_1_3, ctrl_2_0, ctrl_3_0}, + rmf_task::Event::Status::Completed); + check_active({ctrl_3_1}); + check_statuses( + {last_snapshot->final_event()}, + {rmf_task::Event::Status::Underway}); + check_statuses( + last_snapshot->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Underway + }); + + CHECK(!last_finished_phase); + CHECK(last_backup.has_value()); + + CHECK(task->completed_phases().size() == 2); + CHECK(task->pending_phases().size() == 0); + last_snapshot = nullptr; + last_finished_phase = nullptr; + last_backup = std::nullopt; + + ctrl_3_1->active->complete(); + check_status( + {ctrl_1_0, ctrl_1_1, ctrl_1_2, ctrl_1_3, ctrl_2_0, ctrl_3_0, ctrl_3_1}, + rmf_task::Event::Status::Completed); + + CHECK(!last_snapshot); + REQUIRE(last_finished_phase); + CHECK(last_finished_phase->snapshot()->tag()->id() == 3); + check_statuses( + {last_finished_phase->snapshot()->final_event()}, + {rmf_task::Event::Status::Completed}); + check_statuses( + last_finished_phase->snapshot()->final_event()->dependencies(), + { + rmf_task::Event::Status::Completed, + rmf_task::Event::Status::Completed + }); + + CHECK(task->completed_phases().size() == 3); + CHECK(task->pending_phases().size() == 0); + } } From d11032eee64582fe4140af9ccb1989650eb5f957 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Dec 2021 18:31:53 +0800 Subject: [PATCH 74/85] Allow event bundles to be activated from a vector of Standbys Signed-off-by: Michael X. Grey --- .../rmf_task_sequence/events/Bundle.hpp | 36 +++++++++- .../src/rmf_task_sequence/events/Bundle.cpp | 65 ++++++++++++++----- 2 files changed, 82 insertions(+), 19 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp index 60cfb301..d4ff08f0 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp @@ -19,6 +19,7 @@ #define RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP #include +#include #include #include @@ -40,11 +41,13 @@ class Bundle : public Event /// The bundle will execute its dependencies in parallel and will finish /// when all of its dependencies are finished. - ParallelAll, + // (Not implemented yet) +// ParallelAll, /// The bundle will execute its dependencies in parallel and will finished /// when any (one or more) of its dependencies finishes. - ParallelAny + // (Not implemented yet) +// ParallelAny }; /// Give an initializer the ability to initialize event bundles @@ -69,6 +72,35 @@ class Bundle : public Event Event::Initializer& add_to, const Event::ConstInitializerPtr& initialize_from); + /// Activate a Bundle by directly providing the standby dependencies. + /// + /// \param[in] type + /// The type of bundle to activate + /// + /// \param[in] dependencies + /// The dependencies that are being bundled together + /// + /// \param[in] state + /// The state to modify as the bundle progresses. This class will not modify + /// the name or detail of the state. + /// + /// \param[in] update + /// The callback that will be triggered when the bundle has an update. + /// + /// \param[in] checkpoint + /// The callback that will be triggered when the bundle reaches a + /// checkpoint. + /// + /// \param[in] finished + /// The callback that will be triggered when the bundle is finished. + static ActivePtr activate( + Type type, + std::vector dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function checkpoint, + std::function finished); + class Description; }; diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index c3e95dc1..d270e4a2 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -69,7 +69,9 @@ Event::StandbyPtr initiate( } throw std::runtime_error( - "Bundle type not yet implemented: " + std::to_string(description.type())); + "[rmf_task_sequence::events::Bundle::initiate] " + "Bundle type not yet implemented: " + + std::to_string(description.type())); } //============================================================================== @@ -123,10 +125,11 @@ class Bundle::Description::Implementation { case Type::Sequence: return "Sequence"; - case Type::ParallelAll: - return "All of"; - case Type::ParallelAny: - return "One of"; + // TODO(MXG): Bring back this code when we want to support other types +// case Type::ParallelAll: +// return "All of"; +// case Type::ParallelAny: +// return "One of"; } return ""; @@ -136,18 +139,19 @@ class Bundle::Description::Implementation std::optional current_estimate, rmf_traffic::Duration next_dependency_estimate) const { - if (current_estimate.has_value()) - { - if (Type::ParallelAll == type) - return std::max(*current_estimate, next_dependency_estimate); - else if (Type::ParallelAny == type) - return std::min(*current_estimate, next_dependency_estimate); - } - else - { - if (Type::ParallelAll == type || Type::ParallelAny == type) - return next_dependency_estimate; - } + // TODO(MXG): Bring back this code when we want to support other types +// if (current_estimate.has_value()) +// { +// if (Type::ParallelAll == type) +// return std::max(*current_estimate, next_dependency_estimate); +// else if (Type::ParallelAny == type) +// return std::min(*current_estimate, next_dependency_estimate); +// } +// else +// { +// if (Type::ParallelAll == type || Type::ParallelAny == type) +// return next_dependency_estimate; +// } return current_estimate.value_or(rmf_traffic::Duration(0)) + next_dependency_estimate; @@ -338,5 +342,32 @@ void Bundle::add( }); } +//============================================================================== +Event::ActivePtr Bundle::activate( + Type type, + std::vector dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function checkpoint, + std::function finished) +{ + if (type == Bundle::Type::Sequence) + { + auto sequence = std::make_shared( + std::move(dependencies), + std::move(state), + std::move(update), + std::move(checkpoint), + std::move(finished)); + + sequence->next(); + return sequence; + } + + throw std::runtime_error( + "[rmf_task_sequence::events::Bundle::activate] " + "Bundle type not yet implemented: " + std::to_string(type)); +} + } // namespace events } // namespace rmf_task_sequence From cc8bf3d2272dc76d2ec504b62ea73ac57cdba970 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 15 Dec 2021 00:43:49 +0800 Subject: [PATCH 75/85] Allow users to directly create Bundle Standbys, not Actives Signed-off-by: Michael X. Grey --- .../include/rmf_task_sequence/events/Bundle.hpp | 15 ++++----------- .../src/rmf_task_sequence/events/Bundle.cpp | 13 ++++--------- 2 files changed, 8 insertions(+), 20 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp index d4ff08f0..6f2f921b 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp @@ -72,7 +72,8 @@ class Bundle : public Event Event::Initializer& add_to, const Event::ConstInitializerPtr& initialize_from); - /// Activate a Bundle by directly providing the standby dependencies. + /// Create a Bundle on standby by directly providing the standby dependencies + /// and the state object. /// /// \param[in] type /// The type of bundle to activate @@ -87,19 +88,11 @@ class Bundle : public Event /// \param[in] update /// The callback that will be triggered when the bundle has an update. /// - /// \param[in] checkpoint - /// The callback that will be triggered when the bundle reaches a - /// checkpoint. - /// - /// \param[in] finished - /// The callback that will be triggered when the bundle is finished. - static ActivePtr activate( + static StandbyPtr standby( Type type, std::vector dependencies, rmf_task::events::SimpleEventStatePtr state, - std::function update, - std::function checkpoint, - std::function finished); + std::function update); class Description; }; diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index d270e4a2..d55b7f38 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -343,24 +343,19 @@ void Bundle::add( } //============================================================================== -Event::ActivePtr Bundle::activate( +Event::StandbyPtr Bundle::standby( Type type, std::vector dependencies, rmf_task::events::SimpleEventStatePtr state, - std::function update, - std::function checkpoint, - std::function finished) + std::function update) { if (type == Bundle::Type::Sequence) { - auto sequence = std::make_shared( + auto sequence = std::make_shared( std::move(dependencies), std::move(state), - std::move(update), - std::move(checkpoint), - std::move(finished)); + std::move(update)); - sequence->next(); return sequence; } From 7420e52dbfd6434965ef1137566d61f3bd63446d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 16 Dec 2021 23:45:21 +0800 Subject: [PATCH 76/85] Implement unfolding for tasks and events Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Activator.hpp | 8 +- rmf_task/include/rmf_task/Log.hpp | 7 +- .../rmf_task/events/SimpleEventState.hpp | 3 +- .../include/rmf_task/requests/Delivery.hpp | 20 ++++- rmf_task/src/rmf_task/Activator.cpp | 8 +- rmf_task/src/rmf_task/Log.cpp | 20 +++-- .../src/rmf_task/events/SimpleEventState.cpp | 5 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 54 ++++++++++--- rmf_task/test/integration/test_backups.cpp | 6 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 68 +++++++++++++++++ rmf_task/test/unit/test_Activator.cpp | 2 +- .../include/rmf_task_sequence/Task.hpp | 28 ++++++- .../rmf_task_sequence/detail/impl_Task.hpp | 66 ++++++++++++++++ .../rmf_task_sequence/events/Bundle.hpp | 72 +++++++++++++++++- .../{phases => events}/DropOff.hpp | 14 ++-- .../{phases => events}/PickUp.hpp | 14 ++-- .../rmf_task_sequence/events/WaitFor.hpp | 4 +- .../events/detail/impl_Bundle.hpp | 76 +++++++++++++++++++ .../{ => phases}/SimplePhase.hpp | 8 +- .../src/rmf_task_sequence/Task.cpp | 6 +- .../src/rmf_task_sequence/events/Bundle.cpp | 11 ++- .../{phases => events}/DropOff.cpp | 6 +- .../{phases => events}/PayloadTransfer.cpp | 4 +- .../{phases => events}/PickUp.cpp | 6 +- .../src/rmf_task_sequence/events/Sequence.cpp | 48 ++++++++++-- .../src/rmf_task_sequence/events/WaitFor.cpp | 17 ++--- .../internal_PayloadTransfer.hpp | 4 +- .../events/internal_Sequence.hpp | 11 ++- .../{ => phases}/SimplePhase.cpp | 4 +- rmf_task_sequence/test/unit/test_Sequence.cpp | 9 ++- 30 files changed, 504 insertions(+), 105 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/impl_Task.hpp rename rmf_task_sequence/include/rmf_task_sequence/{phases => events}/DropOff.hpp (91%) rename rmf_task_sequence/include/rmf_task_sequence/{phases => events}/PickUp.hpp (91%) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/detail/impl_Bundle.hpp rename rmf_task_sequence/include/rmf_task_sequence/{ => phases}/SimplePhase.hpp (93%) rename rmf_task_sequence/src/rmf_task_sequence/{phases => events}/DropOff.cpp (97%) rename rmf_task_sequence/src/rmf_task_sequence/{phases => events}/PayloadTransfer.cpp (97%) rename rmf_task_sequence/src/rmf_task_sequence/{phases => events}/PickUp.cpp (97%) rename rmf_task_sequence/src/rmf_task_sequence/{phases => events}/internal_PayloadTransfer.hpp (97%) rename rmf_task_sequence/src/rmf_task_sequence/{ => phases}/SimplePhase.cpp (98%) diff --git a/rmf_task/include/rmf_task/Activator.hpp b/rmf_task/include/rmf_task/Activator.hpp index 1e07a509..3476134f 100644 --- a/rmf_task/include/rmf_task/Activator.hpp +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -69,7 +69,7 @@ class Activator using Activate = std::function< Task::ActivePtr( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Task::ConstBookingPtr& booking, const Description& description, @@ -116,7 +116,7 @@ class Activator /// /// \return an active, running instance of the requested task. Task::ActivePtr activate( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Request& request, std::function update, @@ -153,7 +153,7 @@ class Activator /// /// \return an active, running instance of the requested task. Task::ActivePtr restore( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Request& request, std::string backup_state, @@ -173,6 +173,8 @@ class Activator rmf_utils::impl_ptr _pimpl; }; +using ActivatorPtr = std::shared_ptr; +using ConstActivatorPtr = std::shared_ptr; } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/Log.hpp b/rmf_task/include/rmf_task/Log.hpp index 717b52df..2fe214e0 100644 --- a/rmf_task/include/rmf_task/Log.hpp +++ b/rmf_task/include/rmf_task/Log.hpp @@ -20,11 +20,12 @@ #include -#include #include #include #include +#include + namespace rmf_task { class Log; @@ -44,7 +45,7 @@ class Log /// \param[in] clock /// Specify a clock for this log to use. If nullptr is given, then /// std::chrono::system_clock::now() will be used. - Log(std::function clock = nullptr); + Log(std::function clock = nullptr); // TODO(MXG): Should we have a debug log option? @@ -97,7 +98,7 @@ class Log::Entry Tier tier() const; /// What was the timestamp of this entry. - std::chrono::system_clock::time_point time() const; + rmf_traffic::Time time() const; /// What was the text of this entry. const std::string& text() const; diff --git a/rmf_task/include/rmf_task/events/SimpleEventState.hpp b/rmf_task/include/rmf_task/events/SimpleEventState.hpp index 24657bad..dcc74855 100644 --- a/rmf_task/include/rmf_task/events/SimpleEventState.hpp +++ b/rmf_task/include/rmf_task/events/SimpleEventState.hpp @@ -41,7 +41,8 @@ class SimpleEventState : public Event::State std::string name, std::string detail, Status initial_status, - std::vector dependencies = {}); + std::vector dependencies = {}, + std::function clock = nullptr); // Documentation inherited uint64_t id() const final; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 352086be..5fb8edb2 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -31,6 +31,7 @@ #include #include #include +#include namespace rmf_task { namespace requests { @@ -56,7 +57,10 @@ class Delivery std::size_t pickup_waypoint, rmf_traffic::Duration pickup_duration, std::size_t dropoff_waypoint, - rmf_traffic::Duration dropoff_duration); + rmf_traffic::Duration dropoff_duration, + Payload payload, + std::string pickup_from_dispenser = "", + std::string dropoff_to_ingestor = ""); /// Documentation inherited Task::ConstModelPtr make_model( @@ -66,15 +70,24 @@ class Delivery /// Get the pickup waypoint in this request std::size_t pickup_waypoint() const; + /// Get the name of the dispenser that we're picking up from + std::string pickup_from_dispenser() const; + /// Get the duration over which delivery items are loaded rmf_traffic::Duration pickup_wait() const; /// Get the dropoff waypoint in this request std::size_t dropoff_waypoint() const; + /// Get the name of the ingestor that we're dropping off to + std::string dropoff_to_ingestor() const; + /// Get the duration over which delivery items are unloaded rmf_traffic::Duration dropoff_wait() const; + /// Get the payload that is being delivered + const Payload& payload() const; + class Implementation; private: Description(); @@ -113,10 +126,13 @@ class Delivery rmf_traffic::Duration pickup_wait, std::size_t dropoff_waypoint, rmf_traffic::Duration dropoff_wait, + Payload payload, const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority = nullptr, - bool automatic = false); + bool automatic = false, + std::string pickup_from_dispenser = "", + std::string dropoff_to_ingestor = ""); }; } // namespace requests diff --git a/rmf_task/src/rmf_task/Activator.cpp b/rmf_task/src/rmf_task/Activator.cpp index 1a36d8fe..eccb78aa 100644 --- a/rmf_task/src/rmf_task/Activator.cpp +++ b/rmf_task/src/rmf_task/Activator.cpp @@ -37,7 +37,7 @@ Activator::Activator() //============================================================================== Task::ActivePtr Activator::activate( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Request& request, std::function update, @@ -58,7 +58,7 @@ Task::ActivePtr Activator::activate( return nullptr; return it->second( - std::move(get_state), + get_state, parameters, request.booking(), *request.description(), @@ -71,7 +71,7 @@ Task::ActivePtr Activator::activate( //============================================================================== Task::ActivePtr Activator::restore( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const Request& request, std::string backup_state, @@ -90,7 +90,7 @@ Task::ActivePtr Activator::restore( return nullptr; return it->second( - std::move(get_state), + get_state, parameters, request.booking(), *request.description(), diff --git a/rmf_task/src/rmf_task/Log.cpp b/rmf_task/src/rmf_task/Log.cpp index a40b067e..f0900205 100644 --- a/rmf_task/src/rmf_task/Log.cpp +++ b/rmf_task/src/rmf_task/Log.cpp @@ -25,16 +25,21 @@ namespace rmf_task { class Log::Implementation { public: - std::function clock; + std::function clock; std::shared_ptr> entries; - Implementation(std::function clock_) + Implementation(std::function clock_) : clock(std::move(clock_)), entries(std::make_shared>()) { if (!clock) { - clock = []() { return std::chrono::system_clock::now(); }; + clock = []() + { + return rmf_traffic::Time( + rmf_traffic::Duration( + std::chrono::system_clock::now().time_since_epoch())); + }; } } @@ -47,7 +52,7 @@ class Log::Entry::Implementation static Entry make( Tier tier, - std::chrono::system_clock::time_point time, + rmf_traffic::Time time, std::string text) { Log::Entry output; @@ -62,7 +67,7 @@ class Log::Entry::Implementation } Tier tier; - std::chrono::system_clock::time_point time; + rmf_traffic::Time time; std::string text; }; @@ -218,8 +223,7 @@ auto Log::Reader::Implementation::read(const View& view) -> Iterable } //============================================================================== -Log::Log( - std::function clock) +Log::Log(std::function clock) : _pimpl(rmf_utils::make_impl(std::move(clock))) { // Do nothing @@ -268,7 +272,7 @@ auto Log::Entry::tier() const -> Tier } //============================================================================== -std::chrono::system_clock::time_point Log::Entry::time() const +rmf_traffic::Time Log::Entry::time() const { return _pimpl->time; } diff --git a/rmf_task/src/rmf_task/events/SimpleEventState.cpp b/rmf_task/src/rmf_task/events/SimpleEventState.cpp index 11be7ef2..f5ce6036 100644 --- a/rmf_task/src/rmf_task/events/SimpleEventState.cpp +++ b/rmf_task/src/rmf_task/events/SimpleEventState.cpp @@ -40,7 +40,8 @@ std::shared_ptr SimpleEventState::make( std::string name, std::string detail, Event::Status initial_status, - std::vector dependencies) + std::vector dependencies, + std::function clock) { SimpleEventState output; output._pimpl = rmf_utils::make_unique_impl( @@ -49,7 +50,7 @@ std::shared_ptr SimpleEventState::make( initial_status, std::move(name), std::move(detail), - Log(), + Log(std::move(clock)), std::move(dependencies) }); diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 5420e9f7..64b7cb2b 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -208,13 +208,13 @@ class Delivery::Description::Implementation { public: - Implementation() - {} - std::size_t pickup_waypoint; rmf_traffic::Duration pickup_wait; std::size_t dropoff_waypoint; rmf_traffic::Duration dropoff_wait; + rmf_task::Payload payload; + std::string pickup_from_dispenser; + std::string dropoff_to_ingestor; }; //============================================================================== @@ -222,20 +222,28 @@ Task::ConstDescriptionPtr Delivery::Description::make( std::size_t pickup_waypoint, rmf_traffic::Duration pickup_wait, std::size_t dropoff_waypoint, - rmf_traffic::Duration dropoff_wait) + rmf_traffic::Duration dropoff_wait, + Payload payload, + std::string pickup_from_dispenser, + std::string dropoff_to_ingestor) { std::shared_ptr delivery(new Description()); - delivery->_pimpl->pickup_waypoint = pickup_waypoint; - delivery->_pimpl->pickup_wait = pickup_wait; - delivery->_pimpl->dropoff_waypoint = dropoff_waypoint; - delivery->_pimpl->dropoff_wait = dropoff_wait; + delivery->_pimpl = rmf_utils::make_impl( + Implementation{ + pickup_waypoint, + pickup_wait, + dropoff_waypoint, + dropoff_wait, + std::move(payload), + std::move(pickup_from_dispenser), + std::move(dropoff_to_ingestor) + }); return delivery; } //============================================================================== Delivery::Description::Description() -: _pimpl(rmf_utils::make_impl(Implementation())) { // Do nothing } @@ -260,6 +268,18 @@ std::size_t Delivery::Description::pickup_waypoint() const return _pimpl->pickup_waypoint; } +//============================================================================== +std::string Delivery::Description::pickup_from_dispenser() const +{ + return _pimpl->pickup_from_dispenser; +} + +//============================================================================== +std::string Delivery::Description::dropoff_to_ingestor() const +{ + return _pimpl->dropoff_to_ingestor; +} + //============================================================================== rmf_traffic::Duration Delivery::Description::pickup_wait() const { @@ -278,22 +298,34 @@ std::size_t Delivery::Description::dropoff_waypoint() const return _pimpl->dropoff_waypoint; } +//============================================================================== +const Payload& Delivery::Description::payload() const +{ + return _pimpl->payload; +} + //============================================================================== ConstRequestPtr Delivery::make( std::size_t pickup_waypoint, rmf_traffic::Duration pickup_wait, std::size_t dropoff_waypoint, rmf_traffic::Duration dropoff_wait, + Payload payload, const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - bool automatic) + bool automatic, + std::string pickup_from_dispenser, + std::string dropoff_to_ingestor) { const auto description = Description::make( pickup_waypoint, pickup_wait, dropoff_waypoint, - dropoff_wait); + dropoff_wait, + std::move(payload), + std::move(pickup_from_dispenser), + std::move(dropoff_to_ingestor)); return std::make_shared( id, earliest_start_time, std::move(priority), description, automatic); diff --git a/rmf_task/test/integration/test_backups.cpp b/rmf_task/test/integration/test_backups.cpp index c23464a9..a9742388 100644 --- a/rmf_task/test/integration/test_backups.cpp +++ b/rmf_task/test/integration/test_backups.cpp @@ -45,7 +45,7 @@ SCENARIO("Backup file creation and clearing tests") using namespace std::chrono_literals; auto request = rmf_task::requests::Delivery::make( - 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + 0, 1min, 1, 1min, {{}}, "request_0", rmf_traffic::Time()); rmf_task::Phase::ConstSnapshotPtr phase_snapshot; auto active_task = activator.activate( nullptr, @@ -95,7 +95,7 @@ SCENARIO("RAII tests for temporary BackupFileManager::Robot instances") using namespace std::chrono_literals; auto request = rmf_task::requests::Delivery::make( - 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + 0, 1min, 1, 1min, {{}}, "request_0", rmf_traffic::Time()); rmf_task::Phase::ConstSnapshotPtr phase_snapshot; auto active_task = activator.activate( nullptr, @@ -143,7 +143,7 @@ SCENARIO("Back up to file") using namespace std::chrono_literals; auto request = rmf_task::requests::Delivery::make( - 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + 0, 1min, 1, 1min, {{}}, "request_0", rmf_traffic::Time()); rmf_task::Phase::ConstSnapshotPtr phase_snapshot; auto active_task = activator.activate( diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 2f728db2..cdce6bbd 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -254,6 +254,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -262,6 +263,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -270,6 +272,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)) }; @@ -337,6 +340,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -345,6 +349,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -353,6 +358,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -361,6 +367,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)), @@ -369,6 +376,7 @@ SCENARIO("Grid World") delivery_wait, 0, delivery_wait, + {{}}, "5", now + rmf_traffic::time::from_seconds(50000)), @@ -377,6 +385,7 @@ SCENARIO("Grid World") delivery_wait, 8, delivery_wait, + {{}}, "6", now + rmf_traffic::time::from_seconds(60000)), @@ -385,6 +394,7 @@ SCENARIO("Grid World") delivery_wait, 14, delivery_wait, + {{}}, "7", now + rmf_traffic::time::from_seconds(60000)), @@ -393,6 +403,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "8", now + rmf_traffic::time::from_seconds(60000)), @@ -401,6 +412,7 @@ SCENARIO("Grid World") delivery_wait, 0, delivery_wait, + {{}}, "9", now + rmf_traffic::time::from_seconds(60000)), @@ -409,6 +421,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "10", now + rmf_traffic::time::from_seconds(60000)), @@ -417,6 +430,7 @@ SCENARIO("Grid World") delivery_wait, 12, delivery_wait, + {{}}, "11", now + rmf_traffic::time::from_seconds(60000)) }; @@ -484,6 +498,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -492,6 +507,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -500,6 +516,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -508,6 +525,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)) }; @@ -584,6 +602,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -592,6 +611,7 @@ SCENARIO("Grid World") delivery_wait, 7, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -600,6 +620,7 @@ SCENARIO("Grid World") delivery_wait, 12, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -608,6 +629,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)), @@ -616,6 +638,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "5", now + rmf_traffic::time::from_seconds(50000)), @@ -624,6 +647,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "6", now + rmf_traffic::time::from_seconds(70000)), @@ -632,6 +656,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "7", now + rmf_traffic::time::from_seconds(70000)), @@ -640,6 +665,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "8", now + rmf_traffic::time::from_seconds(70000)), @@ -648,6 +674,7 @@ SCENARIO("Grid World") delivery_wait, 1, delivery_wait, + {{}}, "9", now + rmf_traffic::time::from_seconds(70000)), @@ -656,6 +683,7 @@ SCENARIO("Grid World") delivery_wait, 5, delivery_wait, + {{}}, "10", now + rmf_traffic::time::from_seconds(70000)), @@ -664,6 +692,7 @@ SCENARIO("Grid World") delivery_wait, 10, delivery_wait, + {{}}, "11", now + rmf_traffic::time::from_seconds(70000)) }; @@ -821,6 +850,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -829,6 +859,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -837,6 +868,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)) }; @@ -871,6 +903,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()); @@ -921,6 +954,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -930,6 +964,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -939,6 +974,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()) @@ -987,6 +1023,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -996,6 +1033,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1004,6 +1042,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1012,6 +1051,7 @@ SCENARIO("Grid World") delivery_wait, 7, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()) @@ -1069,6 +1109,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -1078,6 +1119,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1086,6 +1128,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(100000)), @@ -1094,6 +1137,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(100000), rmf_task::BinaryPriorityScheme::make_high_priority()) @@ -1153,6 +1197,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1161,6 +1206,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1169,6 +1215,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1177,6 +1224,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0)) }; @@ -1217,6 +1265,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1225,6 +1274,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1233,6 +1283,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -1242,6 +1293,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()) @@ -1298,6 +1350,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1306,6 +1359,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1314,6 +1368,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1322,6 +1377,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0)) }; @@ -1370,6 +1426,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -1379,6 +1436,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -1388,6 +1446,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0), rmf_task::BinaryPriorityScheme::make_high_priority()), @@ -1397,6 +1456,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0)) }; @@ -1466,6 +1526,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1474,6 +1535,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1482,6 +1544,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1490,6 +1553,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)) }; @@ -1634,6 +1698,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1642,6 +1707,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1650,6 +1716,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1658,6 +1725,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)) }; diff --git a/rmf_task/test/unit/test_Activator.cpp b/rmf_task/test/unit/test_Activator.cpp index 217f5c5e..7e72afb9 100644 --- a/rmf_task/test/unit/test_Activator.cpp +++ b/rmf_task/test/unit/test_Activator.cpp @@ -26,7 +26,7 @@ SCENARIO("Activate fresh task") using namespace std::chrono_literals; auto request = rmf_task::requests::Delivery::make( - 0, 1min, 1, 1min, "request_0", rmf_traffic::Time()); + 0, 1min, 1, 1min, {{}}, "request_0", rmf_traffic::Time()); rmf_task::Phase::ConstSnapshotPtr phase_snapshot; std::optional backup; diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index 2730520b..018697b8 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -86,6 +86,32 @@ class Task : public rmf_task::Task Phase::ConstActivatorPtr phase_activator, std::function clock); + /// Give an initializer the ability to build a sequence task for some other + /// task description. + /// + /// This is useful when the described task is best implemented as a sequence + /// of phases. + /// + /// \param[in] unfold_description + /// This will be used to unfold the other description into a task sequence + /// Description. + /// + /// \param[in] activator + /// This activator will be given the ability to unfold and activate the + /// OtherDesc type. + /// + /// \param[in] phase_activator + /// This phase activator will be used to activate the task + /// + /// \param[in] clock + /// A callback that gives the current time when called + template + static void unfold( + std::function unfold_description, + rmf_task::Activator& activator, + Phase::ConstActivatorPtr phase_activator, + std::function clock); + }; //============================================================================== @@ -116,7 +142,7 @@ class Task::Builder /// /// \param[in] detail /// Any detailed information that will go into the Task::Tag - DescriptionPtr build( + std::shared_ptr build( std::string category, std::string detail); diff --git a/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Task.hpp new file mode 100644 index 00000000..ae907e7c --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Task.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__DETAIL__IMPL_TASK_HPP +#define RMF_TASK_SEQUENCE__DETAIL__IMPL_TASK_HPP + +#include + +namespace rmf_task_sequence { + +//============================================================================== +template +void Task::unfold( + std::function unfold_description, + rmf_task::Activator& task_activator, + Phase::ConstActivatorPtr phase_activator, + std::function clock) +{ + auto sequence_activator = + make_activator(std::move(phase_activator), std::move(clock)); + + task_activator.add_activator( + [ + unfold = std::move(unfold_description), + sequence_activator = std::move(sequence_activator) + ]( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Task::ConstBookingPtr& booking, + const OtherDesc& other_desc, + std::optional backup_state, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) + { + return sequence_activator( + get_state, + parameters, + booking, + unfold(other_desc), + std::move(backup_state), + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished)); + }); +} + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__IMPL_TASK_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp index 6f2f921b..2bbe3311 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp @@ -50,6 +50,8 @@ class Bundle : public Event // ParallelAny }; + class Description; + /// Give an initializer the ability to initialize event bundles /// /// \param[in] initializer @@ -65,13 +67,47 @@ class Bundle : public Event /// This Initializer will be given the ability to initialize event sequences /// /// \param[in] initialize_from - /// This Initializer will be used by the Event Sequence to initialize the + /// This Initializer will be used by the Bundle to initialize the /// events that it depends on. This may be a different initializer than the /// one that the event sequence is added to. static void add( Event::Initializer& add_to, const Event::ConstInitializerPtr& initialize_from); + /// Given an event description, return a vector of other event descriptions. + template + using UnfoldDescription = + std::function; + + /// Give an initializer the ability to initialize an event bundle for some + /// other event description. This is useful when you want a certain event to + /// be implemented as a bundle of other events without requiring users to + /// explicitly specify an event bundle. + /// + /// This is also useful if there is an event type that is safe to initialize + /// when bundled in a specific way with other events but should not be run on + /// its own. You can keep the Description type of that event private to the + /// downstream user but load it into the initializer for this bundle. + /// + /// \param[in] unfold_description + /// This will be used to produce the bundle to create + /// + /// \param[in] add_to + /// This Initializer will be given the ability to initialize this type of + /// event bundle. + /// + /// \param[in] initialize_from + /// This Initializer will be used to initialize the dependencies of this + /// Bundle. + template + static void unfold( + const UnfoldDescription& unfold_description, + Event::Initializer& add_to, + const Event::ConstInitializerPtr& initialize_from); + + using UpdateFn = std::function; + using DependencySpecifiers = std::vector>; + /// Create a Bundle on standby by directly providing the standby dependencies /// and the state object. /// @@ -79,7 +115,9 @@ class Bundle : public Event /// The type of bundle to activate /// /// \param[in] dependencies - /// The dependencies that are being bundled together + /// Factories for the dependencies that are being bundled together. Each + /// factory should take in an update callback and then give back the + /// StandbyPtr for the dependency. /// /// \param[in] state /// The state to modify as the bundle progresses. This class will not modify @@ -90,11 +128,36 @@ class Bundle : public Event /// static StandbyPtr standby( Type type, - std::vector dependencies, + const DependencySpecifiers& dependencies, rmf_task::events::SimpleEventStatePtr state, std::function update); - class Description; + /// Initiate a Bundle in Standby mode. For advanced use only. + /// + /// \warning It is not recommended to use this function directly. You should + /// consider using add(~) or unfold(~) with an initializer instead. + static Event::StandbyPtr initiate( + const Event::Initializer& initializer, + const Event::AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + std::function update); + + /// Restore a Bundle into Active mode. For advanced use only. + /// + /// \warning It is not recommended to use this function directly. You should + /// consider using add(~) or unfold(~) with an initializer instead. + static Event::ActivePtr restore( + const Event::Initializer& initializer, + const Event::AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + const std::string& backup, + std::function update, + std::function checkpoint, + std::function finished); }; //============================================================================== @@ -167,5 +230,6 @@ class Bundle::Description : public Event::Description } // namespace events } // namespace rmf_task_sequence +#include #endif // RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp similarity index 91% rename from rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp rename to rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp index 7aabedb8..d44eaaed 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/DropOff.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp @@ -15,16 +15,16 @@ * */ -#ifndef RMF_TASK_SEQUENCE__PHASES__DROPOFF_HPP -#define RMF_TASK_SEQUENCE__PHASES__DROPOFF_HPP +#ifndef RMF_TASK_SEQUENCE__EVENTS__DROPOFF_HPP +#define RMF_TASK_SEQUENCE__EVENTS__DROPOFF_HPP #include #include -#include +#include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== /// A DropOff phase encompasses going to a location and transferring a payload @@ -42,7 +42,7 @@ class DropOff }; //============================================================================== -class DropOff::Description : public Phase::Description +class DropOff::Description : public Event::Description { public: @@ -107,7 +107,7 @@ class DropOff::Description : public Phase::Description rmf_utils::unique_impl_ptr _pimpl; }; -} // namespace phases +} // namespace events } // namespace rmf_task_sequence -#endif // RMF_TASK_SEQUENCE__PHASES__DROPOFF_HPP +#endif // RMF_TASK_SEQUENCE__EVENTS__DROPOFF_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/PickUp.hpp similarity index 91% rename from rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp rename to rmf_task_sequence/include/rmf_task_sequence/events/PickUp.hpp index d4ed9923..8f2e957d 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/phases/PickUp.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PickUp.hpp @@ -15,16 +15,16 @@ * */ -#ifndef RMF_TASK_SEQUENCE__PHASES__PICKUP_HPP -#define RMF_TASK_SEQUENCE__PHASES__PICKUP_HPP +#ifndef RMF_TASK_SEQUENCE__EVENTS__PICKUP_HPP +#define RMF_TASK_SEQUENCE__EVENTS__PICKUP_HPP #include #include -#include +#include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== /// A PickUp phase encompasses going to a location and transferring a payload @@ -42,7 +42,7 @@ class PickUp }; //============================================================================== -class PickUp::Description : public Phase::Description +class PickUp::Description : public Event::Description { public: @@ -107,7 +107,7 @@ class PickUp::Description : public Phase::Description rmf_utils::unique_impl_ptr _pimpl; }; -} // namespace phases +} // namespace events } // namespace rmf_task_sequence -#endif // RMF_TASK_SEQUENCE__PHASES__PICKUP_HPP +#endif // RMF_TASK_SEQUENCE__EVENTS__PICKUP_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp index 864c85af..2b67eb30 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp @@ -56,6 +56,9 @@ class WaitFor::Description : public Phase::Description /// The duration of time that the phase should be waiting. static DescriptionPtr make(rmf_traffic::Duration wait_duration); + /// Constructor + Description(rmf_traffic::Duration duration_); + /// Get the duration of the wait rmf_traffic::Duration duration() const; @@ -74,7 +77,6 @@ class WaitFor::Description : public Phase::Description class Implementation; private: - Description(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/detail/impl_Bundle.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/detail/impl_Bundle.hpp new file mode 100644 index 00000000..453c9f9a --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/detail/impl_Bundle.hpp @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__DETAIL__IMPL_BUNDLE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__DETAIL__IMPL_BUNDLE_HPP + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +template +void Bundle::unfold( + const UnfoldDescription& unfold_description, + Event::Initializer& add_to, + const Event::ConstInitializerPtr& initialize_from) +{ + add_to.add( + [initialize_from, unfold_description]( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const OtherDesc& description, + std::function update) + { + return initiate( + *initialize_from, + id, + get_state, + parameters, + unfold_description(description), + std::move(update)); + }, + [initialize_from, unfold_description]( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const OtherDesc& description, + const nlohmann::json& backup_state, + std::function update, + std::function checkpoint, + std::function finished) + { + return restore( + *initialize_from, + id, + get_state, + parameters, + unfold_description(description), + backup_state, + std::move(update), + std::move(checkpoint), + std::move(finished)); + }); +} + + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__DETAIL__IMPL_BUNDLE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/SimplePhase.hpp similarity index 93% rename from rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp rename to rmf_task_sequence/include/rmf_task_sequence/phases/SimplePhase.hpp index 0cfabc26..ce41325f 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/SimplePhase.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/SimplePhase.hpp @@ -15,13 +15,14 @@ * */ -#ifndef RMF_TASK_SEQUENCE__SIMPLEPHASE_HPP -#define RMF_TASK_SEQUENCE__SIMPLEPHASE_HPP +#ifndef RMF_TASK_SEQUENCE__PHASES__SIMPLEPHASE_HPP +#define RMF_TASK_SEQUENCE__PHASES__SIMPLEPHASE_HPP #include #include namespace rmf_task_sequence { +namespace phases { //============================================================================== class SimplePhase : public Phase @@ -95,6 +96,7 @@ class SimplePhase::Description : public Phase::Description rmf_utils::impl_ptr _pimpl; }; +} // namespace phases } // namespace rmf_task_sequence -#endif // RMF_TASK_SEQUENCE__SIMPLEPHASE_HPP +#endif // RMF_TASK_SEQUENCE__PHASES__SIMPLEPHASE_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 3259a821..143927a9 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -320,9 +320,9 @@ auto Task::Builder::add_phase( } //============================================================================== -Task::DescriptionPtr Task::Builder::build( +auto Task::Builder::build( std::string category, - std::string detail) + std::string detail) -> std::shared_ptr { return Description::Implementation::make( std::move(category), @@ -874,7 +874,7 @@ auto Task::make_activator( phase_activator = std::move(phase_activator), clock = std::move(clock) ]( - std::function get_state, + const std::function& get_state, const ConstParametersPtr& parameters, const ConstBookingPtr& booking, const Description& description, diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index d55b7f38..e151915f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -47,9 +47,10 @@ nlohmann::json convert_to_json(const std::string& input) return output; } +} // anonymous namespace //============================================================================== -Event::StandbyPtr initiate( +Event::StandbyPtr Bundle::initiate( const Event::Initializer& initializer, const Event::AssignIDPtr& id, const std::function& get_state, @@ -75,7 +76,7 @@ Event::StandbyPtr initiate( } //============================================================================== -Event::ActivePtr restore( +Event::ActivePtr Bundle::restore( const Event::Initializer& initializer, const Event::AssignIDPtr& id, const std::function& get_state, @@ -104,8 +105,6 @@ Event::ActivePtr restore( "Bundle type not yet implemented: " + std::to_string(description.type())); } -} // anonymous namespace - //============================================================================== class Bundle::Description::Implementation { @@ -345,13 +344,13 @@ void Bundle::add( //============================================================================== Event::StandbyPtr Bundle::standby( Type type, - std::vector dependencies, + const std::vector>& dependencies, rmf_task::events::SimpleEventStatePtr state, std::function update) { if (type == Bundle::Type::Sequence) { - auto sequence = std::make_shared( + auto sequence = internal::Sequence::Standby::initiate( std::move(dependencies), std::move(state), std::move(update)); diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp similarity index 97% rename from rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp rename to rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp index 96ca321d..576a722d 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/DropOff.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp @@ -17,10 +17,10 @@ #include "internal_PayloadTransfer.hpp" -#include +#include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== class DropOff::Description::Implementation @@ -128,5 +128,5 @@ DropOff::Description::Description() // Do nothing } -} // namespace phases +} // namespace events } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.cpp similarity index 97% rename from rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp rename to rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.cpp index 11506fd2..a279a4d8 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/PayloadTransfer.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.cpp @@ -18,7 +18,7 @@ #include "internal_PayloadTransfer.hpp" namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== PayloadTransfer::PayloadTransfer( @@ -60,5 +60,5 @@ Header PayloadTransfer::generate_header( model->invariant_duration()); } -} // namespace phases +} // namespace events } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PickUp.cpp similarity index 97% rename from rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp rename to rmf_task_sequence/src/rmf_task_sequence/events/PickUp.cpp index 9e7a33ce..a4231fa3 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/PickUp.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PickUp.cpp @@ -17,10 +17,10 @@ #include "internal_PayloadTransfer.hpp" -#include +#include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== class PickUp::Description::Implementation @@ -126,5 +126,5 @@ PickUp::Description::Description() // Do nothing } -} // namespace phases +} // namespace events } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index 399b22d4..c3a09324 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -38,19 +38,44 @@ Event::StandbyPtr Sequence::Standby::initiate( }; std::vector dependencies; + dependencies.reserve(description.dependencies().size()); for (const auto& desc : description.dependencies()) { auto element = initializer.initialize( id, get_state, parameters, *desc, update); - state->add_dependency(element->state()); - dependencies.emplace_back(std::move(element)); } + // We reverse the dependencies so we can always pop them off the back of the + // queue. + std::reverse(dependencies.begin(), dependencies.end()); + + return std::make_shared( + std::move(dependencies), std::move(state), std::move(parent_update)); +} + +//============================================================================== +Event::StandbyPtr Sequence::Standby::initiate( + const std::vector& dependencies_fn, + rmf_task::events::SimpleEventStatePtr state, + std::function parent_update) +{ + const auto update = [parent_update, state]() + { + update_status(*state); + parent_update(); + }; + + std::vector dependencies; + dependencies.reserve(dependencies_fn.size()); + for (const auto& fn : dependencies_fn) + dependencies.push_back(fn(update)); + + // We reverse the dependencies so we can always pop them off the back of the + // queue. std::reverse(dependencies.begin(), dependencies.end()); - update_status(*state); return std::make_shared( std::move(dependencies), std::move(state), std::move(parent_update)); } @@ -65,21 +90,28 @@ Event::ConstStatePtr Sequence::Standby::state() const rmf_traffic::Duration Sequence::Standby::duration_estimate() const { auto estimate = rmf_traffic::Duration(0); - for (const auto& element : _dependencies) + for (const auto& element : _reverse_dependencies) estimate += element->duration_estimate(); return estimate; } +//============================================================================== Sequence::Standby::Standby( - std::vector dependencies, + std::vector reverse_dependencies, rmf_task::events::SimpleEventStatePtr state, std::function parent_update) -: _dependencies(std::move(dependencies)), +: _reverse_dependencies(std::move(reverse_dependencies)), _state(std::move(state)), _parent_update(std::move(parent_update)) { - // Do nothing + std::vector state_deps; + state_deps.reserve(_reverse_dependencies.size()); + for (auto rit = _reverse_dependencies.rbegin(); rit != _reverse_dependencies.rend(); ++rit) + state_deps.push_back((*rit)->state()); + + _state->update_dependencies(std::move(state_deps)); + update_status(*state); } //============================================================================== @@ -91,7 +123,7 @@ Event::ActivePtr Sequence::Standby::begin( return _active; _active = std::make_shared( - std::move(_dependencies), _state, _parent_update, + std::move(_reverse_dependencies), _state, _parent_update, std::move(checkpoint), std::move(finish)); _active->next(); diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp index ca277335..e4f3f9ae 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp @@ -59,11 +59,14 @@ class WaitFor::Description::Implementation auto WaitFor::Description::make(rmf_traffic::Duration wait_duration) -> DescriptionPtr { - auto output = std::shared_ptr(new Description); - output->_pimpl = - rmf_utils::make_impl(Implementation{wait_duration}); + return std::make_shared(wait_duration); +} - return output; +//============================================================================== +WaitFor::Description::Description(rmf_traffic::Duration duration_) +: _pimpl(rmf_utils::make_impl(Implementation{duration_})) +{ + // Do nothing } //============================================================================== @@ -102,12 +105,6 @@ Header WaitFor::Description::generate_header( _pimpl->duration); } -//============================================================================== -WaitFor::Description::Description() -{ - // Do nothing -} - //============================================================================== WaitFor::Model::Model( State invariant_initial_state, diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.hpp similarity index 97% rename from rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp rename to rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.hpp index fd7ac310..e4add81a 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/phases/internal_PayloadTransfer.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.hpp @@ -26,7 +26,7 @@ #include namespace rmf_task_sequence { -namespace phases { +namespace events { //============================================================================== class PayloadTransfer @@ -57,7 +57,7 @@ class PayloadTransfer const Parameters& parameters) const; }; -} // namespace phases +} // namespace events } // namespace rmf_task_sequence #endif // SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp index 444fa7e2..fc026582 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp @@ -70,6 +70,13 @@ class Sequence::Standby : public Event::Standby const Bundle::Description& description, std::function parent_update); + using MakeStandby = std::function; + + static Event::StandbyPtr initiate( + const std::vector& dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function update); + Event::ConstStatePtr state() const final; rmf_traffic::Duration duration_estimate() const final; @@ -79,7 +86,7 @@ class Sequence::Standby : public Event::Standby std::function finish) final; Standby( - std::vector dependencies, + std::vector reverse_dependencies, rmf_task::events::SimpleEventStatePtr state, std::function parent_update); @@ -91,7 +98,7 @@ class Sequence::Standby : public Event::Standby private: - std::vector _dependencies; + std::vector _reverse_dependencies; rmf_task::events::SimpleEventStatePtr _state; std::function _parent_update; std::shared_ptr _active; diff --git a/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp similarity index 98% rename from rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp rename to rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp index 2ee8f707..0d369232 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/SimplePhase.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp @@ -15,9 +15,10 @@ * */ -#include +#include namespace rmf_task_sequence { +namespace phases { //============================================================================== class SimplePhase::Description::Implementation @@ -254,4 +255,5 @@ SimplePhase::Description::Description() // Do nothing } +} // namespace phases } // namespace rmf_task_sequence diff --git a/rmf_task_sequence/test/unit/test_Sequence.cpp b/rmf_task_sequence/test/unit/test_Sequence.cpp index 9efca8f1..1654c1aa 100644 --- a/rmf_task_sequence/test/unit/test_Sequence.cpp +++ b/rmf_task_sequence/test/unit/test_Sequence.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include @@ -49,14 +49,14 @@ rmf_task_sequence::Phase::ConstDescriptionPtr make_sequence_for_phase( std::make_shared( descs, rmf_task_sequence::events::Bundle::Type::Sequence); - return rmf_task_sequence::SimplePhase::Description::make(sequence); + return rmf_task_sequence::phases::SimplePhase::Description::make(sequence); } //============================================================================== rmf_task_sequence::Phase::ConstDescriptionPtr make_phase( std::shared_ptr ctrl) { - return rmf_task_sequence::SimplePhase::Description::make( + return rmf_task_sequence::phases::SimplePhase::Description::make( std::make_shared(std::move(ctrl))); } @@ -123,7 +123,8 @@ SCENARIO("Test Event Sequences") const auto phase_activator = std::make_shared(); - rmf_task_sequence::SimplePhase::add(*phase_activator, event_initializer); + rmf_task_sequence::phases::SimplePhase::add( + *phase_activator, event_initializer); rmf_task::Activator task_activator; rmf_task_sequence::Task::add( From 9842bd9a801a6782105fb9f5638e1c74e1ccbb1a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 16 Dec 2021 23:50:19 +0800 Subject: [PATCH 77/85] Remember to include implementation header Signed-off-by: Michael X. Grey --- rmf_task_sequence/include/rmf_task_sequence/Task.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index 018697b8..48587f1e 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -185,4 +185,6 @@ class Task::Description : public rmf_task::Task::Description } // namespace rmf_task_sequence +#include + #endif // RMF_TASK_SEQUENCE__TASK_HPP From 0112e25baafc98222a5255996dba1e92b7b8e5be Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 17 Dec 2021 19:02:26 +0800 Subject: [PATCH 78/85] Make activate and restore const Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Activator.hpp | 4 ++-- rmf_task/src/rmf_task/Activator.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmf_task/include/rmf_task/Activator.hpp b/rmf_task/include/rmf_task/Activator.hpp index 3476134f..4be605fa 100644 --- a/rmf_task/include/rmf_task/Activator.hpp +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -122,7 +122,7 @@ class Activator std::function update, std::function checkpoint, std::function phase_finished, - std::function task_finished); + std::function task_finished) const; /// Restore a Task that crashed or disconnected. /// @@ -160,7 +160,7 @@ class Activator std::function update, std::function checkpoint, std::function phase_finished, - std::function task_finished); + std::function task_finished) const; class Implementation; private: diff --git a/rmf_task/src/rmf_task/Activator.cpp b/rmf_task/src/rmf_task/Activator.cpp index eccb78aa..3182cf46 100644 --- a/rmf_task/src/rmf_task/Activator.cpp +++ b/rmf_task/src/rmf_task/Activator.cpp @@ -43,7 +43,7 @@ Task::ActivePtr Activator::activate( std::function update, std::function checkpoint, std::function phase_finished, - std::function task_finished) + std::function task_finished) const { // TODO(MXG): Should we issue some kind of error/warning to distinguish // between a missing description versus a description that doesn't have a @@ -78,7 +78,7 @@ Task::ActivePtr Activator::restore( std::function update, std::function checkpoint, std::function phase_finished, - std::function task_finished) + std::function task_finished) const { if (!request.description()) return nullptr; From 070e86440b2d3f2e16cbd3669c8010f8a9b08e55 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 20 Dec 2021 22:22:30 +0800 Subject: [PATCH 79/85] Add sequence numbers to logs and fix log iteration Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Log.hpp | 7 +++ rmf_task/src/rmf_task/Log.cpp | 60 ++++++++++++++----- rmf_task/test/unit/test_Log.cpp | 60 +++++++++++++++++++ .../cmake/rmf_task_sequence-config.cmake.in | 1 + .../src/rmf_task_sequence/events/Sequence.cpp | 2 +- 5 files changed, 113 insertions(+), 17 deletions(-) create mode 100644 rmf_task/test/unit/test_Log.cpp diff --git a/rmf_task/include/rmf_task/Log.hpp b/rmf_task/include/rmf_task/Log.hpp index 2fe214e0..ca6055cd 100644 --- a/rmf_task/include/rmf_task/Log.hpp +++ b/rmf_task/include/rmf_task/Log.hpp @@ -97,6 +97,10 @@ class Log::Entry /// What was the tier of this entry. Tier tier() const; + /// Sequence number for this log entry. This increments once for each new + /// log entry, until overflowing and wrapping around to 0. + uint32_t seq() const; + /// What was the timestamp of this entry. rmf_traffic::Time time() const; @@ -203,6 +207,9 @@ class Log::Reader::Iterable::iterator /// Equality comparison operator bool operator==(const iterator& other) const; + /// Inequality comparison operator + bool operator!=(const iterator& other) const; + class Implementation; private: iterator(); diff --git a/rmf_task/src/rmf_task/Log.cpp b/rmf_task/src/rmf_task/Log.cpp index f0900205..76a073da 100644 --- a/rmf_task/src/rmf_task/Log.cpp +++ b/rmf_task/src/rmf_task/Log.cpp @@ -27,6 +27,7 @@ class Log::Implementation public: std::function clock; std::shared_ptr> entries; + uint32_t seq = 0; Implementation(std::function clock_) : clock(std::move(clock_)), @@ -52,6 +53,7 @@ class Log::Entry::Implementation static Entry make( Tier tier, + uint32_t seq, rmf_traffic::Time time, std::string text) { @@ -59,6 +61,7 @@ class Log::Entry::Implementation output._pimpl = rmf_utils::make_impl( Implementation{ tier, + seq, time, std::move(text) }); @@ -67,6 +70,7 @@ class Log::Entry::Implementation } Tier tier; + uint32_t seq; rmf_traffic::Time time; std::string text; @@ -140,25 +144,25 @@ class Log::Reader::Implementation class Log::Reader::Iterable::Implementation { public: + using base_iterator = std::list::const_iterator; std::shared_ptr> shared; std::optional begin; static Log::Reader::Iterable make( std::shared_ptr> shared, - std::optional::const_iterator> begin, - std::optional::const_iterator> last); + std::optional begin, + std::optional last); }; //============================================================================== class Log::Reader::Iterable::iterator::Implementation { public: - std::list::const_iterator it; - std::list::const_iterator last; + using base_iterator = std::list::const_iterator; + base_iterator it; + base_iterator last; - static iterator make( - std::list::const_iterator it, - std::list::const_iterator last) + static iterator make(base_iterator it, base_iterator last) { iterator output; output._pimpl = rmf_utils::make_impl( @@ -176,16 +180,25 @@ class Log::Reader::Iterable::iterator::Implementation //============================================================================== Log::Reader::Iterable Log::Reader::Iterable::Implementation::make( std::shared_ptr> shared, - std::optional::const_iterator> begin, - std::optional::const_iterator> last) + std::optional begin, + std::optional last) { Iterable iterable; iterable._pimpl = rmf_utils::make_impl(); iterable._pimpl->shared = std::move(shared); if (begin.has_value()) { - iterable._pimpl->begin = - iterator::Implementation::make(*begin, last.value()); + if (++base_iterator(last.value()) == *begin) + { + // If the beginning iterator is already the end() iterator, we should + // directly set it to that right now. + iterable._pimpl->begin = iterator::Implementation::end(); + } + else + { + iterable._pimpl->begin = + iterator::Implementation::make(*begin, last.value()); + } } else { @@ -234,7 +247,7 @@ void Log::info(std::string text) { _pimpl->entries->emplace_back( Entry::Implementation::make( - Entry::Tier::Info, _pimpl->clock(), std::move(text))); + Entry::Tier::Info, _pimpl->seq++, _pimpl->clock(), std::move(text))); } //============================================================================== @@ -242,7 +255,7 @@ void Log::warn(std::string text) { _pimpl->entries->emplace_back( Entry::Implementation::make( - Entry::Tier::Warning, _pimpl->clock(), std::move(text))); + Entry::Tier::Warning, _pimpl->seq++, _pimpl->clock(), std::move(text))); } //============================================================================== @@ -250,7 +263,7 @@ void Log::error(std::string text) { _pimpl->entries->emplace_back( Entry::Implementation::make( - Entry::Tier::Error, _pimpl->clock(), std::move(text))); + Entry::Tier::Error, _pimpl->seq++, _pimpl->clock(), std::move(text))); } //============================================================================== @@ -271,6 +284,12 @@ auto Log::Entry::tier() const -> Tier return _pimpl->tier; } +//============================================================================== +uint32_t Log::Entry::seq() const +{ + return _pimpl->seq; +} + //============================================================================== rmf_traffic::Time Log::Entry::time() const { @@ -335,6 +354,9 @@ auto Log::Reader::Iterable::iterator::operator->() const -> const Entry* //============================================================================== auto Log::Reader::Iterable::iterator::operator++() -> iterator& { + if (!_pimpl.get()) + return *this; + if (_pimpl->it == _pimpl->last) _pimpl = nullptr; else @@ -354,12 +376,18 @@ auto Log::Reader::Iterable::iterator::operator++(int) -> iterator //============================================================================== bool Log::Reader::Iterable::iterator::operator==(const iterator& other) const { - if (!_pimpl || !other._pimpl) - return _pimpl == other._pimpl; + if (!_pimpl.get() || !other._pimpl.get()) + return _pimpl.get() == other._pimpl.get(); return _pimpl->it == other._pimpl->it; } +//============================================================================== +bool Log::Reader::Iterable::iterator::operator!=(const iterator& other) const +{ + return !(*this == other); +} + //============================================================================== Log::Reader::Iterable::iterator::iterator() { diff --git a/rmf_task/test/unit/test_Log.cpp b/rmf_task/test/unit/test_Log.cpp new file mode 100644 index 00000000..eacbf803 --- /dev/null +++ b/rmf_task/test/unit/test_Log.cpp @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2021 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 + +#include + +SCENARIO("Writing and reading logs") +{ + rmf_task::Log log; + rmf_task::Log::Reader reader; + + std::size_t expected_count = 0; + std::size_t count = 0; + for (const auto& entry : reader.read(log.view())) + { + CHECK(entry.seq() == count); + ++count; + } + + CHECK(count == expected_count); + + log.info("Test text"); + log.info("More test text"); + log.warn("Some warning text"); + + expected_count = 3; + count = 0; + for (const auto& entry : reader.read(log.view())) + { + CHECK(entry.seq() == count); + ++count; + } + + CHECK(count == expected_count); + + expected_count = 0; + count = 0; + for (const auto& entry : reader.read(log.view())) + { + (void)(entry); + ++count; + } + + CHECK(count == expected_count); +} diff --git a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in index d4cd585a..01c062ba 100644 --- a/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in +++ b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in @@ -6,6 +6,7 @@ include(CMakeFindDependencyMacro) find_dependency(rmf_task) find_dependency(nlohmann_json) +find_dependency(nlohmann_json_schema_validator_vendor) if(NOT TARGET rmf_task_sequence::rmf_task_sequence) include("${rmf_task_sequence_CMAKE_DIR}/rmf_task_sequence-targets.cmake") diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp index c3a09324..6f2519ff 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -111,7 +111,7 @@ Sequence::Standby::Standby( state_deps.push_back((*rit)->state()); _state->update_dependencies(std::move(state_deps)); - update_status(*state); + update_status(*_state); } //============================================================================== From 3aef75feeb958761dc03fe5438b662a293a9d834 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 21 Dec 2021 23:21:15 +0800 Subject: [PATCH 80/85] Add plain text info for task descriptions Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/Header.hpp | 6 +++ rmf_task/include/rmf_task/Task.hpp | 21 ++++++++++ .../rmf_task/requests/ChargeBattery.hpp | 7 +++- rmf_task/include/rmf_task/requests/Clean.hpp | 7 +++- .../include/rmf_task/requests/Delivery.hpp | 7 +++- rmf_task/include/rmf_task/requests/Loop.hpp | 7 +++- rmf_task/src/rmf_task/Header.cpp | 17 ++++++++ .../src/rmf_task/requests/ChargeBattery.cpp | 11 +++++ rmf_task/src/rmf_task/requests/Clean.cpp | 13 ++++++ rmf_task/src/rmf_task/requests/Delivery.cpp | 13 ++++++ rmf_task/src/rmf_task/requests/Loop.cpp | 13 ++++++ rmf_task/test/mock/MockTask.cpp | 9 ++++ rmf_task/test/mock/MockTask.hpp | 2 + .../include/rmf_task_sequence/Task.hpp | 5 +++ .../src/rmf_task_sequence/Task.cpp | 41 +++++++++++++++++++ .../rmf_task_sequence/events/GoToPlace.cpp | 28 ++++--------- 16 files changed, 182 insertions(+), 25 deletions(-) diff --git a/rmf_task/include/rmf_task/Header.hpp b/rmf_task/include/rmf_task/Header.hpp index b9924968..dbf7f77e 100644 --- a/rmf_task/include/rmf_task/Header.hpp +++ b/rmf_task/include/rmf_task/Header.hpp @@ -19,6 +19,7 @@ #define RMF_TASK__HEADER_HPP #include +#include #include @@ -60,6 +61,11 @@ class Header rmf_utils::impl_ptr _pimpl; }; +//============================================================================== +std::string standard_waypoint_name( + const rmf_traffic::agv::Graph& graph, + std::size_t waypoint); + } // namespace rmf_task #endif // RMF_TASK__HEADER_HPP diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index a96f5b49..00a487fb 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -163,6 +163,24 @@ class Task::Description rmf_traffic::Time earliest_start_time, const Parameters& parameters) const = 0; + struct Info + { + std::string category; + std::string detail; + }; + + /// Generate a plain text info description for the task, given the predicted + /// initial state and the task planning parameters. + /// + /// \param[in] initial_state + /// The predicted initial state for the task + /// + /// \param[in] parameters + /// The task planning parameters + virtual Info generate_info( + const State& initial_state, + const Parameters& parameters) const = 0; + // Virtual destructor virtual ~Description() = default; }; @@ -180,6 +198,9 @@ class Task::Active /// with the highest sequence number will be kept. using Backup = detail::Backup; + /// Get a quick overview status of how the task is going + virtual Event::Status status_overview() const = 0; + /// Descriptions of the phases that have been completed virtual const std::vector& completed_phases() const = 0; diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index a8b09dd6..61fecf17 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -53,11 +53,16 @@ class ChargeBattery /// Generate the description for this request static Task::ConstDescriptionPtr make(); - /// Documentation inherited + // Documentation inherited Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; + // Documentation inherited + Info generate_info( + const State& initial_state, + const Parameters& parameters) const final; + class Implementation; private: Description(); diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index bac6ec03..5f651965 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -56,11 +56,16 @@ class Clean std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path); - /// Documentation inherited + // Documentation inherited Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; + // Documentation inherited + Info generate_info( + const State& initial_state, + const Parameters& parameters) const final; + /// Get the start waypoint in this request std::size_t start_waypoint() const; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 5fb8edb2..200bea28 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -62,11 +62,16 @@ class Delivery std::string pickup_from_dispenser = "", std::string dropoff_to_ingestor = ""); - /// Documentation inherited + // Documentation inherited Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; + // Documentation inherited + Info generate_info( + const State& initial_state, + const Parameters& parameters) const final; + /// Get the pickup waypoint in this request std::size_t pickup_waypoint() const; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 633e5481..660f3014 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -57,11 +57,16 @@ class Loop std::size_t finish_waypoint, std::size_t num_loops); - /// Documentation inherited + // Documentation inherited Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; + // Documentation inherited + Info generate_info( + const State& initial_state, + const Parameters& parameters) const final; + /// Get the start waypoint of the loop in this request std::size_t start_waypoint() const; diff --git a/rmf_task/src/rmf_task/Header.cpp b/rmf_task/src/rmf_task/Header.cpp index a46bf733..97d86fd6 100644 --- a/rmf_task/src/rmf_task/Header.cpp +++ b/rmf_task/src/rmf_task/Header.cpp @@ -61,4 +61,21 @@ rmf_traffic::Duration Header::original_duration_estimate() const return _pimpl->duration; } +//============================================================================== +std::string standard_waypoint_name( + const rmf_traffic::agv::Graph& graph, + std::size_t waypoint) +{ + if (waypoint >= graph.num_waypoints()) + { + throw std::runtime_error( + "[rmf_task::standard_waypoint_name] Waypoint index [" + + std::to_string(waypoint) + "] is too high for the number of waypoints [" + + std::to_string(graph.num_waypoints()) + "] in the graph"); + } + + return graph.get_waypoint(waypoint).name_or_index( + "[place:%s]", "[graph-wp:%d]"); +} + } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 6bf7f0b4..32c5364f 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -190,6 +190,17 @@ Task::ConstModelPtr ChargeBattery::Description::make_model( parameters); } +//============================================================================== +auto ChargeBattery::Description::generate_info( + const State&, + const Parameters&) const -> Info +{ + return Info{ + "Charge battery", + "" + }; +} + //============================================================================== ConstRequestPtr ChargeBattery::make( rmf_traffic::Time earliest_start_time, diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 5529808d..2b0b8e5c 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -246,6 +246,19 @@ Task::ConstModelPtr Clean::Description::make_model( _pimpl->end_waypoint); } +//============================================================================== +auto Clean::Description::generate_info( + const State&, + const Parameters& parameters) const -> Info +{ + const auto& graph = parameters.planner()->get_configuration().graph(); + + return Info{ + "Clean " + standard_waypoint_name(graph, _pimpl->start_waypoint), + "", + }; +} + //============================================================================== std::size_t Clean::Description::start_waypoint() const { diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 64b7cb2b..78644bce 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -262,6 +262,19 @@ Task::ConstModelPtr Delivery::Description::make_model( _pimpl->dropoff_wait); } +//============================================================================== +auto Delivery::Description::generate_info( + const State&, + const Parameters& parameters) const -> Info +{ + const auto& graph = parameters.planner()->get_configuration().graph(); + return Info{ + "Delivery from " + standard_waypoint_name(graph, _pimpl->pickup_waypoint) + + " to " + standard_waypoint_name(graph, _pimpl->dropoff_waypoint), + "" // TODO(MXG): Add details about the payload + }; +} + //============================================================================== std::size_t Delivery::Description::pickup_waypoint() const { diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 0508e9ca..49331ef2 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -253,6 +253,19 @@ Task::ConstModelPtr Loop::Description::make_model( _pimpl->num_loops); } +//============================================================================== +auto Loop::Description::generate_info( + const rmf_task::State&, + const Parameters& parameters) const -> Info +{ + const auto& graph = parameters.planner()->get_configuration().graph(); + return Info{ + "Loop between " + standard_waypoint_name(graph, _pimpl->start_waypoint) + + " and " + standard_waypoint_name(graph, _pimpl->finish_waypoint), + std::to_string(_pimpl->num_loops) + " times" + }; +} + //============================================================================== std::size_t Loop::Description::start_waypoint() const { diff --git a/rmf_task/test/mock/MockTask.cpp b/rmf_task/test/mock/MockTask.cpp index 78068faa..0a7391cb 100644 --- a/rmf_task/test/mock/MockTask.cpp +++ b/rmf_task/test/mock/MockTask.cpp @@ -19,6 +19,15 @@ namespace test_rmf_task { +//============================================================================== +rmf_task::Event::Status MockTask::Active::status_overview() const +{ + if (_active_phase) + return _active_phase->final_event()->status(); + + return rmf_task::Event::Status::Completed; +} + //============================================================================== auto MockTask::Active::completed_phases() const -> const std::vector& diff --git a/rmf_task/test/mock/MockTask.hpp b/rmf_task/test/mock/MockTask.hpp index f4ad58aa..05b58286 100644 --- a/rmf_task/test/mock/MockTask.hpp +++ b/rmf_task/test/mock/MockTask.hpp @@ -35,6 +35,8 @@ class MockTask : public rmf_task::Task { public: + rmf_task::Event::Status status_overview() const override; + const std::vector& completed_phases() const override; diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp index 48587f1e..b67d450a 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -161,6 +161,11 @@ class Task::Description : public rmf_task::Task::Description rmf_traffic::Time earliest_start_time, const Parameters& parameters) const final; + // Documentation inherited + Info generate_info( + const State& initial_state, + const Parameters& parameters) const final; + /// Get the category for this task const std::string& category() const; diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 143927a9..5c70d048 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -164,6 +164,9 @@ class Task::Active return task; } + // Documentation inherited + Event::Status status_overview() const final; + // Documentation inherited const std::vector& completed_phases() const final; @@ -348,6 +351,17 @@ Task::ConstModelPtr Task::Description::make_model( earliest_start_time); } +//============================================================================== +auto Task::Description::generate_info( + const rmf_task::State&, + const rmf_task::Parameters&) const -> Info +{ + return Info{ + category(), + detail() + }; +} + //============================================================================== const std::string& Task::Description::category() const { @@ -395,6 +409,33 @@ Task::Description::Description() // Do nothing } +//============================================================================== +rmf_task::Event::Status Task::Active::status_overview() const +{ + if (_active_phase) + return _active_phase->final_event()->status(); + + if (_completed_phases.empty() && _pending_phases.empty()) + { + // This means the task had no phases..? So it's completed by default. + return Event::Status::Completed; + } + + if (_pending_phases.empty()) + { + // There are no pending phases, so the status of this task should be + // reflected by the status of the last phase. + return _completed_phases.back()->snapshot()->final_event()->status(); + } + + // There is no active phase, but there are pending phases remaining. This + // must mean this function is being called while the task phase is switching, + // which could technically cause a data race. We'll just go ahead and say that + // the status is Underway, but maybe we should consider making noise about + // this. + return Event::Status::Underway; +} + //============================================================================== const std::vector& Task::Active::completed_phases() const diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp index b53529d8..bc9ac2ec 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp @@ -179,20 +179,6 @@ class GoToPlace::Description::Implementation public: rmf_traffic::agv::Plan::Goal destination; - - static std::string waypoint_name( - const std::size_t index, - const Parameters& parameters) - { - const auto& graph = parameters.planner()->get_configuration().graph(); - if (index < graph.num_waypoints()) - { - if (const auto* name = graph.get_waypoint(index).name()) - return *name; - } - - return "#" + std::to_string(index); - } }; //============================================================================== @@ -243,7 +229,7 @@ Header GoToPlace::Description::generate_header( + "]"); } - const auto start_name = Implementation::waypoint_name(start_wp, parameters); + const auto start_name = rmf_task::standard_waypoint_name(graph, start_wp); if (graph.num_waypoints() <= _pimpl->destination.waypoint()) { @@ -260,13 +246,12 @@ Header GoToPlace::Description::generate_header( if (!estimate.has_value()) { - fail("Cannot find a path from [" - + start_name + "] to [" + goal_name_ + "]"); + fail("Cannot find a path from " + start_name + " to " + goal_name_); } return Header( - "Go to [" + goal_name_ + "]", - "Moving the robot from [" + start_name + "] to [" + goal_name_ + "]", + "Go to " + goal_name_, + "Moving the robot from " + start_name + " to " + goal_name_, *estimate); } @@ -287,8 +272,9 @@ auto GoToPlace::Description::destination(Goal new_goal) -> Description& std::string GoToPlace::Description::destination_name( const Parameters& parameters) const { - return Implementation::waypoint_name( - _pimpl->destination.waypoint(), parameters); + return rmf_task::standard_waypoint_name( + parameters.planner()->get_configuration().graph(), + _pimpl->destination.waypoint()); } //============================================================================== From 416503d71679c5227e1c6b98fb80212548c987e3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 22 Dec 2021 21:55:09 +0800 Subject: [PATCH 81/85] Handle cases with no ambient drain Signed-off-by: Michael X. Grey --- .../src/rmf_task_sequence/events/WaitFor.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp index e4f3f9ae..b82506aa 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp @@ -113,9 +113,16 @@ WaitFor::Model::Model( : _invariant_finish_state(std::move(invariant_initial_state)), _duration(duration) { - _invariant_battery_drain = - parameters.ambient_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(_duration)); + if (parameters.ambient_sink()) + { + _invariant_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_duration)); + } + else + { + _invariant_battery_drain = 0.0; + } } //============================================================================== From f05dda0a7ad7eb9cae84dbbaa6c2fa2920297158 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 22 Dec 2021 22:28:27 +0800 Subject: [PATCH 82/85] Create a Placeholder event to help eliminate boilerplate Signed-off-by: Michael X. Grey --- .../rmf_task_sequence/events/Placeholder.hpp | 62 ++++++++++++ .../rmf_task_sequence/events/WaitFor.hpp | 6 +- .../rmf_task_sequence/events/Placeholder.cpp | 95 +++++++++++++++++++ 3 files changed, 160 insertions(+), 3 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/Placeholder.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Placeholder.cpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/Placeholder.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/Placeholder.hpp new file mode 100644 index 00000000..8b199c8f --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Placeholder.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__PLACEHOLDER_HPP +#define RMF_TASK_SEQUENCE__EVENTS__PLACEHOLDER_HPP + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// A Placeholder event takes care of the boilerplate needed to define a +/// description for an event whose model does not matter and will not be used +/// for planning. The model generated by this description will not perform any +/// changes to the task state, and it will provide an estimated duration of 0. +class Placeholder +{ +public: + + class Description; + class Model; +}; + +//============================================================================== +class Placeholder::Description : public Event::Description +{ +public: + + Description(std::string category, std::string detail); + + Activity::ConstModelPtr make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const override; + + rmf_task::Header generate_header( + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const override; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__PLACEHOLDER_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp index 2b67eb30..290f446c 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp @@ -20,13 +20,13 @@ #include -#include +#include namespace rmf_task_sequence { namespace events { //============================================================================== -/// A WaitFor phase encompasses having the robot sit in place and wait for a +/// A WaitFor event encompasses having the robot sit in place and wait for a /// period of time to pass. /// /// The Model of this phase may be useful for calculating the Models of other @@ -46,7 +46,7 @@ class WaitFor }; //============================================================================== -class WaitFor::Description : public Phase::Description +class WaitFor::Description : public Event::Description { public: diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Placeholder.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Placeholder.cpp new file mode 100644 index 00000000..59f7fded --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Placeholder.cpp @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class Placeholder::Description::Implementation +{ +public: + + std::string category; + std::string detail; + +}; + +//============================================================================== +class Placeholder::Model : public Activity::Model +{ +public: + + Model(State invariant_initial_state) + : _invariant_finish_state(std::move(invariant_initial_state)) + { + // Do nothing + } + + std::optional estimate_finish( + State initial_state, + rmf_traffic::Time earliest_arrival_time, + const Constraints&, + const TravelEstimator&) const final + { + return Estimate(std::move(initial_state), earliest_arrival_time); + } + + rmf_traffic::Duration invariant_duration() const final + { + return rmf_traffic::Duration(0); + } + + State invariant_finish_state() const final + { + return _invariant_finish_state; + } + + State _invariant_finish_state; +}; + +//============================================================================== +Placeholder::Description::Description( + std::string category, + std::string detail) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(category), + std::move(detail) + })) +{ + // Do nothing +} + +//============================================================================== +Activity::ConstModelPtr Placeholder::Description::make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters&) const +{ + return std::make_shared(std::move(invariant_initial_state)); +} + +//============================================================================== +Header Placeholder::Description::generate_header( + const State&, const Parameters&) const +{ + return Header(_pimpl->category, _pimpl->detail, rmf_traffic::Duration(0)); +} + +} // namespace events +} // namespace rmf_task_sequence From 9fbd9d403d7e5d0dba6045b722efc56d15687c80 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 31 Dec 2021 14:20:01 +0800 Subject: [PATCH 83/85] Fix circular reference Signed-off-by: Michael X. Grey --- .../src/rmf_task_sequence/events/Bundle.cpp | 56 ++++++++++++++++++- 1 file changed, 55 insertions(+), 1 deletion(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp index e151915f..fd5e5f04 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -294,7 +294,61 @@ Header Bundle::Description::generate_header( //============================================================================== void Bundle::add(const Event::InitializerPtr& initializer) { - add(*initializer, initializer); + initializer->add( + [w = std::weak_ptr(initializer)]( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + std::function update) + { + const auto& initialize_from = w.lock(); + if (!initialize_from) + { + throw std::runtime_error( + "[rmf_task_sequence::Bundle::add] Use-after-free error: Event " + "initializer has already destructed, but is still being used to " + "initialize an event."); + } + + return initiate( + *initialize_from, + id, + get_state, + parameters, + description, + std::move(update)); + }, + [w = std::weak_ptr(initializer)]( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + const nlohmann::json& backup_state, + std::function update, + std::function checkpoint, + std::function finished) + { + const auto& initialize_from = w.lock(); + if (!initialize_from) + { + throw std::runtime_error( + "[rmf_task_sequence::Bundle::add] Use-after-free error: Event " + "initializer has already destructed, but is still being used to " + "initialize an event."); + } + + return restore( + *initialize_from, + id, + get_state, + parameters, + description, + backup_state, + std::move(update), + std::move(checkpoint), + std::move(finished)); + }); } //============================================================================== From 113b671d97b8bc78c3774f85d3854b9fc78bd224 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 9 Jan 2022 19:57:09 +0800 Subject: [PATCH 84/85] Return more specific Clean Description Signed-off-by: Michael X. Grey --- rmf_task/include/rmf_task/requests/Clean.hpp | 2 +- rmf_task/src/rmf_task/requests/Clean.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 5f651965..e9f94c84 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -51,7 +51,7 @@ class Clean public: /// Generate the description for this request - static Task::ConstDescriptionPtr make( + static std::shared_ptr make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path); diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 2b0b8e5c..8d995cf4 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -205,7 +205,7 @@ class Clean::Description::Implementation }; //============================================================================== -Task::ConstDescriptionPtr Clean::Description::make( +std::shared_ptr Clean::Description::make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path) From 11bf2815e2daa35ad1c988c7360fa2a4fea70e64 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 14 Jan 2022 11:22:52 +0800 Subject: [PATCH 85/85] Add missing files Signed-off-by: Yadunund --- .../rmf_task_sequence/events/ContactCard.hpp | 115 +++++++++++++ .../rmf_task_sequence/events/ContactCard.cpp | 154 ++++++++++++++++++ 2 files changed, 269 insertions(+) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/ContactCard.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/ContactCard.cpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/ContactCard.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/ContactCard.hpp new file mode 100644 index 00000000..bb07299e --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/ContactCard.hpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2021 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 RMF_TASK_SEQUENCE__EVENTS__CONTACTCARD_HPP +#define RMF_TASK_SEQUENCE__EVENTS__CONTACTCARD_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// Store contact details for telecommunication +class ContactCard +{ +public: + + class PhoneNumber + { + public: + + /// Constructor + /// + /// \param[in] country_code + /// The country code without "+"" + /// + /// \param[in] number + /// The phone number + PhoneNumber(uint32_t country_code, uint32_t number); + + /// Get the country code + uint32_t country_code() const; + + /// Set the country code + PhoneNumber& country_code(uint32_t new_code); + + /// Get the phone number + uint32_t number() const; + + /// Set the phone number + PhoneNumber& number(uint32_t new_number); + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + /// Constructor + /// + /// \param[in] name + /// The name of the contact + /// + /// \param[in] address + /// The physical address of the contact + /// + /// \param[in] email + /// The email address of the contact + /// + /// \param[in] number + /// The phone number of the contact + ContactCard( + const std::string& name, + std::optional address, + std::optional email, + PhoneNumber number); + + /// Get the name + const std::string& name() const; + + /// Set the name + ContactCard& name(const std::string& new_name); + + /// Get the address + std::optional address() const; + + /// Set the address + ContactCard& address(std::optional new_address); + + /// Get the email + std::optional email() const; + + /// Set the email + ContactCard& email(std::optional new_email); + + /// Get the phone number + const PhoneNumber& number() const; + + /// Set the phone number + ContactCard& number(PhoneNumber new_number); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__CONTACTCARD_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/ContactCard.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/ContactCard.cpp new file mode 100644 index 00000000..a52325c1 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/ContactCard.cpp @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2021 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 + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class ContactCard::PhoneNumber::Implementation +{ +public: + + uint32_t country_code; + uint32_t number; +}; + +//============================================================================== +ContactCard::PhoneNumber::PhoneNumber( + uint32_t country_code, + uint32_t number) +: _pimpl(rmf_utils::make_impl( + Implementation{ + country_code, + number + })) +{ + // Do nothing +} + +//============================================================================== +uint32_t ContactCard::PhoneNumber::country_code() const +{ + return _pimpl->country_code; +} + +//============================================================================== +auto ContactCard::PhoneNumber::country_code(uint32_t new_code) +-> PhoneNumber& +{ + _pimpl->country_code = new_code; + return *this; +} + +//============================================================================== +uint32_t ContactCard::PhoneNumber::number() const +{ + return _pimpl->number; +} + +//============================================================================== +auto ContactCard::PhoneNumber::number(uint32_t new_number) +-> PhoneNumber& +{ + _pimpl->number = new_number; + return *this; +} + +//============================================================================== +class ContactCard::Implementation +{ +public: + + std::string name; + std::optional address; + std::optional email; + PhoneNumber number; +}; + +//============================================================================== +ContactCard::ContactCard( + const std::string& name, + std::optional address, + std::optional< std::string> email, + PhoneNumber number) +: _pimpl(rmf_utils::make_impl( + Implementation{ + name, + address, + email, + std::move(number) + })) +{ + // Do nothing +} + +//============================================================================== +const std::string& ContactCard::name() const +{ + return _pimpl->name; +} + +//============================================================================== +ContactCard& ContactCard::name(const std::string& new_name) +{ + _pimpl->name = new_name; + return *this; +} + +//============================================================================== +std::optional ContactCard::address() const +{ + return _pimpl->address; +} + +//============================================================================== +ContactCard& ContactCard::address(std::optional new_address) +{ + _pimpl->address = new_address; + return *this; +} + +//============================================================================== +std::optional ContactCard::email() const +{ + return _pimpl->email; +} + +//============================================================================== +ContactCard& ContactCard::email(std::optional new_email) +{ + _pimpl->email = new_email; + return *this; +} + +//============================================================================== +const ContactCard::PhoneNumber& ContactCard::number() const +{ + return _pimpl->number; +} + +//============================================================================== +ContactCard& ContactCard::number(ContactCard::PhoneNumber new_number) +{ + _pimpl->number = std::move(new_number); + return *this; +} + +} // namespace events +} // namespace rmf_task_sequence