From 27f28653dfd4bcf24af8be4c453fb96c81d3d57c Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 14 Feb 2022 00:56:16 +0800 Subject: [PATCH] Support flexible task definitions (#39) Co-authored-by: Michael X. Grey Signed-off-by: Michael X. Grey Co-authored-by: Charayaphan Nakorn Boon Han Signed-off-by: Charayaphan Nakorn Boon Han Co-authored-by: youliang Signed-off-by: youliang Co-authored-by: Yadu Signed-off-by: Yadu Co-authored-by: Xiyu Signed-off-by: Xiyu --- .github/workflows/build.yaml | 4 +- .github/workflows/tsan.yaml | 6 +- rmf_task/CMakeLists.txt | 5 +- rmf_task/include/rmf_task/Activator.hpp | 183 ++++ .../include/rmf_task/BackupFileManager.hpp | 109 ++ rmf_task/include/rmf_task/CompositeData.hpp | 117 +++ .../rmf_task/{agv => }/Constraints.hpp | 2 - rmf_task/include/rmf_task/Estimate.hpp | 57 +- rmf_task/include/rmf_task/Event.hpp | 192 ++++ rmf_task/include/rmf_task/Header.hpp | 71 ++ rmf_task/include/rmf_task/Log.hpp | 221 ++++ .../include/rmf_task/{agv => }/Parameters.hpp | 6 +- rmf_task/include/rmf_task/Payload.hpp | 79 ++ rmf_task/include/rmf_task/Phase.hpp | 174 ++++ rmf_task/include/rmf_task/Request.hpp | 85 +- rmf_task/include/rmf_task/RequestFactory.hpp | 2 +- rmf_task/include/rmf_task/State.hpp | 111 ++ rmf_task/include/rmf_task/Task.hpp | 312 ++++++ .../rmf_task/{agv => }/TaskPlanner.hpp | 14 +- rmf_task/include/rmf_task/VersionedString.hpp | 97 ++ rmf_task/include/rmf_task/agv/State.hpp | 96 -- rmf_task/include/rmf_task/detail/Backup.hpp | 66 ++ rmf_task/include/rmf_task/detail/Resume.hpp | 49 + .../rmf_task/detail/impl_Activator.hpp | 57 ++ .../rmf_task/detail/impl_CompositeData.hpp | 80 ++ .../rmf_task/events/SimpleEventState.hpp | 95 ++ .../include/rmf_task/phases/RestoreBackup.hpp | 80 ++ .../rmf_task/requests/ChargeBattery.hpp | 17 +- .../requests/ChargeBatteryFactory.hpp | 4 +- rmf_task/include/rmf_task/requests/Clean.hpp | 17 +- .../include/rmf_task/requests/Delivery.hpp | 37 +- rmf_task/include/rmf_task/requests/Loop.hpp | 17 +- .../rmf_task/requests/ParkRobotFactory.hpp | 4 +- rmf_task/src/rmf_task/Activator.cpp | 112 ++ rmf_task/src/rmf_task/BackupFileManager.cpp | 345 +++++++ .../rmf_task/BinaryPriorityCostCalculator.cpp | 16 +- .../rmf_task/BinaryPriorityCostCalculator.hpp | 3 +- rmf_task/src/rmf_task/CompositeData.cpp | 84 ++ .../src/rmf_task/{agv => }/Constraints.cpp | 6 +- rmf_task/src/rmf_task/CostCalculator.hpp | 5 +- rmf_task/src/rmf_task/Estimate.cpp | 161 ++- rmf_task/src/rmf_task/Event.cpp | 178 ++++ rmf_task/src/rmf_task/Header.cpp | 83 ++ rmf_task/src/rmf_task/Log.cpp | 397 ++++++++ .../src/rmf_task/{agv => }/Parameters.cpp | 6 +- rmf_task/src/rmf_task/Payload.cpp | 142 +++ rmf_task/src/rmf_task/Phase.cpp | 183 ++++ rmf_task/src/rmf_task/Request.cpp | 41 +- rmf_task/src/rmf_task/State.cpp | 172 ++++ rmf_task/src/rmf_task/Task.cpp | 114 +++ .../src/rmf_task/{agv => }/TaskPlanner.cpp | 83 +- rmf_task/src/rmf_task/VersionedString.cpp | 172 ++++ rmf_task/src/rmf_task/agv/State.cpp | 137 --- rmf_task/src/rmf_task/detail/Backup.cpp | 74 ++ rmf_task/src/rmf_task/detail/Resume.cpp | 76 ++ .../src/rmf_task/detail/internal_Resume.hpp | 63 ++ .../src/rmf_task/events/SimpleEventState.cpp | 145 +++ .../{agv => }/internal_task_planning.cpp | 36 +- .../{agv => }/internal_task_planning.hpp | 24 +- .../src/rmf_task/phases/RestoreBackup.cpp | 125 +++ .../src/rmf_task/requests/ChargeBattery.cpp | 128 +-- .../requests/ChargeBatteryFactory.cpp | 4 +- rmf_task/src/rmf_task/requests/Clean.cpp | 181 ++-- rmf_task/src/rmf_task/requests/Delivery.cpp | 251 ++--- rmf_task/src/rmf_task/requests/Loop.cpp | 208 ++-- .../rmf_task/requests/ParkRobotFactory.cpp | 10 +- rmf_task/test/integration/test_backups.cpp | 216 ++++ rmf_task/test/mock/MockDelivery.cpp | 58 ++ rmf_task/test/mock/MockDelivery.hpp | 91 ++ rmf_task/test/mock/MockEvent.cpp | 73 ++ rmf_task/test/mock/MockEvent.hpp | 56 + rmf_task/test/mock/MockPhase.cpp | 62 ++ rmf_task/test/mock/MockPhase.hpp | 60 ++ rmf_task/test/mock/MockTask.cpp | 173 ++++ rmf_task/test/mock/MockTask.hpp | 99 ++ rmf_task/test/unit/agv/test_Constraints.cpp | 14 +- rmf_task/test/unit/agv/test_State.cpp | 24 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 239 +++-- rmf_task/test/unit/test_Activator.cpp | 85 ++ rmf_task/test/unit/test_Log.cpp | 60 ++ rmf_task_sequence/CHANGELOG.md | 5 + rmf_task_sequence/CMakeLists.txt | 136 +++ rmf_task_sequence/QUALITY_DECLARATION.md | 154 +++ rmf_task_sequence/README.md | 7 + .../cmake/generate_schema_header.cmake | 16 + .../cmake/rmf_task_sequence-config.cmake.in | 15 + .../include/rmf_task_sequence/Activity.hpp | 216 ++++ .../include/rmf_task_sequence/Event.hpp | 303 ++++++ .../include/rmf_task_sequence/Phase.hpp | 176 ++++ .../include/rmf_task_sequence/Task.hpp | 195 ++++ .../rmf_task_sequence/detail/Backup.hpp | 67 ++ .../rmf_task_sequence/detail/impl_Event.hpp | 71 ++ .../rmf_task_sequence/detail/impl_Phase.hpp | 55 + .../rmf_task_sequence/detail/impl_Task.hpp | 66 ++ .../rmf_task_sequence/events/Bundle.hpp | 235 +++++ .../rmf_task_sequence/events/DropOff.hpp | 113 ++ .../rmf_task_sequence/events/GoToPlace.hpp | 78 ++ .../events/PerformAction.hpp | 123 +++ .../rmf_task_sequence/events/PickUp.hpp | 113 ++ .../rmf_task_sequence/events/Placeholder.hpp | 62 ++ .../rmf_task_sequence/events/WaitFor.hpp | 86 ++ .../events/detail/impl_Bundle.hpp | 76 ++ .../rmf_task_sequence/phases/SimplePhase.hpp | 102 ++ .../schemas/ErrorHandler.hpp | 55 + .../include/rmf_task_sequence/typedefs.hpp | 42 + rmf_task_sequence/package.xml | 23 + .../samples/description_Clean.schema.json | 17 + .../samples/description_PickUp.schema.json | 29 + .../backup_EventSequence_v0_1.schema.json | 27 + .../backup_PhaseSequenceTask_v0_1.schema.json | 40 + .../src/rmf_task_sequence/Activity.cpp | 118 +++ .../src/rmf_task_sequence/Event.cpp | 97 ++ .../src/rmf_task_sequence/Phase.cpp | 72 ++ .../src/rmf_task_sequence/Task.cpp | 964 ++++++++++++++++++ .../src/rmf_task_sequence/detail/Backup.cpp | 61 ++ .../src/rmf_task_sequence/events/Bundle.cpp | 427 ++++++++ .../src/rmf_task_sequence/events/DropOff.cpp | 132 +++ .../rmf_task_sequence/events/GoToPlace.cpp | 286 ++++++ .../events/PayloadTransfer.cpp | 64 ++ .../events/PerformAction.cpp | 276 +++++ .../src/rmf_task_sequence/events/PickUp.cpp | 130 +++ .../rmf_task_sequence/events/Placeholder.cpp | 95 ++ .../src/rmf_task_sequence/events/Sequence.cpp | 403 ++++++++ .../src/rmf_task_sequence/events/WaitFor.cpp | 159 +++ .../events/internal_PayloadTransfer.hpp | 63 ++ .../events/internal_Sequence.hpp | 175 ++++ .../src/rmf_task_sequence/events/utils.cpp | 45 + .../src/rmf_task_sequence/events/utils.hpp | 42 + .../rmf_task_sequence/phases/SimplePhase.cpp | 259 +++++ .../schemas/ErrorHandler.cpp | 43 + .../templates/schemas_template.hpp.in | 22 + rmf_task_sequence/test/main.cpp | 21 + rmf_task_sequence/test/mock/MockActivity.cpp | 219 ++++ rmf_task_sequence/test/mock/MockActivity.hpp | 128 +++ .../test/unit/events/test_PerformAction.cpp | 108 ++ rmf_task_sequence/test/unit/test_Sequence.cpp | 423 ++++++++ rmf_task_sequence/test/unit/utils.hpp | 233 +++++ 137 files changed, 14205 insertions(+), 1104 deletions(-) create mode 100644 rmf_task/include/rmf_task/Activator.hpp create mode 100644 rmf_task/include/rmf_task/BackupFileManager.hpp create mode 100644 rmf_task/include/rmf_task/CompositeData.hpp rename rmf_task/include/rmf_task/{agv => }/Constraints.hpp (98%) create mode 100644 rmf_task/include/rmf_task/Event.hpp create mode 100644 rmf_task/include/rmf_task/Header.hpp create mode 100644 rmf_task/include/rmf_task/Log.hpp rename rmf_task/include/rmf_task/{agv => }/Parameters.hpp (95%) create mode 100644 rmf_task/include/rmf_task/Payload.hpp create mode 100644 rmf_task/include/rmf_task/Phase.hpp create mode 100644 rmf_task/include/rmf_task/State.hpp create mode 100644 rmf_task/include/rmf_task/Task.hpp rename rmf_task/include/rmf_task/{agv => }/TaskPlanner.hpp (96%) create mode 100644 rmf_task/include/rmf_task/VersionedString.hpp delete mode 100644 rmf_task/include/rmf_task/agv/State.hpp create mode 100644 rmf_task/include/rmf_task/detail/Backup.hpp create mode 100644 rmf_task/include/rmf_task/detail/Resume.hpp create mode 100644 rmf_task/include/rmf_task/detail/impl_Activator.hpp create mode 100644 rmf_task/include/rmf_task/detail/impl_CompositeData.hpp create mode 100644 rmf_task/include/rmf_task/events/SimpleEventState.hpp create mode 100644 rmf_task/include/rmf_task/phases/RestoreBackup.hpp create mode 100644 rmf_task/src/rmf_task/Activator.cpp create mode 100644 rmf_task/src/rmf_task/BackupFileManager.cpp create mode 100644 rmf_task/src/rmf_task/CompositeData.cpp rename rmf_task/src/rmf_task/{agv => }/Constraints.cpp (97%) create mode 100644 rmf_task/src/rmf_task/Event.cpp create mode 100644 rmf_task/src/rmf_task/Header.cpp create mode 100644 rmf_task/src/rmf_task/Log.cpp rename rmf_task/src/rmf_task/{agv => }/Parameters.cpp (97%) create mode 100644 rmf_task/src/rmf_task/Payload.cpp create mode 100644 rmf_task/src/rmf_task/Phase.cpp create mode 100644 rmf_task/src/rmf_task/State.cpp create mode 100644 rmf_task/src/rmf_task/Task.cpp rename rmf_task/src/rmf_task/{agv => }/TaskPlanner.cpp (93%) create mode 100644 rmf_task/src/rmf_task/VersionedString.cpp delete mode 100644 rmf_task/src/rmf_task/agv/State.cpp create mode 100644 rmf_task/src/rmf_task/detail/Backup.cpp create mode 100644 rmf_task/src/rmf_task/detail/Resume.cpp create mode 100644 rmf_task/src/rmf_task/detail/internal_Resume.hpp create mode 100644 rmf_task/src/rmf_task/events/SimpleEventState.cpp rename rmf_task/src/rmf_task/{agv => }/internal_task_planning.cpp (87%) rename rmf_task/src/rmf_task/{agv => }/internal_task_planning.hpp (90%) create mode 100644 rmf_task/src/rmf_task/phases/RestoreBackup.cpp create mode 100644 rmf_task/test/integration/test_backups.cpp 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/MockEvent.cpp create mode 100644 rmf_task/test/mock/MockEvent.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 create mode 100644 rmf_task/test/unit/test_Log.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/generate_schema_header.cmake create mode 100644 rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in 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/Phase.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/Task.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/detail/impl_Task.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/PickUp.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/Placeholder.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/detail/impl_Bundle.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/phases/SimplePhase.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/schemas/ErrorHandler.hpp create mode 100644 rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp create mode 100644 rmf_task_sequence/package.xml create mode 100644 rmf_task_sequence/samples/description_Clean.schema.json create mode 100644 rmf_task_sequence/samples/description_PickUp.schema.json create mode 100644 rmf_task_sequence/schemas/backup/backup_EventSequence_v0_1.schema.json create mode 100644 rmf_task_sequence/schemas/backup/backup_PhaseSequenceTask_v0_1.schema.json create mode 100644 rmf_task_sequence/src/rmf_task_sequence/Activity.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/Event.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/Phase.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/Task.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/detail/Backup.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/PickUp.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Placeholder.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp 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 create mode 100644 rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/schemas/ErrorHandler.cpp create mode 100644 rmf_task_sequence/templates/schemas_template.hpp.in create mode 100644 rmf_task_sequence/test/main.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/events/test_PerformAction.cpp create mode 100644 rmf_task_sequence/test/unit/test_Sequence.cpp create mode 100644 rmf_task_sequence/test/unit/utils.hpp diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 2496c3f0..5e631361 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -1,5 +1,5 @@ name: build -on: +on: push: pull_request: schedule: @@ -22,7 +22,7 @@ jobs: package-name: | rmf_task vcs-repo-file-url: | - https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos + https://raw.githubusercontent.com/open-rmf/rmf/redesign_v2/rmf.repos colcon-defaults: | { "build": { diff --git a/.github/workflows/tsan.yaml b/.github/workflows/tsan.yaml index 2e06f113..95e26169 100644 --- a/.github/workflows/tsan.yaml +++ b/.github/workflows/tsan.yaml @@ -1,5 +1,5 @@ name: tsan -on: +on: pull_request: schedule: - cron: '31 0 * * *' @@ -23,9 +23,9 @@ jobs: target-ros2-distro: foxy # build all packages listed in the meta package package-name: | - rmf_task + rmf_task vcs-repo-file-url: | - https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos + https://raw.githubusercontent.com/open-rmf/rmf/redesign_v2/rmf.repos colcon-defaults: | { "build": { diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 83022f87..f8b5eb1c 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -29,10 +29,7 @@ find_package(ament_cmake_catch2 QUIET) find_package(ament_cmake_uncrustify QUIET) # ===== RMF Tasks library -file(GLOB lib_srcs - "src/rmf_task/agv/*.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/Activator.hpp b/rmf_task/include/rmf_task/Activator.hpp new file mode 100644 index 00000000..4be605fa --- /dev/null +++ b/rmf_task/include/rmf_task/Activator.hpp @@ -0,0 +1,183 @@ +/* + * 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__ACTIVATOR_HPP +#define RMF_TASK__ACTIVATOR_HPP + +#include + +namespace rmf_task { + +//============================================================================== +/// A factory for generating Task::Active instances from requests. +class Activator +{ +public: + + /// Construct an empty TaskFactory + Activator(); + + /// Signature for activating a task + /// + /// \tparam Description + /// 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 robot + /// + /// \param[in] booking + /// An immutable reference to the booking information for the task + /// + /// \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. + /// + /// \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. + /// + /// \return an active, running instance of the requested task. + template + using Activate = + std::function< + Task::ActivePtr( + const std::function& get_state, + const 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) + >; + + /// Add a callback to convert from a Description 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); + + /// Activate a Task object based on a 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] request + /// The task request + /// + /// \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 + /// + /// \param[in] task_finished + /// A callback that will be triggered when the task has finished + /// + /// \return an active, running instance of the requested task. + Task::ActivePtr activate( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Request& request, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) const; + + /// 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 + /// + /// \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] 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 + /// + /// \param[in] task_finished + /// A callback that will be triggered when the task has finished + /// + /// \return an active, running instance of the requested task. + Task::ActivePtr restore( + const 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) const; + + class Implementation; +private: + + /// \private + void _add_activator( + std::type_index type, + Activate activator); + + rmf_utils::impl_ptr _pimpl; +}; + +using ActivatorPtr = std::shared_ptr; +using ConstActivatorPtr = std::shared_ptr; + +} // namespace rmf_task + +#include + +#endif // RMF_TASK__ACTIVATOR_HPP diff --git a/rmf_task/include/rmf_task/BackupFileManager.hpp b/rmf_task/include/rmf_task/BackupFileManager.hpp new file mode 100644 index 00000000..53ce0597 --- /dev/null +++ b/rmf_task/include/rmf_task/BackupFileManager.hpp @@ -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. + * +*/ + +#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, + 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. + /// + /// \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/include/rmf_task/CompositeData.hpp b/rmf_task/include/rmf_task/CompositeData.hpp new file mode 100644 index 00000000..ac28e1fa --- /dev/null +++ b/rmf_task/include/rmf_task/CompositeData.hpp @@ -0,0 +1,117 @@ +/* + * 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 { + +//============================================================================== +/// 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: + + /// 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); + + /// 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 + 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 + +//============================================================================== +/// 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/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..67d6f206 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -21,8 +21,10 @@ #include #include -#include +#include +#include #include +#include #include namespace rmf_task { @@ -44,13 +46,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; @@ -64,37 +66,48 @@ 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 + /// The result of a travel estimation + 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; + /// How long the travelling will take + 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); + /// How much the battery will drain while travelling + double change_in_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/Event.hpp b/rmf_task/include/rmf_task/Event.hpp new file mode 100644 index 00000000..82a3e608 --- /dev/null +++ b/rmf_task/include/rmf_task/Event.hpp @@ -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. + * +*/ + +#ifndef RMF_TASK__EVENT_HPP +#define RMF_TASK__EVENT_HPP + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rmf_task { + +//============================================================================== +class Event +{ +public: + + /// 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 + /// an operator. + enum class Status : uint32_t + { + /// The event status has not been initialized. This is a sentinel value + /// 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, + + /// The event is underway, and proceeding as expected. + Underway, + + /// The event is underway but it has been temporarily delayed. + Delayed, + + /// An operator has instructed this event to be skipped + Skipped, + + /// An operator has instructed this event to be canceled + Canceled, + + /// An operator has instructed this event to be killed + Killed, + + /// 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; + + class Snapshot; + using ConstSnapshotPtr = std::shared_ptr; + + class AssignID; + using AssignIDPtr = std::shared_ptr; +}; + +//============================================================================== +/// The interface to an active event. +class Event::State +{ +public: + + 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; + + /// 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. + virtual VersionedString::View name() const = 0; + + /// A detailed explanation of this event. + virtual VersionedString::View detail() const = 0; + + /// A view of the log for this event. + virtual Log::View log() const = 0; + + /// Get more granular dependencies of this event, if any exist. + virtual std::vector dependencies() const = 0; + + // Virtual destructor + virtual ~State() = default; +}; + +//============================================================================== +/// 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 Event::Snapshot : public Event::State +{ +public: + + /// 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; + + // Documentation inherited + VersionedString::View name() const final; + + // Documentation inherited + VersionedString::View detail() const final; + + // Documentation inherited + Log::View log() const final; + + // Documentation inherited + std::vector dependencies() const final; + + class Implementation; +private: + Snapshot(); + 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/Header.hpp b/rmf_task/include/rmf_task/Header.hpp new file mode 100644 index 00000000..dbf7f77e --- /dev/null +++ b/rmf_task/include/rmf_task/Header.hpp @@ -0,0 +1,71 @@ +/* + * 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 + +#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; +}; + +//============================================================================== +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/Log.hpp b/rmf_task/include/rmf_task/Log.hpp new file mode 100644 index 00000000..ca6055cd --- /dev/null +++ b/rmf_task/include/rmf_task/Log.hpp @@ -0,0 +1,221 @@ +/* + * 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__LOG_HPP +#define RMF_TASK__LOG_HPP + +#include + +#include +#include +#include + +#include + +namespace rmf_task { + +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; + + /// 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; + + /// 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; + + /// Inequality comparison operator + bool operator!=(const iterator& other) const; + + class Implementation; +private: + iterator(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__LOG_HPP diff --git a/rmf_task/include/rmf_task/agv/Parameters.hpp b/rmf_task/include/rmf_task/Parameters.hpp similarity index 95% rename from rmf_task/include/rmf_task/agv/Parameters.hpp rename to rmf_task/include/rmf_task/Parameters.hpp index 08bf0f06..55b36b29 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 @@ -100,10 +99,11 @@ class Parameters private: rmf_utils::impl_ptr _pimpl; - }; -} // namespace agv +//============================================================================== +using ConstParametersPtr = std::shared_ptr; + } // namespace rmf_task #endif // RMF_TASK__AGV__PARAMETERS_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..084da6af --- /dev/null +++ b/rmf_task/include/rmf_task/Payload.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 RMF_TASK__PAYLOAD_HPP +#define RMF_TASK__PAYLOAD_HPP + +#include + +#include +#include + +namespace rmf_task { + +//============================================================================== +class Payload +{ +public: + + class Component; + + /// Constructor + Payload(std::vector components); + + /// Components in the payload + const std::vector& components() const; + + /// 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; +}; + +//============================================================================== +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; + + /// 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; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__PAYLOAD_HPP diff --git a/rmf_task/include/rmf_task/Phase.hpp b/rmf_task/include/rmf_task/Phase.hpp new file mode 100644 index 00000000..666ca1bf --- /dev/null +++ b/rmf_task/include/rmf_task/Phase.hpp @@ -0,0 +1,174 @@ +/* + * 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__PHASE_HPP +#define RMF_TASK__PHASE_HPP + +#include +#include + +#include + +#include + +#include + +namespace rmf_task { + +//============================================================================== +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; +}; + +//============================================================================== +/// 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: + + using Id = uint64_t; + + /// Constructor + /// + /// \param[in] id_ + /// ID of the phase. This phase ID must be unique within its Task instance. + /// + /// \param[in] header_ + /// Header of the phase. + Tag(Id id_, Header header_); + + /// Unique ID of the phase + Id id() const; + + /// Header of the phase, containing human-friendly high-level information + /// about the phase. + const Header& header() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// Information about a phase that has been completed. +class Phase::Completed +{ +public: + + /// Constructor + Completed( + ConstSnapshotPtr snapshot_, + rmf_traffic::Time start_, + rmf_traffic::Time finish_); + + /// The final log of the phase + const ConstSnapshotPtr& snapshot() const; + + /// The actual time that the phase started + rmf_traffic::Time start_time() const; + + /// The actual time that the phase finished. + rmf_traffic::Time finish_time() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Phase::Active +{ +public: + + /// Tag of the phase + virtual ConstTagPtr tag() const = 0; + + /// The Event that needs to finish for this phase to be complete + virtual Event::ConstStatePtr final_event() const = 0; + + /// The estimated finish time for this phase + virtual rmf_traffic::Duration estimate_remaining_time() const = 0; + + // Virtual destructor + virtual ~Active() = default; +}; + +//============================================================================== +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 + Event::ConstStatePtr final_event() const final; + + // Documentation inherited + rmf_traffic::Duration estimate_remaining_time() const final; + + class Implementation; +private: + Snapshot(); + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Phase::Pending +{ +public: + + /// Constructor + Pending(ConstTagPtr tag); + + /// 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; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__PHASE_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index e59cd55f..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,84 +30,45 @@ namespace rmf_task { class Request { public: - /// 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 agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) 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 agv::Parameters& parameters) const = 0; - - virtual ~Description() = default; - }; - - using DescriptionPtr = std::shared_ptr; - /// Constructor /// /// \param[in] earliest_start_time /// 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 /// /// \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, ConstPriorityPtr priority, - DescriptionPtr description, + Task::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] booking + /// Booking information for this request + /// + /// \param[in] description + /// Description for this request + Request( + Task::ConstBookingPtr booking, + Task::ConstDescriptionPtr description); - /// Get the priority of this request - ConstPriorityPtr priority() const; + /// Get the tag of this request + const Task::ConstBookingPtr& booking() const; /// Get the description of this request - const DescriptionPtr& description() const; - - // Returns true if this request was automatically generated - bool automatic() const; + const Task::ConstDescriptionPtr& description() const; class Implementation; private: @@ -122,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/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/State.hpp b/rmf_task/include/rmf_task/State.hpp new file mode 100644 index 00000000..0770fd20 --- /dev/null +++ b/rmf_task/include/rmf_task/State.hpp @@ -0,0 +1,111 @@ +/* + * 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. + * +*/ + +#ifndef RMF_TASK__STATE_HPP +#define RMF_TASK__STATE_HPP + +#include + +#include + +#include +#include + +#include + +namespace rmf_task { + +//============================================================================== +class State : public CompositeData +{ +public: + + /// The current waypoint of the robot state + 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 + RMF_TASK_DEFINE_COMPONENT(double, CurrentOrientation); + std::optional orientation() const; + State& orientation(double new_orientation); + + /// The current time for the robot + RMF_TASK_DEFINE_COMPONENT(rmf_traffic::Time, CurrentTime); + 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. + 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. + RMF_TASK_DEFINE_COMPONENT(double, CurrentBatterySoC); + std::optional battery_soc() const; + State& battery_soc(double new_battery_soc); + + /// 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); + + /// 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); + + /// 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 + +#endif // RMF_TASK__AGV__STATE_HPP diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp new file mode 100644 index 00000000..d350c5a4 --- /dev/null +++ b/rmf_task/include/rmf_task/Task.hpp @@ -0,0 +1,312 @@ +/* + * 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__TASK_HPP +#define RMF_TASK__TASK_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace rmf_task { + +//============================================================================== +/// Pure abstract interface for an executable Task +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; +}; + +//============================================================================== +/// 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: + + /// Constructor + /// + /// \param[in] id_ + /// The identity of the booking + /// + /// \param[in] earliest_start_time_ + /// The earliest time that the task may begin + /// + /// \param[in] priority_ + /// The priority of the booking + /// + /// \param[in] automatic_ + /// 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 booking + const std::string& id() const; + + /// Get the earliest time that this booking may begin + rmf_traffic::Time earliest_start_time() const; + + /// Get the priority of this booking + ConstPriorityPtr priority() const; + + // Returns true if this booking was automatically generated + bool automatic() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +/// Basic static information about the task. +class Task::Tag +{ +public: + + /// Constructor + Tag( + ConstBookingPtr booking_, + Header header_); + + /// The booking information of the request that this Task is carrying out + const ConstBookingPtr& booking() const; + + /// The header for this Task + const Header& header() 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 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 task's duration + virtual rmf_traffic::Duration invariant_duration() const = 0; + + virtual ~Model() = default; +}; + +//============================================================================== +/// 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 the task based on the unique traits of this + /// description + /// + /// \param[in] earliest_start_time + /// 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 + virtual ConstModelPtr make_model( + 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; +}; + +//============================================================================== +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. + /// + /// 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. + 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; + + /// Interface for the phase that is currently active + virtual Phase::ConstActivePtr active_phase() const = 0; + + /// Time that the current active phase started + virtual std::optional active_phase_start_time() const = 0; + + /// Descriptions of the phases that are expected in the future + virtual const std::vector& pending_phases() const = 0; + + /// The tag of this Task + virtual const ConstTagPtr& tag() const = 0; + + /// Estimate the overall finishing time of the task + virtual rmf_traffic::Duration estimate_remaining_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. + 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. + /// + /// 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 a safe idle state as soon as possible, even if it remains encumbered by + /// something related to this Task. + /// + /// 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. + 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 ~Active() = 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); +}; + +} // namespace rmf_task + +#endif // RMF_TASK__TASK_HPP diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/TaskPlanner.hpp similarity index 96% rename from rmf_task/include/rmf_task/agv/TaskPlanner.hpp rename to rmf_task/include/rmf_task/TaskPlanner.hpp index 5d3120e7..08d6ab05 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 @@ -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 @@ -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: @@ -257,7 +254,6 @@ class TaskPlanner }; -} // namespace agv } // namespace rmf_task #endif // RMF_TASK__AGV__TASKPLANNER_HPP diff --git a/rmf_task/include/rmf_task/VersionedString.hpp b/rmf_task/include/rmf_task/VersionedString.hpp new file mode 100644 index 00000000..337a3aaa --- /dev/null +++ b/rmf_task/include/rmf_task/VersionedString.hpp @@ -0,0 +1,97 @@ +/* + * 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 + +#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/agv/State.hpp b/rmf_task/include/rmf_task/agv/State.hpp deleted file mode 100644 index 957ac793..00000000 --- a/rmf_task/include/rmf_task/agv/State.hpp +++ /dev/null @@ -1,96 +0,0 @@ -/* - * 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. - * -*/ - -#ifndef RMF_TASK__AGV__STATE_HPP -#define RMF_TASK__AGV__STATE_HPP - -#include - -#include - -#include -#include - -namespace rmf_task { -namespace agv { - -//============================================================================== -/// A class that is used to describe the state of an AGV -class State -{ -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 state's location. - rmf_traffic::agv::Plan::Start location() const; - - /// Sets the state's location. - State& location(rmf_traffic::agv::Plan::Start new_location); - - /// Robot's charging waypoint index. - std::size_t charging_waypoint() const; - - /// Sets the charging waypoint index. - State& 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. - double battery_soc() const; - - /// Sets a new battery state of charge value. This value needs to be between - /// 0.0 and 1.0. - State& battery_soc(double new_battery_soc); - - /// The current location waypoint index. - std::size_t waypoint() const; - - /// Sets the current location waypoint index. - State& waypoint(std::size_t new_waypoint); - - /// The time which the robot finishes its current task or when it is ready for - /// a new task. - rmf_traffic::Time finish_time() const; - - /// Sets the finish time for the robot. - State& finish_time(rmf_traffic::Time new_finish_time); - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; -}; - -} // namespace agv -} // namespace rmf_task - -#endif // RMF_TASK__AGV__STATE_HPP 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..5a9ad720 --- /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: + /// Make 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 Activator + /// 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/detail/Resume.hpp b/rmf_task/include/rmf_task/detail/Resume.hpp new file mode 100644 index 00000000..49571581 --- /dev/null +++ b/rmf_task/include/rmf_task/detail/Resume.hpp @@ -0,0 +1,49 @@ +/* + * 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 + +#include + +namespace rmf_task { +namespace detail { + +//============================================================================== +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; + + class Implementation; +private: + Resume(); + 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/detail/impl_Activator.hpp b/rmf_task/include/rmf_task/detail/impl_Activator.hpp new file mode 100644 index 00000000..dd96a41a --- /dev/null +++ b/rmf_task/include/rmf_task/detail/impl_Activator.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__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 ConstParametersPtr& parameters, + 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), + parameters, + 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/include/rmf_task/detail/impl_CompositeData.hpp b/rmf_task/include/rmf_task/detail/impl_CompositeData.hpp new file mode 100644 index 00000000..f8548f3c --- /dev/null +++ b/rmf_task/include/rmf_task/detail/impl_CompositeData.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__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 +CompositeData& CompositeData::with(T&& value) +{ + insert_or_assign(std::forward(value)); + return *this; +} + +//============================================================================== +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/events/SimpleEventState.hpp b/rmf_task/include/rmf_task/events/SimpleEventState.hpp new file mode 100644 index 00000000..dcc74855 --- /dev/null +++ b/rmf_task/include/rmf_task/events/SimpleEventState.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__EVENTS__SIMPLEEVENTSTATE_HPP +#define RMF_TASK__EVENTS__SIMPLEEVENTSTATE_HPP + +#include + +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 SimpleEventState : public Event::State +{ +public: + + static std::shared_ptr make( + uint64_t id, + std::string name, + std::string detail, + Status initial_status, + std::vector dependencies = {}, + std::function clock = nullptr); + + // Documentation inherited + uint64_t id() const final; + + // Documentation inherited + Status status() const final; + + /// Update the status of this event + SimpleEventState& update_status(Status new_status); + + // Documentation inherited + VersionedString::View name() const final; + + /// Update the name of this event + SimpleEventState& update_name(std::string new_name); + + // Documentation inherited + VersionedString::View detail() const final; + + /// Update the detail of this event + SimpleEventState& 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 + SimpleEventState& update_dependencies( + std::vector new_dependencies); + + /// Add one dependency to the state + SimpleEventState& add_dependency(ConstStatePtr new_dependency); + + class Implementation; +private: + SimpleEventState(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +using SimpleEventStatePtr = std::shared_ptr; + +} // namespace events +} // namespace rmf_task + +#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 new file mode 100644 index 00000000..125946a2 --- /dev/null +++ b/rmf_task/include/rmf_task/phases/RestoreBackup.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__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; + +}; + +//============================================================================== +/// 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( + const std::string& backup_state_str, + rmf_traffic::Duration estimated_remaining_time); + + // Documentation inherited + ConstTagPtr tag() const final; + + // Documentation inherited + Event::ConstStatePtr final_event() const final; + + // Documentation inherited + rmf_traffic::Duration estimate_remaining_time() const final; + + /// Call this function if the parsing fails + 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 restoration_succeeded(); + + /// Get the log to pass in some other kind of message + Log& update_log(); + + 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/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 1171dbaf..61fecf17 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 @@ -46,17 +46,22 @@ 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( + // Documentation inherited + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + const Parameters& parameters) const final; + + // Documentation inherited + Info generate_info( + const State& initial_state, + 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..e9f94c84 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 @@ -46,20 +46,25 @@ 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 std::shared_ptr make( std::size_t start_waypoint, std::size_t end_waypoint, const rmf_traffic::Trajectory& cleaning_path); - /// Documentation inherited - std::shared_ptr make_model( + // Documentation inherited + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + 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 10a74da3..200bea28 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -28,9 +28,10 @@ #include #include -#include +#include #include #include +#include namespace rmf_task { namespace requests { @@ -45,36 +46,53 @@ 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); + rmf_traffic::Duration dropoff_duration, + Payload payload, + std::string pickup_from_dispenser = "", + std::string dropoff_to_ingestor = ""); - /// Documentation inherited - std::shared_ptr make_model( + // Documentation inherited + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + 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; + /// 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 +131,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/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 33c74e53..660f3014 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 @@ -47,20 +47,25 @@ 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( + // Documentation inherited + Task::ConstModelPtr make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const final; + 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/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/Activator.cpp b/rmf_task/src/rmf_task/Activator.cpp new file mode 100644 index 00000000..3182cf46 --- /dev/null +++ b/rmf_task/src/rmf_task/Activator.cpp @@ -0,0 +1,112 @@ +/* + * 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( + const std::function& get_state, + const ConstParametersPtr& parameters, + const Request& request, + std::function update, + std::function checkpoint, + std::function phase_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 + // 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( + 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( + const 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) const +{ + 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( + 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)); +} + +//============================================================================== +void Activator::_add_activator( + std::type_index type, + Activate activator) +{ + _pimpl->activators.insert_or_assign(type, std::move(activator)); +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/BackupFileManager.cpp b/rmf_task/src/rmf_task/BackupFileManager.cpp new file mode 100644 index 00000000..9ea310e3 --- /dev/null +++ b/rmf_task/src/rmf_task/BackupFileManager.cpp @@ -0,0 +1,345 @@ +/* + * 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 + +namespace rmf_task { + +//============================================================================== +class BackupFileManager::Implementation +{ +public: + + struct Settings + { + 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, + std::function info, + std::function debug) + : root_directory(std::move(directory)), + settings(std::make_shared()) + { + settings->info_logger = std::move(info); + settings->debug_logger = std::move(debug); + } + + const 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)) + { + std::filesystem::create_directories(this->group_directory); + } + + 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)); + } + + const 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 (this->settings->clear_on_startup) + this->clear_backup(); + + std::filesystem::create_directories(this->robot_directory); + } + + 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(); + } + + 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) + { + 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()); + } + + 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) + { + 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() + { + 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, + std::function info, + std::function debug) +: _pimpl(rmf_utils::make_unique_impl( + std::move(root_directory), + std::move(info), + std::move(debug))) +{ + // 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 +{ + 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; + } + +} + +//============================================================================== +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/src/rmf_task/BinaryPriorityCostCalculator.cpp b/rmf_task/src/rmf_task/BinaryPriorityCostCalculator.cpp index 2a8c542a..c3e523b7 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()->booking()->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()); } } @@ -138,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()->booking()->priority() != nullptr) priority_count[i] += 1; } } @@ -173,7 +175,7 @@ bool BinaryPriorityCostCalculator::valid_assignment_priority( return true; } - auto prev_priority = it->assignment.request()->priority(); + auto prev_priority = it->assignment.request()->booking()->priority(); ++it; for (; it != agent.end(); ++it) { @@ -181,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()->booking()->priority(); if ((prev_priority == nullptr) && (curr_priority != nullptr)) return false; @@ -220,7 +222,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..95ee595e --- /dev/null +++ b/rmf_task/src/rmf_task/CompositeData.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 + +namespace rmf_task { + +//============================================================================== +class CompositeData::Implementation +{ +public: + + std::unordered_map data; + +}; + +//============================================================================== +CompositeData::CompositeData() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +void CompositeData::clear() +{ + _pimpl->data.clear(); +} + +//============================================================================== +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 +{ + if (or_assign) + { + 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}; +} + +//============================================================================== +bool CompositeData::_erase(std::type_index type) +{ + return _pimpl->data.erase(type) > 0; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/Constraints.cpp b/rmf_task/src/rmf_task/Constraints.cpp similarity index 97% rename from rmf_task/src/rmf_task/agv/Constraints.cpp rename to rmf_task/src/rmf_task/Constraints.cpp index 7b0f2452..2d269845 100644 --- a/rmf_task/src/rmf_task/agv/Constraints.cpp +++ b/rmf_task/src/rmf_task/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/CostCalculator.hpp b/rmf_task/src/rmf_task/CostCalculator.hpp index 61e41240..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 @@ -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..781b5efc 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; @@ -70,16 +70,109 @@ 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_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; + } + + auto duration = rmf_traffic::Duration(0); + if (!plan->get_itinerary().empty()) + { + 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 +188,52 @@ 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; + static Cache make_cache( + const std::shared_ptr& planner) + { + const auto N = planner->get_configuration().graph().num_waypoints(); + return Cache(N, PairHash(N)); + } + + mutable std::mutex mutex; }; //============================================================================== -EstimateCache::EstimateCache(std::size_t N) -: _pimpl(rmf_utils::make_unique_impl(N)) +TravelEstimator::TravelEstimator(const Parameters& parameters) +: _pimpl(rmf_utils::make_unique_impl(parameters)) { + // Do nothing } //============================================================================== -std::optional EstimateCache::get( - std::pair waypoints) const +rmf_traffic::Duration TravelEstimator::Result::duration() const { - std::lock_guard guard(_pimpl->_mutex); - auto it = _pimpl->_cache.find(waypoints); - if (it != _pimpl->_cache.end()) - { - return it->second; - } - return std::nullopt; + return _pimpl->duration; +} + +//============================================================================== +double TravelEstimator::Result::change_in_charge() const +{ + return _pimpl->change_in_charge; +} + +//============================================================================== +TravelEstimator::Result::Result() +{ + // Do nothing } //============================================================================== -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/Event.cpp b/rmf_task/src/rmf_task/Event.cpp new file mode 100644 index 00000000..2cb42b8c --- /dev/null +++ b/rmf_task/src/rmf_task/Event.cpp @@ -0,0 +1,178 @@ +/* + * 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_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; + output.reserve(queue.size()); + for (const auto& c : queue) + output.push_back(Event::Snapshot::make(*c)); + + return output; +} +} // 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 +{ +public: + + uint64_t id; + Status status; + VersionedString::View name; + VersionedString::View detail; + Log::View log; + std::vector dependencies; + +}; + +//============================================================================== +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(), + other.log(), + snapshot_dependencies(other.dependencies()) + }); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +uint64_t Event::Snapshot::id() const +{ + return _pimpl->id; +} + +//============================================================================== +auto Event::Snapshot::status() const -> Status +{ + return _pimpl->status; +} + +//============================================================================== +VersionedString::View Event::Snapshot::name() const +{ + return _pimpl->name; +} + +//============================================================================== +VersionedString::View Event::Snapshot::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +Log::View Event::Snapshot::log() const +{ + return _pimpl->log; +} + +//============================================================================== +std::vector Event::Snapshot::dependencies() const +{ + return _pimpl->dependencies; +} + +//============================================================================== +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/Header.cpp b/rmf_task/src/rmf_task/Header.cpp new file mode 100644 index 00000000..be934e5a --- /dev/null +++ b/rmf_task/src/rmf_task/Header.cpp @@ -0,0 +1,83 @@ +/* + * 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; +} + +//============================================================================== +std::string standard_waypoint_name( + const rmf_traffic::agv::Graph& graph, + std::size_t waypoint) +{ + if (waypoint >= graph.num_waypoints()) + { + // *INDENT-OFF* + 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"); + // *INDENT-ON* + } + + return graph.get_waypoint(waypoint).name_or_index( + "[place:%s]", "[graph-wp:%d]"); +} + +} // 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..ac888bbe --- /dev/null +++ b/rmf_task/src/rmf_task/Log.cpp @@ -0,0 +1,397 @@ +/* + * 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; + uint32_t seq = 0; + + Implementation(std::function clock_) + : clock(std::move(clock_)), + entries(std::make_shared>()) + { + if (!clock) + { + clock = []() + { + return rmf_traffic::Time( + rmf_traffic::Duration( + std::chrono::system_clock::now().time_since_epoch())); + }; + } + } + +}; + +//============================================================================== +class Log::Entry::Implementation +{ +public: + + static Entry make( + Tier tier, + uint32_t seq, + rmf_traffic::Time time, + std::string text) + { + Log::Entry output; + output._pimpl = rmf_utils::make_impl( + Implementation{ + tier, + seq, + time, + std::move(text) + }); + + return output; + } + + Tier tier; + uint32_t seq; + rmf_traffic::Time 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 read(const View& view); +}; + +//============================================================================== +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 begin, + std::optional last); +}; + +//============================================================================== +class Log::Reader::Iterable::iterator::Implementation +{ +public: + using base_iterator = std::list::const_iterator; + base_iterator it; + base_iterator last; + + static iterator make(base_iterator it, base_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 begin, + std::optional last) +{ + Iterable iterable; + iterable._pimpl = rmf_utils::make_impl(); + iterable._pimpl->shared = std::move(shared); + if (begin.has_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 + { + iterable._pimpl->begin = iterator::Implementation::end(); + } + + return 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; + auto& memory = it->second; + if (memory.weak.lock()) + { + if (!memory.last.has_value()) + memory.last = v.begin; + else + ++(*memory.last); + } + 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->seq++, _pimpl->clock(), std::move(text))); +} + +//============================================================================== +void Log::warn(std::string text) +{ + _pimpl->entries->emplace_back( + Entry::Implementation::make( + Entry::Tier::Warning, _pimpl->seq++, _pimpl->clock(), std::move(text))); +} + +//============================================================================== +void Log::error(std::string text) +{ + _pimpl->entries->emplace_back( + Entry::Implementation::make( + Entry::Tier::Error, _pimpl->seq++, _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; +} + +//============================================================================== +uint32_t Log::Entry::seq() const +{ + return _pimpl->seq; +} + +//============================================================================== +rmf_traffic::Time 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::read(const View& view) -> Iterable +{ + return _pimpl->read(view); +} + +//============================================================================== +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.get()) + return *this; + + 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.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() +{ + // Do nothing +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/Parameters.cpp b/rmf_task/src/rmf_task/Parameters.cpp similarity index 97% rename from rmf_task/src/rmf_task/agv/Parameters.cpp rename to rmf_task/src/rmf_task/Parameters.cpp index f4678c6d..bfe137cc 100644 --- a/rmf_task/src/rmf_task/agv/Parameters.cpp +++ b/rmf_task/src/rmf_task/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/Payload.cpp b/rmf_task/src/rmf_task/Payload.cpp new file mode 100644 index 00000000..8d031cea --- /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/Phase.cpp b/rmf_task/src/rmf_task/Phase.cpp new file mode 100644 index 00000000..60c2c722 --- /dev/null +++ b/rmf_task/src/rmf_task/Phase.cpp @@ -0,0 +1,183 @@ +/* + * 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 Phase::Tag::Implementation +{ +public: + + Id id; + Header header; + +}; + +//============================================================================== +Phase::Tag::Tag( + Id id_, + Header header_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + id_, + std::move(header_) + })) +{ + // Do nothing +} + +//============================================================================== +auto Phase::Tag::id() const -> Id +{ + return _pimpl->id; +} + +//============================================================================== +const Header& Phase::Tag::header() const +{ + return _pimpl->header; +} + +//============================================================================== +class Phase::Completed::Implementation +{ +public: + + ConstSnapshotPtr snapshot; + rmf_traffic::Time start; + rmf_traffic::Time finish; +}; + +//============================================================================== +Phase::Completed::Completed( + ConstSnapshotPtr snapshot_, + rmf_traffic::Time start_, + rmf_traffic::Time finish_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(snapshot_), + start_, + finish_ + })) +{ + // Do nothing +} + +//============================================================================== +auto Phase::Completed::snapshot() const -> const ConstSnapshotPtr& +{ + return _pimpl->snapshot; +} + +//============================================================================== +rmf_traffic::Time Phase::Completed::start_time() const +{ + return _pimpl->start; +} + +//============================================================================== +rmf_traffic::Time Phase::Completed::finish_time() const +{ + return _pimpl->finish; +} + +//============================================================================== +class Phase::Snapshot::Implementation +{ +public: + ConstTagPtr tag; + Event::ConstStatePtr finish_event; + rmf_traffic::Duration estimated_remaining_time; +}; + +//============================================================================== +Phase::ConstSnapshotPtr Phase::Snapshot::make(const Active& active) +{ + Snapshot output; + output._pimpl = rmf_utils::make_impl( + Implementation{ + active.tag(), + Event::Snapshot::make(*active.final_event()), + active.estimate_remaining_time() + }); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +Phase::ConstTagPtr Phase::Snapshot::tag() const +{ + return _pimpl->tag; +} + +//============================================================================== +Event::ConstStatePtr Phase::Snapshot::final_event() const +{ + return _pimpl->finish_event; +} + +//============================================================================== +rmf_traffic::Duration Phase::Snapshot::estimate_remaining_time() const +{ + return _pimpl->estimated_remaining_time; +} + +//============================================================================== +Phase::Snapshot::Snapshot() +{ + // Do nothing +} + +//============================================================================== +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; +} + +//============================================================================== +auto Phase::Pending::will_be_skipped(bool value) -> Pending& +{ + _pimpl->will_be_skipped = value; + return *this; +} + +//============================================================================== +bool Phase::Pending::will_be_skipped() const +{ + return _pimpl->will_be_skipped; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Request.cpp b/rmf_task/src/rmf_task/Request.cpp index 9530c7c4..9ffc5687 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; - DescriptionPtr description; - bool automatic; + Task::ConstBookingPtr booking; + Task::ConstDescriptionPtr description; }; //============================================================================== @@ -35,48 +32,28 @@ Request::Request( const std::string& id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - DescriptionPtr description, + Task::ConstDescriptionPtr description, 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 } //============================================================================== -const std::string& Request::id() const +const Task::ConstBookingPtr& Request::booking() const { - return _pimpl->id; + return _pimpl->booking; } //============================================================================== -rmf_traffic::Time Request::earliest_start_time() const -{ - return _pimpl->earliest_start_time; -} - -//============================================================================== -ConstPriorityPtr Request::priority() const -{ - return _pimpl->priority; -} - -//============================================================================== -const Request::DescriptionPtr& Request::description() const +const Task::ConstDescriptionPtr& Request::description() const { return _pimpl->description; } -//============================================================================== -bool Request::automatic() const -{ - return _pimpl->automatic; -} - } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/State.cpp b/rmf_task/src/rmf_task/State.cpp new file mode 100644 index 00000000..26bc6337 --- /dev/null +++ b/rmf_task/src/rmf_task/State.cpp @@ -0,0 +1,172 @@ +/* + * 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. + * +*/ + +#include + +#include + +namespace rmf_task { + +//============================================================================== +std::optional State::waypoint() const +{ + if (const auto* wp = get()) + return wp->value; + + return std::nullopt; +} + +//============================================================================== +State& State::waypoint(std::size_t new_waypoint) +{ + with(new_waypoint); + return *this; +} + +//============================================================================== +std::optional State::orientation() const +{ + if (const auto* ori = get()) + return ori->value; + + return std::nullopt; +} + +//============================================================================== +State& State::orientation(double new_orientation) +{ + with(new_orientation); + return *this; +} + +//============================================================================== +std::optional State::time() const +{ + if (const auto* time = get()) + return time->value; + + return std::nullopt; +} + +//============================================================================== +State& State::time(rmf_traffic::Time new_time) +{ + with(new_time); + return *this; +} + +//============================================================================== +std::optional State::dedicated_charging_waypoint() const +{ + if (const auto* p = get()) + return p->value; + + return std::nullopt; +} + +//============================================================================== +State& State::dedicated_charging_waypoint(std::size_t new_charging_waypoint) +{ + with(new_charging_waypoint); + return *this; +} + +//============================================================================== +std::optional State::battery_soc() const +{ + if (const auto* b = get()) + return b->value; + + return std::nullopt; +} + +//============================================================================== +State& State::battery_soc(double new_battery_soc) +{ + if (new_battery_soc < 0.0 || new_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* + } + + with(new_battery_soc); + return *this; +} + +//============================================================================== +State& State::load_basic( + const rmf_traffic::agv::Plan::Start& input_location, + std::size_t input_charging_point, + double input_battery_soc) +{ + load(input_location); + dedicated_charging_waypoint(input_charging_point); + battery_soc(input_battery_soc); + return *this; +} + +//============================================================================== +State& State::load(const rmf_traffic::agv::Plan::Start& location) +{ + with(location.waypoint()); + with(location.orientation()); + with(location.time()); + return *this; +} + +//============================================================================== +std::optional State::project_plan_start( + double default_orientation, + rmf_traffic::Time default_time) const +{ + 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; +} + +//============================================================================== +std::optional State::extract_plan_start() const +{ + 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/Task.cpp b/rmf_task/src/rmf_task/Task.cpp new file mode 100644 index 00000000..0897829c --- /dev/null +++ b/rmf_task/src/rmf_task/Task.cpp @@ -0,0 +1,114 @@ +/* + * 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 "detail/internal_Resume.hpp" + +#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; +} + +//============================================================================== +class Task::Tag::Implementation +{ +public: + ConstBookingPtr booking; + Header header; +}; + +//============================================================================== +Task::Tag::Tag( + ConstBookingPtr booking_, + Header header_) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(booking_), + std::move(header_) + })) +{ + // Do nothing +} + +//============================================================================== +auto Task::Tag::booking() const -> const ConstBookingPtr& +{ + return _pimpl->booking; +} + +//============================================================================== +const Header& Task::Tag::header() const +{ + return _pimpl->header; +} + +//============================================================================== +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 93% rename from rmf_task/src/rmf_task/agv/TaskPlanner.cpp rename to rmf_task/src/rmf_task/TaskPlanner.cpp index 575986f0..e981cc1d 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/TaskPlanner.cpp @@ -16,11 +16,11 @@ */ #include -#include +#include #include #include -#include "../BinaryPriorityCostCalculator.hpp" +#include "BinaryPriorityCostCalculator.hpp" #include @@ -28,7 +28,6 @@ #include namespace rmf_task { -namespace agv { //============================================================================== class TaskPlanner::Configuration::Implementation @@ -196,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; } @@ -373,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; @@ -430,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() @@ -440,10 +439,10 @@ 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); + state, config.constraints(), *travel_estimator); if (estimate.has_value()) { agent.push_back( @@ -459,23 +458,23 @@ 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( - state, config.constraints(), *estimate_cache); + state, config.constraints(), *travel_estimator); 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(), config.constraints(), - *estimate_cache); + *travel_estimator); if (estimate.has_value()) { // Append the ChargeBattery and finishing request @@ -514,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->booking()->priority()) { check_priority = true; break; @@ -576,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( @@ -624,7 +623,7 @@ class TaskPlanner::Implementation config.constraints(), config.parameters(), request, - *estimate_cache, + *travel_estimator, error); if (!pending_task) @@ -647,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; @@ -680,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; } @@ -718,13 +719,13 @@ 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(), + charge_battery->booking()->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( @@ -754,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()) { @@ -795,13 +796,15 @@ 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(), + charge_battery->booking()->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( @@ -817,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( @@ -878,16 +881,16 @@ 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(), + charge_battery->booking()->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( @@ -906,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( @@ -1088,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 @@ -1136,17 +1137,10 @@ 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; } // ============================================================================ -const rmf_task::agv::TaskPlanner::Configuration& TaskPlanner::configuration() +const rmf_task::TaskPlanner::Configuration& TaskPlanner::configuration() const { return _pimpl->config; @@ -1164,5 +1158,4 @@ auto TaskPlanner::default_options() -> Options& return _pimpl->default_options; } -} // namespace agv } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/VersionedString.cpp b/rmf_task/src/rmf_task/VersionedString.cpp new file mode 100644 index 00000000..b6dd6d03 --- /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/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp deleted file mode 100644 index 6a4ae0ae..00000000 --- a/rmf_task/src/rmf_task/agv/State.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* - * 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. - * -*/ - -#include - -#include - -namespace rmf_task { -namespace agv { - -//============================================================================== -class State::Implementation -{ -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* - } - } - - rmf_traffic::agv::Plan::Start _location; - std::size_t _charging_waypoint; - double _battery_soc; -}; - -//============================================================================== -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))) -{ -} - -//============================================================================== -rmf_traffic::agv::Plan::Start State::location() const -{ - return _pimpl->_location; -} - -//============================================================================== -State& State::location(rmf_traffic::agv::Plan::Start new_location) -{ - _pimpl->_location = std::move(new_location); - return *this; -} - -//============================================================================== -std::size_t State::charging_waypoint() const -{ - return _pimpl->_charging_waypoint; -} - -//============================================================================== -State& State::charging_waypoint(std::size_t new_charging_waypoint) -{ - _pimpl->_charging_waypoint = new_charging_waypoint; - return *this; -} - -//============================================================================== -double State::battery_soc() const -{ - return _pimpl->_battery_soc; -} - -//============================================================================== -State& State::battery_soc(double new_battery_soc) -{ - if (new_battery_soc < 0.0 || new_battery_soc > 1.0) - { - // *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."); - // *INDENT-ON* - } - - _pimpl->_battery_soc = new_battery_soc; - return *this; -} - -//============================================================================== -std::size_t State::waypoint() const -{ - return _pimpl->_location.waypoint(); -} - -//============================================================================== -State& State::waypoint(std::size_t new_waypoint) -{ - _pimpl->_location.waypoint(new_waypoint); - return *this; -} - -//============================================================================== -rmf_traffic::Time State::finish_time() const -{ - return _pimpl->_location.time(); -} - -//============================================================================== -State& State::finish_time(rmf_traffic::Time new_finish_time) -{ - _pimpl->_location.time(new_finish_time); - return *this; -} - -//============================================================================== -} // namespace agv -} // namespace rmf_task 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/src/rmf_task/detail/Resume.cpp b/rmf_task/src/rmf_task/detail/Resume.cpp new file mode 100644 index 00000000..9086dff8 --- /dev/null +++ b/rmf_task/src/rmf_task/detail/Resume.cpp @@ -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. + * +*/ + +#include "internal_Resume.hpp" + +namespace rmf_task { +namespace detail { + +//============================================================================== +Resume Resume::make(std::function callback) +{ + return Implementation::make(std::move(callback)); +} + +//============================================================================== +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/events/SimpleEventState.cpp b/rmf_task/src/rmf_task/events/SimpleEventState.cpp new file mode 100644 index 00000000..f5ce6036 --- /dev/null +++ b/rmf_task/src/rmf_task/events/SimpleEventState.cpp @@ -0,0 +1,145 @@ +/* + * 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 SimpleEventState::Implementation +{ +public: + + uint64_t id; + Status status; + VersionedString name; + VersionedString detail; + Log log; + std::vector dependencies; + +}; + +//============================================================================== +std::shared_ptr SimpleEventState::make( + uint64_t id, + std::string name, + std::string detail, + Event::Status initial_status, + std::vector dependencies, + std::function clock) +{ + SimpleEventState output; + output._pimpl = rmf_utils::make_unique_impl( + Implementation{ + id, + initial_status, + std::move(name), + std::move(detail), + Log(std::move(clock)), + std::move(dependencies) + }); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +uint64_t SimpleEventState::id() const +{ + return _pimpl->id; +} + +//============================================================================== +Event::Status SimpleEventState::status() const +{ + return _pimpl->status; +} + +//============================================================================== +SimpleEventState& SimpleEventState::update_status(Event::Status new_status) +{ + _pimpl->status = new_status; + return *this; +} + +//============================================================================== +VersionedString::View SimpleEventState::name() const +{ + return _pimpl->name.view(); +} + +//============================================================================== +SimpleEventState& SimpleEventState::update_name(std::string new_name) +{ + _pimpl->name.update(std::move(new_name)); + return *this; +} + +//============================================================================== +VersionedString::View SimpleEventState::detail() const +{ + return _pimpl->detail.view(); +} + +//============================================================================== +SimpleEventState& SimpleEventState::update_detail(std::string new_detail) +{ + _pimpl->detail.update(std::move(new_detail)); + return *this; +} + +//============================================================================== +Log::View SimpleEventState::log() const +{ + return _pimpl->log.view(); +} + +//============================================================================== +Log& SimpleEventState::update_log() +{ + return _pimpl->log; +} + +//============================================================================== +std::vector SimpleEventState::dependencies() const +{ + return _pimpl->dependencies; +} + +//============================================================================== +SimpleEventState& SimpleEventState::update_dependencies( + std::vector new_dependencies) +{ + _pimpl->dependencies = new_dependencies; + return *this; +} + +//============================================================================== +SimpleEventState& SimpleEventState::add_dependency(ConstStatePtr new_dependency) +{ + _pimpl->dependencies.push_back(new_dependency); + return *this; +} + +//============================================================================== +SimpleEventState::SimpleEventState() +{ + // Do nothing +} + +} // namespace events +} // namespace rmf_task 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 87% rename from rmf_task/src/rmf_task/agv/internal_task_planning.cpp rename to rmf_task/src/rmf_task/internal_task_planning.cpp index fda81c3d..f4e0cb4d 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.cpp +++ b/rmf_task/src/rmf_task/internal_task_planning.cpp @@ -19,7 +19,6 @@ #include namespace rmf_task { -namespace agv { // ============================================================================ void Candidates::update_map() @@ -53,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} }); } @@ -101,20 +100,20 @@ std::shared_ptr Candidates::make( const std::vector& initial_states, const Constraints& constraints, const Parameters& parameters, - const Request::Model& request_model, - EstimateCache& estimate_cache, + const Task::Model& task_model, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) { const auto& state = initial_states[i]; - const auto finish = request_model.estimate_finish( - state, constraints, estimate_cache); + const auto finish = task_model.estimate_finish( + state, constraints, travel_estimator); 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(), @@ -131,17 +130,17 @@ 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( + auto new_finish = task_model.estimate_finish( battery_estimate.value().finish_state(), constraints, - estimate_cache); + travel_estimator); 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(), @@ -179,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_)), @@ -193,22 +191,21 @@ 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_, - EstimateCache& estimate_cache, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error) { const auto earliest_start_time = std::max( start_time, - request_->earliest_start_time()); + request_->booking()->earliest_start_time()); const auto model = request_->description()->make_model( 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; @@ -218,5 +215,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/internal_task_planning.hpp similarity index 90% rename from rmf_task/src/rmf_task/agv/internal_task_planning.hpp rename to rmf_task/src/rmf_task/internal_task_planning.hpp index ad89ccd8..b4b239b6 100644 --- a/rmf_task/src/rmf_task/agv/internal_task_planning.hpp +++ b/rmf_task/src/rmf_task/internal_task_planning.hpp @@ -15,10 +15,10 @@ * */ -#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 +#include #include #include @@ -27,7 +27,6 @@ #include namespace rmf_task { -namespace agv { // ============================================================================ struct Invariant @@ -76,8 +75,8 @@ class Candidates const std::vector& initial_states, const Constraints& constraints, const Parameters& parameters, - const Request::Model& request_model, - EstimateCache& estimate_cache, + const Task::Model& task_model, + const TravelEstimator& travel_estimator, TaskPlanner::TaskPlannerError& error); Candidates(const Candidates& other); @@ -113,21 +112,21 @@ 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_, - EstimateCache& estimate_cache, + const TravelEstimator& travel_estimator, 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_); }; @@ -164,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->booking()->earliest_start_time().time_since_epoch()); const auto invariant_duration = u.second.model->invariant_duration(); double earliest_finish_time = earliest_start_time @@ -212,7 +211,6 @@ struct LowestCostEstimate } }; -} // namespace agv } // namespace rmf_task -#endif // SRC__RMF_TASK__AGV__INTERNAL_TASK_PLANNING_HPP +#endif // SRC__RMF_TASK__INTERNAL_TASK_PLANNING_HPP 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..7088bfe4 --- /dev/null +++ b/rmf_task/src/rmf_task/phases/RestoreBackup.cpp @@ -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. + * +*/ + +#include +#include + +namespace rmf_task { +namespace phases { + +//============================================================================== +class RestoreBackup::Active::Implementation +{ +public: + + static ConstTagPtr make_tag() + { + return std::make_shared( + 0, + Header( + "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::Duration estimated_remaining_time_) + : tag(make_tag()), + event(events::SimpleEventState::make( + 0, + tag->header().category(), + tag->header().detail(), + rmf_task::Event::Status::Underway)), + 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::Duration estimated_remaining_time; + +}; + +//============================================================================== +auto RestoreBackup::Active::make( + const std::string& backup_state_str, + rmf_traffic::Duration estimated_remaining_time) -> ActivePtr +{ + Active output; + output._pimpl = rmf_utils::make_unique_impl( + std::move(backup_state_str), estimated_remaining_time); + + return std::make_shared(std::move(output)); +} + +//============================================================================== +auto RestoreBackup::Active::tag() const -> ConstTagPtr +{ + return _pimpl->tag; +} + +//============================================================================== +Event::ConstStatePtr RestoreBackup::Active::final_event() const +{ + return _pimpl->event; +} + +//============================================================================== +rmf_traffic::Duration RestoreBackup::Active::estimate_remaining_time() const +{ + return _pimpl->estimated_remaining_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::Completed); +} + +//============================================================================== +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/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ac5b1ed3..32c5364f 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -47,31 +47,31 @@ 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: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) 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) { @@ -80,84 +80,51 @@ ChargeBattery::Model::Model( //============================================================================== std::optional -ChargeBattery::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const +ChargeBattery::Model::estimate_finish(const State& initial_state, + const Constraints& task_planning_constraints, + 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() == 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()}; - agv::State state{ - std::move(final_plan_start), - initial_state.charging_waypoint(), - initial_state.battery_soc()}; + initial_state.time().value(), + initial_state.dedicated_charging_waypoint().value(), + initial_state.orientation().value()}; - const auto start_time = initial_state.finish_time(); + auto state = State().load_basic( + std::move(final_plan_start), + initial_state.dedicated_charging_waypoint().value(), + initial_state.battery_soc().value()); - double battery_soc = initial_state.battery_soc(); - double dSOC_motion = 0.0; - double dSOC_device = 0.0; - double variant_battery_drain = 0.0; + double battery_soc = initial_state.battery_soc().value(); 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.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& 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.location(), 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(); + 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()) @@ -176,8 +143,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)); @@ -199,7 +166,7 @@ class ChargeBattery::Description::Implementation }; //============================================================================== -rmf_task::DescriptionPtr ChargeBattery::Description::make() +Task::ConstDescriptionPtr ChargeBattery::Description::make() { std::shared_ptr description( new Description()); @@ -214,15 +181,26 @@ ChargeBattery::Description::Description() } //============================================================================== -std::shared_ptr ChargeBattery::Description::make_model( +Task::ConstModelPtr 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, 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/ChargeBatteryFactory.cpp b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp index b3219fc6..9bccbc44 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp @@ -36,9 +36,9 @@ ChargeBatteryFactory::ChargeBatteryFactory() //============================================================================== ConstRequestPtr ChargeBatteryFactory::make_request( - const agv::State& state) const + 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 386b668b..8d995cf4 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -23,27 +23,27 @@ namespace rmf_task { namespace requests { //============================================================================== -class Clean::Model : public Request::Model +class Clean::Model : public Task::Model { public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& 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,76 +85,39 @@ Clean::Model::Model( //============================================================================== std::optional Clean::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) const { rmf_traffic::agv::Plan::Start final_plan_start{ - initial_state.finish_time(), + initial_state.time().value(), _end_waypoint, - initial_state.location().orientation()}; - agv::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(); - double dSOC_motion = 0.0; - double dSOC_ambient = 0.0; + double battery_soc = initial_state.battery_soc().value(); 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(), - _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.location(), 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); - } + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), + _start_waypoint); + + 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; @@ -169,17 +132,18 @@ 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()); - dSOC_ambient = ambient_sink.compute_change_in_charge( + wait_until - initial_state.time().value()); + const double dSOC_ambient = ambient_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_ambient; @@ -190,7 +154,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) @@ -200,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.charging_waypoint()) + if (_end_waypoint != state.dedicated_charging_waypoint().value()) { - const auto endpoints = std::make_pair(_end_waypoint, - state.charging_waypoint()); - 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.finish_time(), - 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.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_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); } @@ -273,7 +205,7 @@ class Clean::Description::Implementation }; //============================================================================== -rmf_task::DescriptionPtr 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) @@ -294,9 +226,9 @@ Clean::Description::Description() } //============================================================================== -std::shared_ptr Clean::Description::make_model( +Task::ConstModelPtr Clean::Description::make_model( rmf_traffic::Time earliest_start_time, - const agv::Parameters& parameters) const + const Parameters& parameters) const { if (parameters.tool_sink() == nullptr) { @@ -314,6 +246,19 @@ std::shared_ptr 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 5a7734ab..78644bce 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -23,20 +23,20 @@ namespace rmf_task { namespace requests { //============================================================================== -class Delivery::Model : public Request::Model +class Delivery::Model : public Task::Model { public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) 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,11 +44,9 @@ 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; - rmf_traffic::Duration _dropoff_wait; rmf_traffic::Duration _invariant_duration; double _invariant_battery_drain; @@ -57,7 +55,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,161 +105,91 @@ Delivery::Model::Model( //============================================================================== std::optional Delivery::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) const { rmf_traffic::agv::Plan::Start final_plan_start{ - initial_state.finish_time(), + initial_state.time().value(), _dropoff_waypoint, - initial_state.location().orientation()}; - agv::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(); - double dSOC_motion = 0.0; - double dSOC_device = 0.0; + double battery_soc = initial_state.battery_soc().value(); 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(), - _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.location(), 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); - } + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), + _pickup_waypoint); - if (battery_soc <= task_planning_constraints.threshold_soc()) + if (!travel) + return std::nullopt; + + variant_duration = travel->duration(); + if (drain_battery) + battery_soc = battery_soc - travel->change_in_charge(); + + if (battery_soc <= battery_threshold) return std::nullopt; } 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()); - dSOC_device = ambient_sink.compute_change_in_charge( + const rmf_traffic::Duration wait_duration( + wait_until - initial_state.time().value()); + + 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 - state.finish_time( - wait_until + variant_duration + _invariant_duration); + state.time(wait_until + variant_duration + _invariant_duration); 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.charging_waypoint()) + if (_dropoff_waypoint != state.dedicated_charging_waypoint().value()) { - const auto endpoints = std::make_pair(_dropoff_waypoint, - state.charging_waypoint()); - 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.finish_time(), - 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.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); - } - } + 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); } @@ -280,42 +208,50 @@ 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; }; //============================================================================== -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, - 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 } //============================================================================== -std::shared_ptr Delivery::Description::make_model( +Task::ConstModelPtr 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, @@ -326,12 +262,37 @@ std::shared_ptr 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 { 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 { @@ -350,22 +311,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/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index b13978c5..49331ef2 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -21,27 +21,27 @@ namespace rmf_task { namespace requests { //============================================================================== -class Loop::Model : public Request::Model +class Loop::Model : public Task::Model { public: std::optional estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const final; + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) 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) @@ -100,173 +100,104 @@ Loop::Model::Model( _invariant_battery_drain = (2 * num_loops - 1) * forward_battery_drain; } - } //============================================================================== std::optional Loop::Model::estimate_finish( - const agv::State& initial_state, - const agv::Constraints& task_planning_constraints, - EstimateCache& estimate_cache) const + const State& initial_state, + const Constraints& task_planning_constraints, + const TravelEstimator& travel_estimator) const { rmf_traffic::Duration variant_duration(0); - const rmf_traffic::Time start_time = initial_state.finish_time(); - double battery_soc = initial_state.battery_soc(); - double dSOC_motion = 0.0; - double dSOC_device = 0.0; + double battery_soc = initial_state.battery_soc().value(); 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(), - _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.location(), 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); - } + const auto travel = travel_estimator.estimate( + initial_state.extract_plan_start().value(), + _start_waypoint); + + 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()) + if (battery_soc <= battery_threshold) return std::nullopt; } // 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()); - dSOC_device = ambient_sink.compute_change_in_charge( + wait_until - initial_state.time().value()); + + 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.charging_waypoint()) - { - const auto endpoints = std::make_pair(_finish_waypoint, - initial_state.charging_waypoint()); - 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 rmf_traffic::agv::Planner::Start location{ state_finish_time, _finish_waypoint, - initial_state.location().orientation()}; - agv::State state{ + initial_state.orientation().value()}; + + auto finish_state = State().load_basic( std::move(location), - initial_state.charging_waypoint(), - battery_soc}; + initial_state.dedicated_charging_waypoint().value(), + battery_soc); + + // 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; - return Estimate(state, wait_until); + 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); } //============================================================================== @@ -289,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) @@ -310,9 +241,9 @@ Loop::Description::Description() } //============================================================================== -std::shared_ptr Loop::Description::make_model( +Task::ConstModelPtr 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, @@ -322,6 +253,19 @@ std::shared_ptr 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/src/rmf_task/requests/ParkRobotFactory.cpp b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp index 609eb278..e10281e3 100644 --- a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp @@ -65,18 +65,20 @@ 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(); + 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/integration/test_backups.cpp b/rmf_task/test/integration/test_backups.cpp new file mode 100644 index 00000000..a9742388 --- /dev/null +++ b/rmf_task/test/integration/test_backups.cpp @@ -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. + * +*/ + +#include +#include +#include + +#include "../mock/MockDelivery.hpp" + +#include + +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"); + + 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); + + 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()); + + 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->_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 + mock_restored_task->_active_phase->send_update(); + REQUIRE(restored_snapshot); + CHECK(restored_snapshot->tag()->id() == phase_snapshot->tag()->id()); + 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/MockDelivery.cpp b/rmf_task/test/mock/MockDelivery.cpp new file mode 100644 index 00000000..6248b1b4 --- /dev/null +++ b/rmf_task/test/mock/MockDelivery.cpp @@ -0,0 +1,58 @@ +/* + * 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" +#include + +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(backup_state), + std::move(get_state), + parameters, + booking, + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished))); + }; +} + +auto MockDelivery::Active::backup() const -> 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 new file mode 100644 index 00000000..8fd66ea0 --- /dev/null +++ b/rmf_task/test/mock/MockDelivery.hpp @@ -0,0 +1,91 @@ +/* + * 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, + std::optional backup, + Args&& ... args) + : MockTask::Active(std::forward(args)...), + _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 + 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); + + 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; + + Description _description; + std::optional _restored_state; + mutable uint64_t _backup_seq = 0; + }; + +}; + +} // namespace test_rmf_task + +#endif // TEST__MOCK__MOCKDELIVERY_HPP diff --git a/rmf_task/test/mock/MockEvent.cpp b/rmf_task/test/mock/MockEvent.cpp new file mode 100644 index 00000000..81a1bfd9 --- /dev/null +++ b/rmf_task/test/mock/MockEvent.cpp @@ -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. + * +*/ + +#include "MockEvent.hpp" + +namespace test_rmf_task { + +//============================================================================== +MockEvent::MockEvent( + uint64_t id_, + std::string name_, + std::string detail_, + 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 +{ + return _status; +} + +//============================================================================== +rmf_task::VersionedString::View MockEvent::name() const +{ + return _name.view(); +} + +//============================================================================== +rmf_task::VersionedString::View MockEvent::detail() const +{ + return _detail.view(); +} + +//============================================================================== +rmf_task::Log::View MockEvent::log() const +{ + return _log.view(); +} + +//============================================================================== +std::vector MockEvent::dependencies() const +{ + return std::vector( + _dependencies.begin(), _dependencies.end()); +} + +} // namespace test_rmf_task diff --git a/rmf_task/test/mock/MockEvent.hpp b/rmf_task/test/mock/MockEvent.hpp new file mode 100644 index 00000000..c43f9418 --- /dev/null +++ b/rmf_task/test/mock/MockEvent.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 TEST__MOCK__MOCKEVENT_HPP +#define TEST__MOCK__MOCKEVENT_HPP + +#include + +namespace test_rmf_task { + +//============================================================================== +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; + rmf_task::Log::View log() const final; + std::vector dependencies() const final; + + // Fields + uint64_t _id; + Status _status; + rmf_task::VersionedString _name; + rmf_task::VersionedString _detail; + rmf_task::Log _log; + std::vector> _dependencies; + +}; + +} // namespace test_rmf_task + +#endif // TEST__MOCK__MOCKCONDITION_HPP diff --git a/rmf_task/test/mock/MockPhase.cpp b/rmf_task/test/mock/MockPhase.cpp new file mode 100644 index 00000000..fe8a73f2 --- /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_)), + _event(std::make_shared( + 0, "Mock Event", "This is a mocked up event")), + _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::Event::ConstStatePtr MockPhase::Active::final_event() const +{ + return _event; +} + +//============================================================================== +rmf_traffic::Duration MockPhase::Active::estimate_remaining_time() const +{ + return _tag->header().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..2f87baf0 --- /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 "MockEvent.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::Event::ConstStatePtr final_event() const final; + rmf_traffic::Duration estimate_remaining_time() const final; + + void send_update() const; + + ConstTagPtr _tag; + std::shared_ptr _event; + 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..5a32d9c0 --- /dev/null +++ b/rmf_task/test/mock/MockTask.cpp @@ -0,0 +1,173 @@ +/* + * 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 { + +//============================================================================== +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& +{ + return _completed_phases; +} + +//============================================================================== +auto MockTask::Active::active_phase() const -> Phase::ConstActivePtr +{ + return _active_phase; +} + +//============================================================================== +std::optional +MockTask::Active::active_phase_start_time() const +{ + return std::nullopt; +} + +//============================================================================== +auto MockTask::Active::pending_phases() const +-> const std::vector& +{ + return _pending_phases; +} + +//============================================================================== +auto MockTask::Active::tag() const -> const ConstTagPtr& +{ + return _tag; +} + +//============================================================================== +rmf_traffic::Duration MockTask::Active::estimate_remaining_time() const +{ + return _tag->header().original_duration_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::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) +: _tag(std::make_shared( + 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)), + _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++, + rmf_task::Header(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( + rmf_task::Phase::Snapshot::make(*_active_phase), + _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)); }, + []() {}); +} + +//============================================================================== +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 new file mode 100644 index 00000000..3fc56f30 --- /dev/null +++ b/rmf_task/test/mock/MockTask.hpp @@ -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. + * +*/ + +#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: + + rmf_task::Event::Status status_overview() const override; + + const std::vector& + completed_phases() const override; + + Phase::ConstActivePtr active_phase() const override; + + std::optional active_phase_start_time() const override; + + const std::vector& pending_phases() const override; + + const ConstTagPtr& tag() const override; + + rmf_traffic::Duration estimate_remaining_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::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); + + void issue_backup(); + + ConstTagPtr _tag; + 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/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..f5684256 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,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::agv::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::agv::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::agv::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::agv::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::agv::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 14f9354e..cdce6bbd 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, @@ -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,19 @@ 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(); + 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.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() + << " <" << a.request()->booking()->id() << ": " + << request_seconds << ", " << start_seconds + << ", "<< finish_seconds << ", " << 100*s.battery_soc().value() << "%>" << std::endl; } } @@ -201,8 +207,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 +241,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().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 2, 1.0) }; std::vector requests = @@ -248,6 +254,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -256,6 +263,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -264,6 +272,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)) }; @@ -318,10 +327,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().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 2, 1.0) }; std::vector requests = @@ -331,6 +340,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -339,6 +349,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -347,6 +358,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -355,6 +367,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)), @@ -363,6 +376,7 @@ SCENARIO("Grid World") delivery_wait, 0, delivery_wait, + {{}}, "5", now + rmf_traffic::time::from_seconds(50000)), @@ -371,6 +385,7 @@ SCENARIO("Grid World") delivery_wait, 8, delivery_wait, + {{}}, "6", now + rmf_traffic::time::from_seconds(60000)), @@ -379,6 +394,7 @@ SCENARIO("Grid World") delivery_wait, 14, delivery_wait, + {{}}, "7", now + rmf_traffic::time::from_seconds(60000)), @@ -387,6 +403,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "8", now + rmf_traffic::time::from_seconds(60000)), @@ -395,6 +412,7 @@ SCENARIO("Grid World") delivery_wait, 0, delivery_wait, + {{}}, "9", now + rmf_traffic::time::from_seconds(60000)), @@ -403,6 +421,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "10", now + rmf_traffic::time::from_seconds(60000)), @@ -411,6 +430,7 @@ SCENARIO("Grid World") delivery_wait, 12, delivery_wait, + {{}}, "11", now + rmf_traffic::time::from_seconds(60000)) }; @@ -465,10 +485,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().load_basic(first_location, 13, initial_soc), + rmf_task::State().load_basic(second_location, 2, initial_soc) }; std::vector requests = @@ -478,6 +498,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -486,6 +507,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -494,6 +516,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -502,6 +525,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)) }; @@ -565,10 +589,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().load_basic(first_location, 9, 1.0), + rmf_task::State().load_basic(second_location, 2, 1.0) }; std::vector requests = @@ -578,6 +602,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -586,6 +611,7 @@ SCENARIO("Grid World") delivery_wait, 7, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -594,6 +620,7 @@ SCENARIO("Grid World") delivery_wait, 12, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -602,6 +629,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)), @@ -610,6 +638,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "5", now + rmf_traffic::time::from_seconds(50000)), @@ -618,6 +647,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "6", now + rmf_traffic::time::from_seconds(70000)), @@ -626,6 +656,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "7", now + rmf_traffic::time::from_seconds(70000)), @@ -634,6 +665,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "8", now + rmf_traffic::time::from_seconds(70000)), @@ -642,6 +674,7 @@ SCENARIO("Grid World") delivery_wait, 1, delivery_wait, + {{}}, "9", now + rmf_traffic::time::from_seconds(70000)), @@ -650,6 +683,7 @@ SCENARIO("Grid World") delivery_wait, 5, delivery_wait, + {{}}, "10", now + rmf_traffic::time::from_seconds(70000)), @@ -658,6 +692,7 @@ SCENARIO("Grid World") delivery_wait, 10, delivery_wait, + {{}}, "11", now + rmf_traffic::time::from_seconds(70000)) }; @@ -710,9 +745,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().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -756,9 +791,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().load_basic(first_location, 13, 0.0) }; std::vector requests = @@ -803,9 +838,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().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -815,6 +850,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -823,6 +859,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -831,6 +868,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)) }; @@ -856,7 +894,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()->booking()->id() == "3"); THEN("When replanning with high priority for request with task_id:3") { @@ -865,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()); @@ -892,7 +931,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()->id() == "3"); + CHECK(new_optimal_assignments.front().front().request()->booking()->id() == + "3"); } WHEN("Planning for one robot, three high priority tasks") @@ -902,9 +942,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().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -914,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()), @@ -923,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()), @@ -932,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()) @@ -958,7 +1001,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()->booking()->id() == "3"); } WHEN("Planning for 1 robot, two high priority and two low priority tasks") @@ -968,9 +1011,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().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -980,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()), @@ -989,6 +1033,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -997,6 +1042,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1005,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()) @@ -1035,7 +1082,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()->booking()->id(), i}); CHECK(index_map["1"] < index_map["2"]); CHECK(index_map["1"] < index_map["3"]); CHECK(index_map["4"] < index_map["2"]); @@ -1050,9 +1097,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().load_basic(first_location, 13, 1.0) }; std::vector requests = @@ -1062,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()), @@ -1071,6 +1119,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1079,6 +1128,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(100000)), @@ -1087,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()) @@ -1117,7 +1168,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()->booking()->id(), i}); CHECK(index_map["1"] < index_map["2"]); CHECK(index_map["1"] < index_map["3"]); CHECK(index_map["1"] < index_map["4"]); @@ -1133,10 +1184,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().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0) }; std::vector requests = @@ -1146,6 +1197,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1154,6 +1206,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1162,6 +1215,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1170,6 +1224,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0)) }; @@ -1198,8 +1253,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()->booking()->id() == "2"); + CHECK(agent_1_assignments.front().request()->booking()->id() == "1"); THEN("When task 3 & 4 are assigned high priority") { @@ -1210,6 +1265,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1218,6 +1274,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1226,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()), @@ -1235,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()) @@ -1263,8 +1322,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()->booking()->id() == "4"); + CHECK(agent_1_assignments.front().request()->booking()->id() == "3"); } } @@ -1277,11 +1336,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().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 = @@ -1291,6 +1350,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1299,6 +1359,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1307,6 +1368,7 @@ SCENARIO("Grid World") delivery_wait, 9, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1315,6 +1377,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0)) }; @@ -1344,7 +1407,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()->booking()->id()); } std::size_t id_count = 0; for (const auto& id : first_assignments) @@ -1363,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()), @@ -1372,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()), @@ -1381,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()), @@ -1390,6 +1456,7 @@ SCENARIO("Grid World") delivery_wait, 6, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(0)) }; @@ -1418,7 +1485,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()->booking()->id()); } std::size_t id_count = 0; for (const auto& id : first_assignments) @@ -1436,9 +1503,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 +1513,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().load_basic(first_location, 13, initial_soc), + rmf_task::State().load_basic(second_location, 2, initial_soc) }; std::vector requests = @@ -1459,6 +1526,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1467,6 +1535,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1475,6 +1544,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1483,6 +1553,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)) }; @@ -1535,7 +1606,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); } } } @@ -1549,9 +1620,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().load_basic(first_location, 13, 1.0) }; const auto start_time = @@ -1606,18 +1677,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().load_basic(first_location, 13, initial_soc) }; std::vector requests = @@ -1627,6 +1698,7 @@ SCENARIO("Grid World") delivery_wait, 3, delivery_wait, + {{}}, "1", now + rmf_traffic::time::from_seconds(0)), @@ -1635,6 +1707,7 @@ SCENARIO("Grid World") delivery_wait, 2, delivery_wait, + {{}}, "2", now + rmf_traffic::time::from_seconds(0)), @@ -1643,6 +1716,7 @@ SCENARIO("Grid World") delivery_wait, 4, delivery_wait, + {{}}, "3", now + rmf_traffic::time::from_seconds(0)), @@ -1651,6 +1725,7 @@ SCENARIO("Grid World") delivery_wait, 11, delivery_wait, + {{}}, "4", now + rmf_traffic::time::from_seconds(50000)) }; @@ -1690,10 +1765,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().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0) }; std::vector requests = @@ -1800,10 +1875,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().load_basic(first_location, 13, 1.0), + rmf_task::State().load_basic(second_location, 1, 1.0) }; std::vector requests = @@ -1855,9 +1930,9 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { 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()); + CHECK_FALSE(last_assignment.request()->booking()->automatic()); + const auto& state = last_assignment.finish_state(); + CHECK_FALSE(state.waypoint() == state.dedicated_charging_waypoint()); } } @@ -1891,9 +1966,9 @@ SCENARIO("Grid World") for (const auto& agent : optimal_assignments) { const auto last_assignment = agent.back(); - CHECK(last_assignment.request()->automatic()); - const auto& state = last_assignment.state(); - CHECK(state.location().waypoint() == state.charging_waypoint()); + 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/test/unit/test_Activator.cpp b/rmf_task/test/unit/test_Activator.cpp new file mode 100644 index 00000000..7e72afb9 --- /dev/null +++ b/rmf_task/test/unit/test_Activator.cpp @@ -0,0 +1,85 @@ +/* + * 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("Activate fresh task") +{ + 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; + std::optional backup; + auto active = activator.activate( + nullptr, + nullptr, + *request, + [&phase_snapshot](auto s) { phase_snapshot = s; }, + [&backup](auto b) { backup = b; }, + [](auto) {}, + []() {}); + + REQUIRE(active); + + auto mock_active = + std::dynamic_pointer_cast(active); + + REQUIRE(mock_active); + + 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); + + // ====== Restoring a task ======== + CHECK_FALSE(backup.has_value()); + mock_active->issue_backup(); + REQUIRE(backup.has_value()); + CHECK(backup->sequence() == 0); + 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) {}, + []() {}); + + auto mock_restored = + std::dynamic_pointer_cast(restored); + + REQUIRE(mock_restored); + REQUIRE(mock_restored->_restored_state.has_value()); + CHECK(mock_restored->_restored_state == backup->state()); +} 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/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..cdda3f47 --- /dev/null +++ b/rmf_task_sequence/CMakeLists.txt @@ -0,0 +1,136 @@ +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_api_msgs REQUIRED) +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(ament_cmake_uncrustify QUIET) + +# ===== RMF Task Sequence Library +file(GLOB_RECURSE 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 + nlohmann_json::nlohmann_json + nlohmann_json_schema_validator +) + +target_include_directories(rmf_task_sequence + PUBLIC + $ + $ + $ # for auto-generated schema headers +) + +if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND ament_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/") + + ament_uncrustify( + ARGN include src test + CONFIG_FILE ${uncrustify_config_file} + MAX_LINE_LENGTH 80 + ) +endif() + +# Generate the schema headers +rmf_api_generate_schema_headers( + PACKAGE rmf_task_sequence + SCHEMAS_DIR ${CMAKE_CURRENT_LIST_DIR}/schemas +) + + +# 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:: +) + 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/generate_schema_header.cmake b/rmf_task_sequence/cmake/generate_schema_header.cmake new file mode 100644 index 00000000..2cac214b --- /dev/null +++ b/rmf_task_sequence/cmake/generate_schema_header.cmake @@ -0,0 +1,16 @@ +################################################# +# 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) + 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/cmake/rmf_task_sequence-config.cmake.in b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in new file mode 100644 index 00000000..01c062ba --- /dev/null +++ b/rmf_task_sequence/cmake/rmf_task_sequence-config.cmake.in @@ -0,0 +1,15 @@ +@PACKAGE_INIT@ + +get_filename_component(rmf_task_sequence_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) + +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") +endif() + +check_required_components(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 new file mode 100644 index 00000000..50b87907 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/Activity.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 RMF_TASK_SEQUENCE__ACTIVITY_HPP +#define RMF_TASK_SEQUENCE__ACTIVITY_HPP + +#include +#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 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 Activity 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; + + /// 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; +}; + +//============================================================================== +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, + rmf_traffic::Time earliest_arrival_time, + 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 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. + /// + /// \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, + rmf_traffic::Time earliest_arrival_time, + 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..926f3c29 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/Event.hpp @@ -0,0 +1,303 @@ +/* + * 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: + + // Event::Active simply uses the Activity::Active API + class Active; + using ActivePtr = std::shared_ptr; + + // 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 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; + + /// 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] 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 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 +/// +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] 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 + /// + /// \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. + /// + /// \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 = + std::function< + StandbyPtr( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Description& description, + std::function update) + >; + + /// 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 + /// + /// \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 AssignIDPtr& id, + 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( + Initialize initializer, + Restore restorer); + + /// 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 + /// + /// \param[in] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] description + /// The description 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. + /// + /// \return an Event in a Standby state + StandbyPtr initialize( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + std::function update) const; + + /// Signature for restoring 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 + /// + /// \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 AssignIDPtr& id, + 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( + std::type_index type, + Initialize initializer, + Restore restorer); + + 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/Phase.hpp b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp new file mode 100644 index 00000000..b0cc7e2a --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/Phase.hpp @@ -0,0 +1,176 @@ +/* + * 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 + +namespace rmf_task_sequence { + +//============================================================================== +/// A factory for generating execute::ActivePhase instances from descriptions. +class Phase : public rmf_task::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 rmf_task::Phase::Active, + public Activity::Active {}; + +//============================================================================== +class Phase::Activator +{ +public: + /// Construct an empty Activator + Activator(); + + /// Signature for activating a phase + /// + /// \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] parameters + /// A reference to the parameters for the robot + /// + /// \param[in] tag + /// The tag of this phase + /// + /// \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 + /// 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. + /// + /// \return an active, running instance of the described phase. + template + using Activate = + std::function< + ActivePtr( + const std::function& get_state, + const ConstParametersPtr& parameters, + ConstTagPtr tag, + const Description& description, + std::optional backup_state, + std::function update, + std::function checkpoint, + 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] 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 + /// + /// \param[in] description + /// The description of the phase + /// + /// \param[in] backup_state + /// 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 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( + const std::function& get_state, + const ConstParametersPtr& parameters, + ConstTagPtr tag, + const Description& description, + std::optional backup_state, + std::function update, + std::function checkpoint, + std::function finished) const; + + class Implementation; +private: + + /// \private + void _add_activator( + std::type_index type, + Activate activator); + + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Phase::Description : public Activity::Description {}; + +} // namespace rmf_task_sequence + +#include + +#endif // RMF_TASK_SEQUENCE__PHASE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/Task.hpp b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp new file mode 100644 index 00000000..b67d450a --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/Task.hpp @@ -0,0 +1,195 @@ +/* + * 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 +#include +#include +#include + +#include + +namespace rmf_task_sequence { + +//============================================================================== +class Task : public rmf_task::Task +{ +public: + + // Declaration + class Builder; + + // Declaration + class Active; + + // Declaration + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; + + using Update = std::function; + using PhaseFinished = std::function; + using TaskFinished = std::function; + + /// Make an activator for a phase sequence task. This activator can be given + /// to the rmf_task::Activator class to activate phase sequence tasks from + /// phase sequence descriptions. + /// + /// \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 rmf_task::Activator::Activate make_activator( + 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); + + /// 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); + +}; + +//============================================================================== +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. + Builder& add_phase( + Phase::ConstDescriptionPtr description, + std::vector cancellation_sequence); + + /// Generate a TaskDescription instance from the phases that have been given + /// to the builder. + /// + /// \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 + std::shared_ptr build( + std::string category, + std::string detail); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +//============================================================================== +class Task::Description : public rmf_task::Task::Description +{ +public: + + // 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 category for this task + const std::string& category() const; + + /// Change the category for this task + Description& category(std::string new_category); + + /// Get the details for this task + const std::string& detail() const; + + /// 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: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace rmf_task_sequence + +#include + +#endif // RMF_TASK_SEQUENCE__TASK_HPP 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..0d719726 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/Backup.hpp @@ -0,0 +1,67 @@ +/* + * 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: + Backup(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace detail +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__BACKUP_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..c7a38f07 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Event.hpp @@ -0,0 +1,71 @@ +/* + * 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( + Initialize initializer, + Restore restorer) +{ + _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, + const nlohmann::json& backup, + std::function update, + std::function checkpoint, + std::function finished) + { + return restorer( + id, + get_state, + parameters, + static_cast(description), + backup, + std::move(update), + std::move(checkpoint), + std::move(finished)); + }); +} + +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__DETAIL__IMPL_EVENT_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..79b3053d --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/detail/impl_Phase.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__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]( + const std::function& get_state, + const ConstParametersPtr& parameters, + ConstTagPtr tag, + const Phase::Description& description, + std::optional backup_state, + std::function phase_update, + std::function phase_checkpoint, + std::function phase_finished) + { + return activator( + get_state, + parameters, + 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/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..52812ee5 --- /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 new file mode 100644 index 00000000..2bbe3311 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/Bundle.hpp @@ -0,0 +1,235 @@ +/* + * 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__BUNDLE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP + +#include +#include + +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class Bundle : public Event +{ +public: + + 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. + // (Not implemented yet) +// ParallelAll, + + /// The bundle will execute its dependencies in parallel and will finished + /// when any (one or more) of its dependencies finishes. + // (Not implemented yet) +// ParallelAny + }; + + class Description; + + /// Give an initializer the ability to initialize event bundles + /// + /// \param[in] 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 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. + /// + /// \param[in] type + /// The type of bundle to activate + /// + /// \param[in] dependencies + /// 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 + /// the name or detail of the state. + /// + /// \param[in] update + /// The callback that will be triggered when the bundle has an update. + /// + static StandbyPtr standby( + Type type, + const DependencySpecifiers& dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function update); + + /// 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); +}; + +//============================================================================== +class Bundle::Description : public Event::Description +{ +public: + + using Dependencies = std::vector; + + /// Construct a Sequence Description + /// + /// \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 bundle. If left unspecified, the + /// category will be based on its type. + /// + /// \param[in] detail + /// Optionally give some detail to this bundle. If left unspecified, the + /// detail will simply aggregate the details of the dependencies. + Description( + Dependencies dependencies, + Type type, + std::optional category = std::nullopt, + std::optional detail = std::nullopt); + + /// Get the elements of the sequence + const Dependencies& dependencies() const; + + /// Change the elements of the sequence + 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; + + /// 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 + +#include + +#endif // RMF_TASK_SEQUENCE__EVENTS__BUNDLE_HPP diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp new file mode 100644 index 00000000..d44eaaed --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/DropOff.hpp @@ -0,0 +1,113 @@ +/* + * 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__DROPOFF_HPP +#define RMF_TASK_SEQUENCE__EVENTS__DROPOFF_HPP + +#include + +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// 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 Event::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 + 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::unique_impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__DROPOFF_HPP 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/events/PerformAction.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp new file mode 100644 index 00000000..db1a2758 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2022 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 + +#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] category + /// A category for this action + /// + /// \param[in] description + /// A json description of the action to perform + /// + /// \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& category, + nlohmann::json description, + rmf_traffic::Duration action_duration_estimate, + bool use_tool_sink, + std::optional expected_finish_location = std::nullopt); + + /// Get the category + const std::string& category() const; + + /// Set the category + Description& category(const std::string& new_category); + + /// Get the description + const nlohmann::json& description() const; + + /// Set the description + Description& description(const nlohmann::json& new_description); + + /// 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/include/rmf_task_sequence/events/PickUp.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/PickUp.hpp new file mode 100644 index 00000000..8f2e957d --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PickUp.hpp @@ -0,0 +1,113 @@ +/* + * 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__PICKUP_HPP +#define RMF_TASK_SEQUENCE__EVENTS__PICKUP_HPP + +#include + +#include +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// A PickUp phase encompasses going to a location and transferring a payload +/// into/onto the robot. +class PickUp +{ +public: + + using Location = rmf_traffic::agv::Plan::Goal; + + class Description; + using DescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class PickUp::Description : public Event::Description +{ +public: + + /// Make a PickUp phase description + /// + /// \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 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 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 + 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::unique_impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__PICKUP_HPP 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 new file mode 100644 index 00000000..b5f77183 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.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__WAITFOR_HPP +#define RMF_TASK_SEQUENCE__EVENTS__WAITFOR_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +/// A WaitFor event encompasses having the robot sit in place and wait for a +/// period of time to pass. +/// +/// 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 +/// to be transferred. +class WaitFor +{ +public: + + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; + + class Model; +}; + +//============================================================================== +class WaitFor::Description : public Event::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); + + /// Constructor + Description(rmf_traffic::Duration 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 + Activity::ConstModelPtr make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const final; + + // Documentation inherited + rmf_task::Header generate_header( + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__PHASES__WAITFOR_HPP 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/phases/SimplePhase.hpp b/rmf_task_sequence/include/rmf_task_sequence/phases/SimplePhase.hpp new file mode 100644 index 00000000..ce41325f --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/phases/SimplePhase.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_SEQUENCE__PHASES__SIMPLEPHASE_HPP +#define RMF_TASK_SEQUENCE__PHASES__SIMPLEPHASE_HPP + +#include +#include + +namespace rmf_task_sequence { +namespace phases { + +//============================================================================== +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 phases +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__PHASES__SIMPLEPHASE_HPP 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/include/rmf_task_sequence/typedefs.hpp b/rmf_task_sequence/include/rmf_task_sequence/typedefs.hpp new file mode 100644 index 00000000..f0b20ae6 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/typedefs.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_SEQUENCE__TYPEDEFS_HPP +#define RMF_TASK_SEQUENCE__TYPEDEFS_HPP + +#include +#include +#include +#include +#include +#include +#include + +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; +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/package.xml b/rmf_task_sequence/package.xml new file mode 100644 index 00000000..14abf2ea --- /dev/null +++ b/rmf_task_sequence/package.xml @@ -0,0 +1,23 @@ + + + + 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_api_msgs + rmf_task + nlohmann-json-dev + nlohmann_json_schema_validator_vendor + + ament_cmake_catch2 + ament_cmake_uncrustify + + + cmake + + 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"] +} diff --git a/rmf_task_sequence/schemas/backup/backup_EventSequence_v0_1.schema.json b/rmf_task_sequence/schemas/backup/backup_EventSequence_v0_1.schema.json new file mode 100644 index 00000000..c98d2ac8 --- /dev/null +++ b/rmf_task_sequence/schemas/backup/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" ] +} diff --git a/rmf_task_sequence/schemas/backup/backup_PhaseSequenceTask_v0_1.schema.json b/rmf_task_sequence/schemas/backup/backup_PhaseSequenceTask_v0_1.schema.json new file mode 100644 index 00000000..5ca948d7 --- /dev/null +++ b/rmf_task_sequence/schemas/backup/backup_PhaseSequenceTask_v0_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 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 Phase Sequence Task Backup 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" ] +} 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..02088dde --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/Activity.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 + +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); + if (!next_model) + { + // TODO: Should we throw an error here? + return nullptr; + } + 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, + rmf_traffic::Time earliest_arrival_time, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const +{ + rmf_task::State finish_state = std::move(initial_state); + std::optional wait_until; + for (const auto& model : _pimpl->models) + { + const auto estimate = model->estimate_finish( + std::move(finish_state), + earliest_arrival_time, + constraints, + travel_estimator); + + 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 Estimate(std::move(finish_state), *wait_until); +} + +//============================================================================== +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/Event.cpp b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp new file mode 100644 index 00000000..4c63f4e8 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/Event.cpp @@ -0,0 +1,97 @@ +/* + * 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; + std::unordered_map> restorers; + +}; + +//============================================================================== +Event::Initializer::Initializer() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +Event::StandbyPtr Event::Initializer::initialize( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Event::Description& description, + std::function update) const +{ + const auto& type = typeid(description); + const auto it = _pimpl->initializers.find(type); + if (it == _pimpl->initializers.end()) + return nullptr; + + return it->second( + id, + get_state, + parameters, + description, + std::move(update)); +} + +//============================================================================== +Event::ActivePtr Event::Initializer::restore( + const AssignIDPtr& id, + 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( + id, + get_state, + parameters, + description, + backup, + std::move(update), + std::move(checkpoint), + std::move(finished)); +} + +//============================================================================== +void Event::Initializer::_add( + std::type_index type, + 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/Phase.cpp b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp new file mode 100644 index 00000000..06b544d1 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/Phase.cpp @@ -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. + * +*/ + +#include + +namespace rmf_task_sequence { + +//============================================================================== +class Phase::Activator::Implementation +{ +public: + + std::unordered_map> activators; + +}; + +//============================================================================== +Phase::Activator::Activator() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +Phase::ActivePtr Phase::Activator::activate( + const std::function& get_state, + const ConstParametersPtr& parameters, + 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( + get_state, + parameters, + 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)); +} + +} // 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 new file mode 100644 index 00000000..b5e6131e --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -0,0 +1,964 @@ +/* + * 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 + +#include + +namespace rmf_task_sequence { + +namespace { + +//============================================================================== +struct Stage +{ + 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; +}; + +//============================================================================== +class Task::Description::Implementation +{ +public: + + std::string category; + std::string detail; + std::vector stages; + + static std::list get_stages(const Description& desc) + { + return std::list( + desc._pimpl->stages.begin(), + 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; +}; + +//============================================================================== +class Task::Active + : public rmf_task::Task::Active, + public std::enable_shared_from_this +{ +public: + + 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, + std::function update, + std::function checkpoint, + std::function phase_finished, + std::function task_finished) + { + auto task = std::shared_ptr( + new Active( + std::move(phase_activator), + std::move(clock), + std::move(get_state), + parameters, + booking, + description, + std::move(update), + std::move(checkpoint), + std::move(phase_finished), + std::move(task_finished))); + + if (backup_state.has_value()) + { + task->_load_backup(std::move(*backup_state)); + return task; + } + + task->_generate_pending_phases(); + task->_begin_next_stage(); + + return task; + } + + // Documentation inherited + Event::Status status_overview() const final; + + // Documentation inherited + const std::vector& completed_phases() const final; + + // Documentation inherited + Phase::ConstActivePtr active_phase() const final; + + // Documentation inherited + std::optional active_phase_start_time() const final; + + // Documentation inherited + const std::vector& pending_phases() const final; + + // Documentation inherited + const ConstTagPtr& tag() const final; + + // Documentation inherited + rmf_traffic::Duration estimate_remaining_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; + + static const nlohmann::json_schema::json_validator backup_schema_validator; + +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. + /// + /// \return false if the task needs to be aborted due to a bad backup_state, + /// otherwise return true. + void _load_backup(std::string backup_state); + void _generate_pending_phases(); + + void _finish_phase(); + 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; + + Backup _generate_backup( + Phase::Tag::Id current_phase_id, + Phase::Active::Backup phase_backup) const; + + 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 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), + _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) + { + // Do nothing + } + + Phase::ConstActivatorPtr _phase_activator; + std::function _clock; + std::function _get_state; + ConstParametersPtr _parameters; + 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; + + 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; + 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; +}; + +//============================================================================== +const nlohmann::json_schema::json_validator +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, + 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::Builder::build( + std::string category, + std::string detail) -> std::shared_ptr +{ + 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); +} + +//============================================================================== +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 +{ + 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 +} + +//============================================================================== +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 +{ + return _completed_phases; +} + +//============================================================================== +Phase::ConstActivePtr Task::Active::active_phase() const +{ + return _active_phase; +} + +//============================================================================== +std::optional Task::Active::active_phase_start_time() const +{ + return _current_phase_start_time; +} + +//============================================================================== +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 ? _active_phase->estimate_remaining_time() : + rmf_traffic::Duration(0); + for (const auto& p : _pending_phases) + remaining_time += p.tag()->header().original_duration_estimate(); + + return remaining_time; +} + +//============================================================================== +auto Task::Active::backup() const -> Backup +{ + return _generate_backup( + _active_phase->tag()->id(), + _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, rmf_traffic::Duration(0)); + + const auto start_time = _clock(); + + const auto failed_to_restore = [&]() -> void + { + _pending_stages.clear(); + _phase_finished( + std::make_shared( + rmf_task::Phase::Snapshot::make(*restore_phase), + start_time, + _clock())); + + _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->parsing_failed(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->parsing_failed( + "Invalid value [" + std::to_string(cancelled_from) + + "] for [cancelled_from]. Value must be less than [" + + std::to_string(_cancel_sequence_initial_id) + "]"); + + 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->parsing_failed( + "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->update_log().warn( + "Unexpected ordering of phase skip IDs"); + continue; + } + + pending_it->will_be_skipped(true); + } + } + + _begin_next_stage(current_phase_json["state"]); +} + +//============================================================================== +void Task::Active::_generate_pending_phases() +{ + auto state = _get_state(); + _pending_phases.reserve(_pending_stages.size()); + for (const auto& s : _pending_stages) + { + _pending_phases.emplace_back( + std::make_shared( + s->id, + s->description->generate_header(state, *_parameters) + ) + ); + + state = s->description->make_model( + state, *_parameters)->invariant_finish_state(); + } +} + +//============================================================================== +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( + 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); + + _begin_next_stage(); +} + +//============================================================================== +void Task::Active::_begin_next_stage(std::optional restore) +{ + 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(); + + while (true) + { + if (_pending_stages.empty()) + return _finish_task(); + + _active_stage = _pending_stages.front(); + assert(_active_stage->id == _pending_phases.front().tag()->id()); + const auto skip_phase = _pending_phases.front().will_be_skipped(); + + _pending_stages.pop_front(); + 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; + + if (skip_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; + } + + const auto phase_id = tag->id(); + _current_phase_start_time = _clock(); + _active_phase = _phase_activator->activate( + _get_state, + _parameters, + std::move(tag), + *_active_stage->description, + std::move(restore), + [me = weak_from_this()](Phase::ConstSnapshotPtr snapshot) + { + if (const auto self = me.lock()) + self->_update(snapshot); + }, + [me = weak_from_this(), id = phase_id]( + Phase::Active::Backup backup) + { + if (const auto self = me.lock()) + self->_issue_backup(id, std::move(backup)); + }, + [me = weak_from_this()]() + { + if (const auto self = me.lock()) + self->_finish_phase(); + }); + + _update(Phase::Snapshot::make(*_active_phase)); + _issue_backup(phase_id, _active_phase->backup()); + return; + } +} + +//============================================================================== +void Task::Active::_finish_task() +{ + _task_finished(); +} + +//============================================================================== +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))); +} + +//============================================================================== +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, + Phase::Active::Backup phase_backup) const -> Backup +{ + nlohmann::json current_phase; + current_phase["id"] = current_phase_id; + if (_cancelled_on_phase.has_value()) + current_phase["cancelled_from"] = *_cancelled_on_phase; + + current_phase["state"] = 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()); + } + + 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++, root.dump()); +} + +//============================================================================== +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) + ]( + const std::function& get_state, + const ConstParametersPtr& parameters, + const ConstBookingPtr& booking, + const Description& description, + std::optional backup_state, + std::function update, + std::function checkpoint, + 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(checkpoint), + std::move(phase_finished), + std::move(task_finished)); + }; +} + +//============================================================================== +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/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 new file mode 100644 index 00000000..441b7cba --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Bundle.cpp @@ -0,0 +1,427 @@ +/* + * 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 "internal_Sequence.hpp" + +#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 + +//============================================================================== +Event::StandbyPtr Bundle::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) +{ + if (description.type() == Bundle::Type::Sequence) + { + return internal::Sequence::Standby::initiate( + initializer, + id, + get_state, + parameters, + description, + std::move(parent_update)); + } + + // *INDENT-OFF* + throw std::runtime_error( + "[rmf_task_sequence::events::Bundle::initiate] " + "Bundle type not yet implemented: " + + std::to_string(description.type())); + // *INDENT-ON* +} + +//============================================================================== +Event::ActivePtr Bundle::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 parent_update, + std::function checkpoint, + std::function finished) +{ + if (description.type() == Bundle::Type::Sequence) + { + return internal::Sequence::Active::restore( + initializer, + id, + get_state, + parameters, + description, + backup, + parent_update, + checkpoint, + finished); + } + + // *INDENT-OFF* + throw std::runtime_error( + "Bundle type not yet implemented: " + std::to_string(description.type())); + // *INDENT-ON* +} + +//============================================================================== +class Bundle::Description::Implementation +{ +public: + + Dependencies dependencies; + Type type; + std::optional category; + std::optional detail; + + std::string generate_category() const + { + if (category.has_value()) + return *category; + + switch (type) + { + case Type::Sequence: + return "Sequence"; + // 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 ""; + } + + rmf_traffic::Duration adjust_estimate( + std::optional current_estimate, + rmf_traffic::Duration next_dependency_estimate) const + { + // 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; + } + + Header generate_header( + rmf_task::State initial_state, + const Parameters& parameters) const + { + std::optional> detail_json; + if (!detail.has_value()) + detail_json = std::vector(); + + std::optional duration_estimate; + + for (const auto& element : dependencies) + { + const auto element_header = + element->generate_header(initial_state, parameters); + + duration_estimate = adjust_estimate( + 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.value_or(rmf_traffic::Duration(0))); + } +}; + +//============================================================================== +Bundle::Description::Description( + Dependencies dependencies, + Type type, + std::optional category, + std::optional detail) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(dependencies), + type, + std::move(category), + std::move(detail) + })) +{ + // Do nothing +} + +//============================================================================== +auto Bundle::Description::dependencies() const -> const Dependencies& +{ + return _pimpl->dependencies; +} + +//============================================================================== +auto Bundle::Description::dependencies(Dependencies new_dependencies) +-> Description& +{ + _pimpl->dependencies = std::move(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 +{ + return _pimpl->category; +} + +//============================================================================== +auto Bundle::Description::category(std::optional new_category) +-> Description& +{ + _pimpl->category = std::move(new_category); + return *this; +} + +//============================================================================== +const std::optional& Bundle::Description::detail() const +{ + return _pimpl->detail; +} + +//============================================================================== +auto Bundle::Description::detail(std::optional new_detail) +-> Description& +{ + _pimpl->detail = std::move(new_detail); + return *this; +} + +//============================================================================== +Activity::ConstModelPtr Bundle::Description::make_model( + rmf_task::State invariant_initial_state, + const Parameters& parameters) const +{ + return Activity::SequenceModel::make( + _pimpl->dependencies, + std::move(invariant_initial_state), + parameters); +} + +//============================================================================== +Header Bundle::Description::generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->generate_header(initial_state, parameters); +} + +//============================================================================== +void Bundle::add(const Event::InitializerPtr& 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)); + }); +} + +//============================================================================== +void Bundle::add( + Event::Initializer& add_to, + const Event::ConstInitializerPtr& initialize_from) +{ + add_to.add( + [initialize_from]( + const AssignIDPtr& id, + const std::function& get_state, + const ConstParametersPtr& parameters, + const Bundle::Description& description, + std::function update) + { + 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, + const nlohmann::json& backup_state, + std::function update, + std::function checkpoint, + std::function finished) + { + return restore( + *initialize_from, + id, + get_state, + parameters, + description, + backup_state, + std::move(update), + std::move(checkpoint), + std::move(finished)); + }); +} + +//============================================================================== +Event::StandbyPtr Bundle::standby( + Type type, + const std::vector>& dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function update) +{ + if (type == Bundle::Type::Sequence) + { + auto sequence = internal::Sequence::Standby::initiate( + std::move(dependencies), + std::move(state), + std::move(update)); + + return sequence; + } + + // *INDENT-OFF* + throw std::runtime_error( + "[rmf_task_sequence::events::Bundle::activate] " + "Bundle type not yet implemented: " + std::to_string(type)); + // *INDENT-ON* +} + +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp new file mode 100644 index 00000000..576a722d --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/DropOff.cpp @@ -0,0 +1,132 @@ +/* + * 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_sequence { +namespace events { + +//============================================================================== +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->destination(); +} + +//============================================================================== +auto DropOff::Description::drop_off_location(Location new_location) +-> Description& +{ + _pimpl->transfer.go_to_place->destination(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; +} + +//============================================================================== +Activity::ConstModelPtr DropOff::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.make_model( + std::move(invariant_initial_state), parameters); +} + +//============================================================================== +Header DropOff::Description::generate_header( + const State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer + .generate_header("Drop off", initial_state, parameters); +} + +//============================================================================== +DropOff::Description::Description() +{ + // Do nothing +} + +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp new file mode 100644 index 00000000..665e14fa --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp @@ -0,0 +1,286 @@ +/* + * 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 { + +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 Activity::Model +{ +public: + + static Activity::ConstModelPtr make( + State invariant_initial_state, + const Parameters& parameters, + Goal goal); + + // Documentation inherited + std::optional estimate_finish( + State initial_state, + rmf_traffic::Time earliest_arrival_time, + const Constraints& constraints, + const TravelEstimator& estimator) 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, + Goal goal); + + State _invariant_finish_state; + rmf_traffic::Duration _invariant_duration; + Goal _goal; +}; + +//============================================================================== +Activity::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, + std::move(goal))); +} + +//============================================================================== +std::optional GoToPlace::Model::estimate_finish( + State initial_state, + rmf_traffic::Time earliest_arrival_time, + 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; + + 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()) + 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 Estimate(finish, wait_until_time); +} + +//============================================================================== +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 +} + +//============================================================================== +class GoToPlace::Description::Implementation +{ +public: + + rmf_traffic::agv::Plan::Goal destination; +}; + +//============================================================================== +auto GoToPlace::Description::make(Goal goal) -> DescriptionPtr +{ + auto desc = std::shared_ptr(new Description); + desc->_pimpl = rmf_utils::make_impl( + Implementation{ + std::move(goal) + }); + + return desc; +} + +//============================================================================== +Activity::ConstModelPtr GoToPlace::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return Model::make( + std::move(invariant_initial_state), + parameters, + _pimpl->destination); +} + +//============================================================================== +Header GoToPlace::Description::generate_header( + const State& initial_state, + const Parameters& parameters) const +{ + const std::string& fail_header = "[GoToPlace::Description::generate_header]"; + + const auto start_wp_opt = initial_state.waypoint(); + if (!start_wp_opt) + 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) + { + 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 = rmf_task::standard_waypoint_name(graph, start_wp); + + if (graph.num_waypoints() <= _pimpl->destination.waypoint()) + { + utils::fail(fail_header, "Destination waypoint [" + + std::to_string(_pimpl->destination.waypoint()) + + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + + "]"); + } + + const auto goal_name_ = destination_name(parameters); + + const auto estimate = estimate_duration( + parameters.planner(), initial_state, _pimpl->destination); + + if (!estimate.has_value()) + { + utils::fail(fail_header, "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_, + *estimate); +} + +//============================================================================== +auto GoToPlace::Description::destination() const -> const Goal& +{ + return _pimpl->destination; +} + +//============================================================================== +auto GoToPlace::Description::destination(Goal new_goal) -> Description& +{ + _pimpl->destination = std::move(new_goal); + return *this; +} + +//============================================================================== +std::string GoToPlace::Description::destination_name( + const Parameters& parameters) const +{ + return rmf_task::standard_waypoint_name( + parameters.planner()->get_configuration().graph(), + _pimpl->destination.waypoint()); +} + +//============================================================================== +GoToPlace::Description::Description() +{ + // Do nothing +} + +} // namespace phases +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.cpp new file mode 100644 index 00000000..a279a4d8 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PayloadTransfer.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 "internal_PayloadTransfer.hpp" + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +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(events::GoToPlace::Description::make(std::move(location_))), + wait_for(events::WaitFor::Description::make(loading_duration_estimate)) +{ + descriptions = {go_to_place, wait_for}; +} + +//============================================================================== +Activity::ConstModelPtr PayloadTransfer::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return Activity::SequenceModel::make( + descriptions, + std::move(invariant_initial_state), + parameters); +} + +//============================================================================== +Header PayloadTransfer::generate_header( + const std::string& type, + const State& initial_state, + const Parameters& parameters) const +{ + const auto model = make_model(initial_state, parameters); + + return Header( + type, + type + " " + payload.brief("into") + " at " + + go_to_place->destination_name(parameters), + model->invariant_duration()); +} + +} // namespace events +} // namespace rmf_task_sequence 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..e00b0932 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -0,0 +1,276 @@ +/* + * Copyright (C) 2022 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, + rmf_traffic::Time earliest_arrival_time, + 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(invariant_finish_state), + _invariant_duration(invariant_duration), + _use_tool_sink(use_tool_sink) +{ + if (parameters.ambient_sink() != nullptr) + { + _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, + rmf_traffic::Time earliest_arrival_time, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const +{ + initial_state.time(initial_state.time().value() + _invariant_duration); + if (_invariant_finish_state.waypoint().has_value()) + 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 Estimate(initial_state, earliest_arrival_time); +} + +//============================================================================== +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 category; + nlohmann::json description; + rmf_traffic::Duration action_duration_estimate; + bool use_tool_sink; + std::optional expected_finish_location; +}; + +//============================================================================== +auto PerformAction::Description::make( + const std::string& category, + nlohmann::json action, + 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(category), + std::move(action), + std::move(duration), + std::move(use_tool_sink), + std::move(location) + }); + + return description; +} + +//============================================================================== +PerformAction::Description::Description() +{ + // Do nothing +} + +//============================================================================== +const std::string& +PerformAction::Description::category() const +{ + return _pimpl->category; +} + +//============================================================================== +auto PerformAction::Description::category( + const std::string& new_category) -> Description& +{ + _pimpl->category = new_category; + return *this; +} + +//============================================================================== +const nlohmann::json& +PerformAction::Description::description() const +{ + return _pimpl->description; +} + +//============================================================================== +auto PerformAction::Description::description( + const nlohmann::json& new_description) -> Description& +{ + _pimpl->description = new_description; + 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 = 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 action", + "Performing action " + _pimpl->category + + " 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/PickUp.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PickUp.cpp new file mode 100644 index 00000000..a4231fa3 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PickUp.cpp @@ -0,0 +1,130 @@ +/* + * 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_sequence { +namespace events { + +//============================================================================== +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->destination(); +} + +//============================================================================== +auto PickUp::Description::pickup_location(Location new_location) -> Description& +{ + _pimpl->transfer.go_to_place->destination(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; +} + +//============================================================================== +Activity::ConstModelPtr PickUp::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.make_model( + std::move(invariant_initial_state), parameters); +} + +//============================================================================== +Header PickUp::Description::generate_header( + const State& initial_state, + const Parameters& parameters) const +{ + return _pimpl->transfer.generate_header("Pick up", initial_state, parameters); +} + +//============================================================================== +PickUp::Description::Description() +{ + // Do nothing +} + +} // namespace events +} // namespace rmf_task_sequence 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..27316f1c --- /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 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..8d5c5351 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/Sequence.cpp @@ -0,0 +1,403 @@ +/* + * 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_Sequence.hpp" + +namespace rmf_task_sequence { +namespace events { +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(id, description); + const auto update = [parent_update, state]() + { + update_status(*state); + parent_update(); + }; + + std::vector dependencies; + dependencies.reserve(description.dependencies().size()); + for (const auto& desc : description.dependencies()) + { + auto element = initializer.initialize( + id, get_state, parameters, *desc, update); + + 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()); + + 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 : _reverse_dependencies) + estimate += element->duration_estimate(); + + return estimate; +} + +//============================================================================== +Sequence::Standby::Standby( + std::vector reverse_dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function parent_update) +: _reverse_dependencies(std::move(reverse_dependencies)), + _state(std::move(state)), + _parent_update(std::move(parent_update)) +{ + std::vector state_deps; + state_deps.reserve(_reverse_dependencies.size()); + const auto rbegin = _reverse_dependencies.rbegin(); + const auto rend = _reverse_dependencies.rend(); + for (auto rit = rbegin; rit != rend; ++rit) + state_deps.push_back((*rit)->state()); + + _state->update_dependencies(std::move(state_deps)); + update_status(*_state); +} + +//============================================================================== +Event::ActivePtr Sequence::Standby::begin( + std::function checkpoint, + std::function finish) +{ + if (_active) + return _active; + + _active = std::make_shared( + std::move(_reverse_dependencies), _state, _parent_update, + std::move(checkpoint), std::move(finish)); + + _active->next(); + return _active; +} + +//============================================================================== +rmf_task::events::SimpleEventStatePtr Sequence::Standby::make_state( + const Event::AssignIDPtr& id, + const Bundle::Description& description) +{ + 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::SimpleEventState& 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 Event::AssignIDPtr& id, + 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(id, 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( + id, + get_state, + parameters, + *element_descriptions.at(current_event_index), + current_event_state, + update, + checkpoint, + event_finished); + state->add_dependency(active->state()); + + const std::size_t num_elements = element_descriptions.size(); + for (std::size_t i = current_event_index + 1; i < num_elements; ++i) + { + const auto& desc = element_descriptions[i]; + auto element = initializer.initialize( + id, 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::SimpleEventStatePtr 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::SimpleEventStatePtr 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(); + _checkpoint(); +} + +//============================================================================== +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/WaitFor.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp new file mode 100644 index 00000000..b82506aa --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp @@ -0,0 +1,159 @@ +/* + * 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 WaitFor::Model : public Activity::Model +{ +public: + + Model( + rmf_task::State invariant_initial_state, + rmf_traffic::Duration duration, + const rmf_task::Parameters& parameters); + + std::optional estimate_finish( + rmf_task::State initial_state, + rmf_traffic::Time earliest_arrival_time, + 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 _duration; +}; + +//============================================================================== +class WaitFor::Description::Implementation +{ +public: + + rmf_traffic::Duration duration; + +}; + +//============================================================================== +auto WaitFor::Description::make(rmf_traffic::Duration wait_duration) +-> DescriptionPtr +{ + return std::make_shared(wait_duration); +} + +//============================================================================== +WaitFor::Description::Description(rmf_traffic::Duration duration_) +: _pimpl(rmf_utils::make_impl(Implementation{duration_})) +{ + // Do nothing +} + +//============================================================================== +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; +} + +//============================================================================== +Activity::ConstModelPtr WaitFor::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + return std::make_shared( + invariant_initial_state, _pimpl->duration, parameters); +} + +//============================================================================== +Header WaitFor::Description::generate_header( + const State&, const Parameters&) const +{ + const auto seconds = std::chrono::duration_cast( + _pimpl->duration); + + return Header( + "Waiting", + "Waiting for [" + std::to_string(seconds.count()) + "] seconds to elapse", + _pimpl->duration); +} + +//============================================================================== +WaitFor::Model::Model( + State invariant_initial_state, + rmf_traffic::Duration duration, + const Parameters& parameters) +: _invariant_finish_state(std::move(invariant_initial_state)), + _duration(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; + } +} + +//============================================================================== +std::optional WaitFor::Model::estimate_finish( + State state, + rmf_traffic::Time earliest_arrival_time, + 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 Estimate(state, earliest_arrival_time); +} + +//============================================================================== +rmf_traffic::Duration WaitFor::Model::invariant_duration() const +{ + return _duration; +} + +//============================================================================== +State WaitFor::Model::invariant_finish_state() const +{ + return _invariant_finish_state; +} + +} // namespace phases +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.hpp new file mode 100644 index 00000000..e4add81a --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_PayloadTransfer.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_SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP +#define SRC__RMF_TASK_SEQUENCE__PHASES__INTERNAL_PAYLOADTRANSFER_HPP + +#include +#include +#include +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class PayloadTransfer +{ +public: + + using Location = rmf_traffic::agv::Plan::Goal; + + std::string target; + Payload payload; + events::GoToPlace::DescriptionPtr go_to_place; + events::WaitFor::DescriptionPtr wait_for; + std::vector descriptions; + + PayloadTransfer( + Location location_, + std::string target_, + Payload payload_, + rmf_traffic::Duration loading_duration_estimate); + + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const; + + Header generate_header( + const std::string& type, + const State& initial_state, + const Parameters& parameters) const; +}; + +} // 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 new file mode 100644 index 00000000..7836d68d --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/internal_Sequence.hpp @@ -0,0 +1,175 @@ +/* + * 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 +#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: + + class Standby; + class Active; + +}; + +//============================================================================== +class Sequence::Standby : public Event::Standby +{ +public: + + 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 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; + + Event::ActivePtr begin( + std::function checkpoint, + std::function finish) final; + + Standby( + std::vector reverse_dependencies, + rmf_task::events::SimpleEventStatePtr state, + std::function parent_update); + + static rmf_task::events::SimpleEventStatePtr make_state( + const Event::AssignIDPtr& id, + const Bundle::Description& description); + + static void update_status(rmf_task::events::SimpleEventState& state); + +private: + + std::vector _reverse_dependencies; + rmf_task::events::SimpleEventStatePtr _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 Event::AssignIDPtr& id, + 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::SimpleEventStatePtr state, + std::function parent_update, + std::function checkpoint, + std::function finished); + + Active( + uint64_t current_event_index, + rmf_task::events::SimpleEventStatePtr 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::SimpleEventStatePtr _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 + +#endif // SRC__RMF_TASK_SEQUENCE__EVENTS__INTERNAL_SEQUENCE_HPP 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..a8ac2ffa --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2022 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" + +#include + +namespace rmf_task_sequence { +namespace events { +namespace utils { + +//============================================================================== +std::string waypoint_name( + const std::size_t index, + const rmf_task::Parameters& parameters) +{ + return rmf_task::standard_waypoint_name( + parameters.planner()->get_configuration().graph(), + index); +} + +//============================================================================== +void fail(const std::string& header, const std::string& msg) +{ + throw std::runtime_error( + header + " " + msg); +} + +} // 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..796b4754 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2022 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); + +//============================================================================== +void fail(const std::string& header, const std::string& msg); + + +} // namespace utils +} // namespace events +} // namespace rmf_task_sequence + +# endif // SRC__RMF_TASK_SEQUENCE__EVENTS__UTILS_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp b/rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp new file mode 100644 index 00000000..7b85ee5a --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/phases/SimplePhase.cpp @@ -0,0 +1,259 @@ +/* + * 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 phases { + +//============================================================================== +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(); + assert(tag != nullptr); + phase->_tag = tag; + + std::function event_update = + [ + weak = phase->weak_from_this(), + phase_update = std::move(phase_update) + ]() + { + if (const auto phase = weak.lock()) + { + if (phase->_final_event) + 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()) + { + if (phase->_final_event) + 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 phases +} // namespace rmf_task_sequence 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 new file mode 100644 index 00000000..80d3b154 --- /dev/null +++ b/rmf_task_sequence/templates/schemas_template.hpp.in @@ -0,0 +1,22 @@ +/* + * 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@ +#define RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ + +#include + +namespace rmf_task_sequence { +namespace schemas { + +static const nlohmann::json @schema_name@ = +R"raw_schema( +@schema_text@)raw_schema"_json; + +} // namespace schemas +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__SCHEMAS__@upper_schema_name@ 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 diff --git a/rmf_task_sequence/test/mock/MockActivity.cpp b/rmf_task_sequence/test/mock/MockActivity.cpp new file mode 100644 index 00000000..caed9a2a --- /dev/null +++ b/rmf_task_sequence/test/mock/MockActivity.cpp @@ -0,0 +1,219 @@ +/* + * 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); + } + else + { + output->state_data->update_status(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::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) +{ + 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..b223268c --- /dev/null +++ b/rmf_task_sequence/test/mock/MockActivity.hpp @@ -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. + * +*/ + +#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; + + void update(rmf_task::Event::Status status, std::string text); + + void complete(); + + 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/events/test_PerformAction.cpp b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp new file mode 100644 index 00000000..fad54508 --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2022 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 category = "test_action"; + const nlohmann::json desc; + const rmf_traffic::agv::Planner::Goal finish_location{1}; + + auto description = PerformAction::Description::make( + category, + desc, + 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->category() == category); + CHECK(description->description() == desc); + 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->category("new_category"); + CHECK(description->category() == "new_category"); + nlohmann::json new_desc = {{"key", "value"}}; + description->description(new_desc); + CHECK(description->description() == new_desc); + 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, + now, + *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/test_Sequence.cpp b/rmf_task_sequence/test/unit/test_Sequence.cpp new file mode 100644 index 00000000..43020b3a --- /dev/null +++ b/rmf_task_sequence/test/unit/test_Sequence.cpp @@ -0,0 +1,423 @@ +/* + * 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 +#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::phases::SimplePhase::Description::make(sequence); +} + +//============================================================================== +rmf_task_sequence::Phase::ConstDescriptionPtr make_phase( + std::shared_ptr ctrl) +{ + return rmf_task_sequence::phases::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 = + 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(); + + const auto phase_activator = + std::make_shared(); + rmf_task_sequence::phases::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); + } +} diff --git a/rmf_task_sequence/test/unit/utils.hpp b/rmf_task_sequence/test/unit/utils.hpp new file mode 100644 index 00000000..bd533d3a --- /dev/null +++ b/rmf_task_sequence/test/unit/utils.hpp @@ -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. + * +*/ + +#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_traffic::Time earliest_arrival_time, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator, + const rmf_task::State& expected_finish_state) +{ + + const auto estimated_finish = model.estimate_finish( + initial_state, + earliest_arrival_time, + constraints, + travel_estimator); + + REQUIRE(estimated_finish.has_value()); + + CHECK_STATE(estimated_finish.value().finish_state(), 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; + + return rmf_traffic::time::from_seconds(*result.ideal_cost()); +} + +} // anonymous namespace + +#endif // TEST__UNIT__UTILS_HPP