diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 8decc4d7..d9c819cd 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -26,7 +26,7 @@ find_package(nlohmann_json REQUIRED) find_package(nlohmann_json_schema_validator_vendor REQUIRED) find_package(ament_cmake_catch2 QUIET) -find_package(rmf_cmake_uncrustify QUIET) +find_package(ament_cmake_uncrustify QUIET) # ===== RMF Task Sequence Library file(GLOB_RECURSE lib_srcs @@ -51,10 +51,8 @@ target_include_directories(rmf_task_sequence $ # for auto-generated schema headers ) -if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) - - file(GLOB_RECURSE test_srcs "test/*.cpp") - +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 ${test_srcs} TIMEOUT 300) @@ -71,7 +69,7 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - rmf_uncrustify( + ament_uncrustify( ARGN include src test CONFIG_FILE ${uncrustify_config_file} MAX_LINE_LENGTH 80 @@ -136,4 +134,3 @@ export( NAMESPACE rmf_task_sequence:: ) -export(PACKAGE rmf_task_sequence) 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/WaitFor.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp index 290f446c..b5f77183 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp @@ -29,7 +29,7 @@ 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 phase may be useful for calculating the Models of other +/// The Model of this event may be useful for calculating the Models of other /// phases that include a period of time where the robot is waiting for a /// process to finish. E.g. the PickUp and DropOff Models use WaitFor::Model to /// calculate how much the robot's battery drains while waiting for the payload diff --git a/rmf_task_sequence/package.xml b/rmf_task_sequence/package.xml index 6f194213..528fe730 100644 --- a/rmf_task_sequence/package.xml +++ b/rmf_task_sequence/package.xml @@ -15,7 +15,7 @@ nlohmann_json_schema_validator_vendor ament_cmake_catch2 - rmf_cmake_uncrustify + ament_cmake_uncrustify cmake diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 5c70d048..a536ebe5 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp @@ -464,7 +464,9 @@ const Task::ConstTagPtr& Task::Active::tag() const //============================================================================== rmf_traffic::Duration Task::Active::estimate_remaining_time() const { - auto remaining_time = _active_phase->estimate_remaining_time(); + 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(); diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp index bc9ac2ec..0cd17c5a 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp @@ -17,6 +17,8 @@ #include +#include "utils.hpp" + namespace rmf_task_sequence { namespace events { @@ -209,22 +211,18 @@ Header GoToPlace::Description::generate_header( const State& initial_state, const Parameters& parameters) const { - const auto fail = [](const std::string& msg) - { - throw std::runtime_error( - "[GoToPlace::Description::generate_header] " + msg); - }; + const std::string& fail_header = "[GoToPlace::Description::generate_header]"; const auto start_wp_opt = initial_state.waypoint(); if (!start_wp_opt) - fail("Initial state is missing a waypoint"); + utils::fail(fail_header, "Initial state is missing a waypoint"); const auto start_wp = *start_wp_opt; const auto& graph = parameters.planner()->get_configuration().graph(); if (graph.num_waypoints() <= start_wp) { - fail("Initial waypoint [" + std::to_string(start_wp) + utils::fail(fail_header, "Initial waypoint [" + std::to_string(start_wp) + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + "]"); } @@ -233,7 +231,7 @@ Header GoToPlace::Description::generate_header( if (graph.num_waypoints() <= _pimpl->destination.waypoint()) { - fail("Destination waypoint [" + utils::fail(fail_header, "Destination waypoint [" + std::to_string(_pimpl->destination.waypoint()) + "] is outside the graph [" + std::to_string(graph.num_waypoints()) + "]"); @@ -246,7 +244,8 @@ Header GoToPlace::Description::generate_header( if (!estimate.has_value()) { - fail("Cannot find a path from " + start_name + " to " + goal_name_); + utils::fail(fail_header, "Cannot find a path from [" + + start_name + "] to [" + goal_name_ + "]"); } return Header( 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..6e272a31 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -0,0 +1,272 @@ +/* + * 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(std::move(invariant_finish_state)), + _invariant_duration(invariant_duration), + _use_tool_sink(use_tool_sink) +{ + _invariant_battery_drain = + parameters.ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_invariant_duration)); + + if (_use_tool_sink && parameters.tool_sink() != nullptr) + { + _invariant_battery_drain += + parameters.tool_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(_invariant_duration)); + } +} + +//============================================================================== +std::optional PerformAction::Model::estimate_finish( + rmf_task::State initial_state, + rmf_traffic::Time earliest_arrival_time, + const Constraints& constraints, + const TravelEstimator& travel_estimator) const +{ + initial_state.time(initial_state.time().value() + _invariant_duration); + initial_state.waypoint(_invariant_finish_state.waypoint().value()); + if (_invariant_finish_state.orientation().has_value()) + initial_state.orientation(_invariant_finish_state.orientation().value()); + + if (constraints.drain_battery()) + initial_state.battery_soc( + initial_state.battery_soc().value() - _invariant_battery_drain); + + if (initial_state.battery_soc().value() <= constraints.threshold_soc()) + return std::nullopt; + + return 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/utils.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp new file mode 100644 index 00000000..3163095d --- /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/test/unit/events/test_PerformAction.cpp b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp new file mode 100644 index 00000000..97037b1c --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp @@ -0,0 +1,107 @@ +/* + * 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, + *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/utils.hpp b/rmf_task_sequence/test/unit/utils.hpp new file mode 100644 index 00000000..ece169fb --- /dev/null +++ b/rmf_task_sequence/test/unit/utils.hpp @@ -0,0 +1,231 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef TEST__UNIT__UTILS_HPP +#define TEST__UNIT__UTILS_HPP + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace { +using namespace std::chrono_literals; +//============================================================================== +void CHECK_CONTACT( + const rmf_task_sequence::detail::ContactCard& contact, + const std::string& name, + const std::string& address, + const std::string& email, + const rmf_task_sequence::detail::ContactCard::PhoneNumber& number) +{ + CHECK(contact.name() == name); + CHECK(contact.address() == address); + CHECK(contact.email() == email); + CHECK(contact.number().country_code == number.country_code); + CHECK(contact.number().number == number.number); +} + +//============================================================================== +std::shared_ptr make_test_constraints( + bool drain_battery = true) +{ + return std::make_shared( + 0.1, + 1.0, + drain_battery); +} + +//============================================================================== +std::shared_ptr make_test_parameters( + bool drain_battery = true) +{ + using BatterySystem = rmf_battery::agv::BatterySystem; + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + using PowerSystem = rmf_battery::agv::PowerSystem; + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + + const std::string test_map_name = "test_map"; + rmf_traffic::agv::Graph graph; + graph.add_waypoint(test_map_name, {-5, -5}).set_passthrough_point(true); // 0 + graph.add_waypoint(test_map_name, { 0, -5}).set_passthrough_point(true); // 1 + graph.add_waypoint(test_map_name, { 5, -5}).set_passthrough_point(true); // 2 + graph.add_waypoint(test_map_name, {10, -5}).set_passthrough_point(true); // 3 + graph.add_waypoint(test_map_name, {-5, 0}); // 4 + graph.add_waypoint(test_map_name, { 0, 0}); // 5 + graph.add_waypoint(test_map_name, { 5, 0}); // 6 + graph.add_waypoint(test_map_name, {10, 0}).set_passthrough_point(true); // 7 + graph.add_waypoint(test_map_name, {10, 4}).set_passthrough_point(true); // 8 + graph.add_waypoint(test_map_name, { 0, 8}).set_passthrough_point(true); // 9 + graph.add_waypoint(test_map_name, { 5, 8}).set_passthrough_point(true); // 10 + graph.add_waypoint(test_map_name, {10, 12}).set_passthrough_point(true); // 11 + graph.add_waypoint(test_map_name, {12, 12}).set_passthrough_point(true); // 12 + REQUIRE(graph.num_waypoints() == 13); + + auto add_bidir_lane = [&](const std::size_t w0, const std::size_t w1) + { + graph.add_lane(w0, w1); + graph.add_lane(w1, w0); + }; + + add_bidir_lane(0, 1); + add_bidir_lane(1, 2); + add_bidir_lane(2, 3); + add_bidir_lane(1, 5); + add_bidir_lane(3, 7); + add_bidir_lane(4, 5); + add_bidir_lane(6, 10); + add_bidir_lane(7, 8); + add_bidir_lane(9, 10); + add_bidir_lane(10, 11); + + const auto shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(1.0); + const rmf_traffic::Profile profile{shape, shape}; + const rmf_traffic::agv::VehicleTraits traits( + {1.0, 0.7}, {0.6, 0.5}, profile); + rmf_traffic::schedule::Database database; + const auto default_planner_options = rmf_traffic::agv::Planner::Options{ + nullptr}; + + auto planner = std::make_shared( + rmf_traffic::agv::Planner::Configuration{graph, traits}, + default_planner_options); + + auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); + REQUIRE(battery_system_optional); + BatterySystem& battery_system = *battery_system_optional; + auto mechanical_system_optional = MechanicalSystem::make(70.0, 40.0, 0.22); + REQUIRE(mechanical_system_optional); + MechanicalSystem& mechanical_system = *mechanical_system_optional; + auto power_system_optional = PowerSystem::make(20.0); + REQUIRE(power_system_optional); + PowerSystem& power_system_processor = *power_system_optional; + + std::shared_ptr motion_sink = + std::make_shared(battery_system, mechanical_system); + std::shared_ptr device_sink = + std::make_shared(battery_system, + power_system_processor); + + return std::make_shared( + planner, + battery_system, + motion_sink, + device_sink); + +} + +//============================================================================== +void CHECK_STATE( + const rmf_task::State& estimated_state, + const rmf_task::State& expected_state) +{ + if (expected_state.waypoint().has_value()) + { + REQUIRE(estimated_state.waypoint().has_value()); + CHECK( + expected_state.waypoint().value() == estimated_state.waypoint().value()); + } + + if (expected_state.orientation().has_value()) + { + REQUIRE(estimated_state.orientation().has_value()); + CHECK(std::abs(expected_state.orientation().value() - + estimated_state.orientation().value()) < 1e-3); + } + + if (expected_state.time().has_value()) + { + REQUIRE(estimated_state.time().has_value()); + CHECK(expected_state.time().value() - + estimated_state.time().value() < 100ms); + } + + if (expected_state.dedicated_charging_waypoint().has_value()) + { + REQUIRE(estimated_state.dedicated_charging_waypoint().has_value()); + CHECK(expected_state.dedicated_charging_waypoint().value() == + estimated_state.dedicated_charging_waypoint().value()); + } + + if (expected_state.battery_soc().has_value()) + { + REQUIRE(estimated_state.battery_soc().has_value()); + CHECK(estimated_state.battery_soc().value() <= + expected_state.battery_soc().value()); + } +} + +//============================================================================== +// TODO(YV): Also check for invariant_finish_state +void CHECK_MODEL( + const rmf_task_sequence::Activity::Model& model, + const rmf_task::State& initial_state, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator, + const rmf_task::State& expected_finish_state) +{ + + const auto estimated_finish = model.estimate_finish( + initial_state, + constraints, + travel_estimator); + + REQUIRE(estimated_finish.has_value()); + + CHECK_STATE(estimated_finish.value(), expected_finish_state); +} + +//============================================================================== +std::optional estimate_travel_duration( + const std::shared_ptr& planner, + const rmf_task::State& initial_state, + const rmf_traffic::agv::Planner::Goal& goal) +{ + const auto result = + planner->setup(initial_state.project_plan_start().value(), goal); + + if (result.disconnected()) + return std::nullopt; + + if (!result.ideal_cost().has_value()) + return std::nullopt; + + return rmf_traffic::time::from_seconds(*result.ideal_cost()); +} + +} // anonymous namespace + +#endif // TEST__UNIT__UTILS_HPP