From 7cde66b68455ed15ee5f5a92fa6b81b4b4d36401 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 16 Jan 2022 07:04:12 +0800 Subject: [PATCH 1/6] Cherry pick diff for PerformAction from new_descriptions Signed-off-by: Yadunund --- rmf_task_sequence/CMakeLists.txt | 10 +- .../events/PerformAction.hpp | 113 ++++++++ .../rmf_task_sequence/events/WaitFor.hpp | 2 +- rmf_task_sequence/package.xml | 2 +- .../rmf_task_sequence/events/GoToPlace.cpp | 17 +- .../events/PerformAction.cpp | 255 ++++++++++++++++++ .../test/unit/events/test_PerformAction.cpp | 101 +++++++ rmf_task_sequence/test/unit/utils.hpp | 231 ++++++++++++++++ 8 files changed, 714 insertions(+), 17 deletions(-) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp create mode 100644 rmf_task_sequence/test/unit/events/test_PerformAction.cpp create mode 100644 rmf_task_sequence/test/unit/utils.hpp diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index 8decc4d7..fbe29d99 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,9 +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} @@ -71,7 +70,7 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - rmf_uncrustify( + ament_uncrustify( ARGN include src test CONFIG_FILE ${uncrustify_config_file} MAX_LINE_LENGTH 80 @@ -136,4 +135,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..9d37d1d6 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.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__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] action + /// 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( + nlohmann::json action, + rmf_traffic::Duration action_duration_estimate, + bool use_tool_sink, + std::optional expected_finish_location = std::nullopt); + + /// Get the action + const nlohmann::json& action() const; + + /// Set the action + Description& action(const nlohmann::json& new_action); + + /// 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/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..5053fb1f --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -0,0 +1,255 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "utils.hpp" + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class PerformAction::Model : public Activity::Model +{ +public: + + Model( + rmf_task::State invariant_finish_state, + rmf_traffic::Duration invariant_duration, + bool use_tool_sink, + const Parameters& parameters); + + std::optional estimate_finish( + rmf_task::State initial_state, + 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: + + nlohmann::json action; + rmf_traffic::Duration action_duration_estimate; + bool use_tool_sink; + std::optional expected_finish_location; +}; + +//============================================================================== +auto PerformAction::Description::make( + 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(action), + std::move(duration), + std::move(use_tool_sink), + std::move(location) + }); + + return description; +} + +//============================================================================== +PerformAction::Description::Description() +{ + // Do nothing +} + +//============================================================================== +const nlohmann::json& +PerformAction::Description::action() const +{ + return _pimpl->action; +} + +//============================================================================== +auto PerformAction::Description::action( + const nlohmann::json& new_action) -> Description& +{ + _pimpl->action = new_action; + return *this; +} + +//============================================================================== +const rmf_traffic::Duration& +PerformAction::Description::action_duration_estimate() const +{ + return _pimpl->action_duration_estimate; +} + +//============================================================================== +auto PerformAction::Description::action_duration_estimate( + rmf_traffic::Duration new_duration) -> Description& +{ + _pimpl->action_duration_estimate = std::move(new_duration); + return *this; +} + +//============================================================================== +bool PerformAction::Description::use_tool_sink() const +{ + return _pimpl->use_tool_sink; +} + +//============================================================================== +auto PerformAction::Description::use_tool_sink( + bool use_tool) -> Description& +{ + _pimpl->use_tool_sink = std::move(use_tool); + return *this; +} + +//============================================================================== +auto PerformAction::Description::expected_finish_location() const +-> std::optional +{ + return _pimpl->expected_finish_location; +} + +//============================================================================== +auto PerformAction::Description::expected_finish_location( + std::optional new_location) -> Description& +{ + _pimpl->expected_finish_location = std::move(new_location); + return *this; +} + +//============================================================================== +Activity::ConstModelPtr PerformAction::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + auto invariant_finish_state = invariant_initial_state; + if (_pimpl->expected_finish_location.has_value()) + { + const auto& goal = _pimpl->expected_finish_location.value(); + invariant_finish_state.waypoint(goal.waypoint()); + if (goal.orientation() != nullptr) + { + invariant_finish_state.orientation(*goal.orientation()); + } + } + + return std::make_shared( + invariant_finish_state, + _pimpl->action_duration_estimate, + _pimpl->use_tool_sink, + parameters); +} + +//============================================================================== +Header PerformAction::Description::generate_header( + const rmf_task::State& initial_state, + const Parameters& parameters) const +{ + + const std::string& error_header = + "[PerformAction::Description::generate_header]"; + + const auto start_wp_opt = initial_state.waypoint(); + if (!start_wp_opt) + utils::fail( + error_header, + "Initial state is missing a waypoint"); + + const auto start_wp = *start_wp_opt; + const auto start_name = utils::waypoint_name(start_wp, parameters); + + return Header( + "Perform action", + "Performing action " + _pimpl->action.dump() + + " at waypoint [" + start_name + "]", + _pimpl->action_duration_estimate); + +} + +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/test/unit/events/test_PerformAction.cpp b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp new file mode 100644 index 00000000..379f24bc --- /dev/null +++ b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include "../utils.hpp" + +using namespace std::chrono_literals; + +SCENARIO("Test PerformAction") +{ + using PerformAction = rmf_task_sequence::events::PerformAction; + const auto duration = 10s; + const std::string action_name = "test_action"; + const rmf_traffic::agv::Planner::Goal finish_location{1}; + + auto description = PerformAction::Description::make( + action_name, + duration, + false, + finish_location); + + const auto parameters = make_test_parameters(); + const auto constraints = make_test_constraints(); + const auto now = std::chrono::steady_clock::now(); + rmf_task::State initial_state; + initial_state.waypoint(0) + .orientation(0.0) + .time(now) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + + WHEN("Testing getters") + { + CHECK(description->action_name() == action_name); + CHECK(description->action_duration_estimate() == duration); + CHECK(description->use_tool_sink() == false); + REQUIRE(description->expected_finish_location().has_value()); + CHECK(description->expected_finish_location().value().waypoint() == 1); + } + + WHEN("Testing setters") + { + description->action_name("new_name"); + CHECK(description->action_name() == "new_name"); + description->action_duration_estimate(20s); + CHECK(description->action_duration_estimate() == 20s); + description->use_tool_sink(true); + CHECK(description->use_tool_sink() == true); + description->expected_finish_location(std::nullopt); + CHECK_FALSE(description->expected_finish_location().has_value()); + } + + WHEN("Testing model") + { + // TODO(YV): Test model for cases where state is missing some parameters + const auto model = description->make_model( + initial_state, + *parameters); + const auto travel_estimator = rmf_task::TravelEstimator(*parameters); + + rmf_task::State expected_finish_state = initial_state; + REQUIRE(initial_state.time().has_value()); + expected_finish_state.time(initial_state.time().value() + duration) + .waypoint(1); + + CHECK_MODEL( + *model, + initial_state, + *constraints, + travel_estimator, + expected_finish_state); + } + + WHEN("Testing header") + { + const auto header = description->generate_header( + initial_state, + *parameters); + + CHECK(!header.category().empty()); + CHECK(!header.detail().empty()); + CHECK(header.original_duration_estimate() == duration); + } +} diff --git a/rmf_task_sequence/test/unit/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 From b190a4c84fae527fa8e08ea319c2aaf05b73b78b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 16 Jan 2022 07:07:02 +0800 Subject: [PATCH 2/6] Add missing utils Signed-off-by: Yadunund --- .../src/rmf_task_sequence/events/utils.cpp | 40 +++++++++++++++++ .../src/rmf_task_sequence/events/utils.hpp | 43 +++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp new file mode 100644 index 00000000..6bbda90f --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "utils.hpp" + +namespace rmf_task_sequence { +namespace events { +namespace utils { + +std::string waypoint_name( + const std::size_t index, + const rmf_task::Parameters& parameters) +{ + const auto& graph = parameters.planner()->get_configuration().graph(); + if (index < graph.num_waypoints()) + { + if (const auto* name = graph.get_waypoint(index).name()) + return *name; + } + + return "#" + std::to_string(index); +} + +} // namespace utils +} // namespace events +} // namespace rmf_task_sequence diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp new file mode 100644 index 00000000..3e80ff7e --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp @@ -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. + * +*/ + +#ifndef SRC__RMF_TASK_SEQUENCE__EVENTS__UTILS_HPP +#define SRC__RMF_TASK_SEQUENCE__EVENTS__UTILS_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { +namespace utils { + +std::string waypoint_name( + const std::size_t index, + const rmf_task::Parameters& parameters); + +const auto fail = [](const std::string& header, const std::string& msg) + { + throw std::runtime_error( + header + " " + msg); + }; + +} // namespace utils +} // namespace events +} // namespace rmf_task_sequence + +# endif // SRC__RMF_TASK_SEQUENCE__EVENTS__UTILS_HPP From b571cb277a1ba08e6180ec49537f46b637e8b611 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 17 Jan 2022 17:24:38 +0800 Subject: [PATCH 3/6] Add category to PerformAction make Signed-off-by: Yadunund --- rmf_task_sequence/CMakeLists.txt | 1 - .../events/PerformAction.hpp | 22 +++++++++---- .../events/PerformAction.cpp | 33 ++++++++++++++----- .../test/unit/events/test_PerformAction.cpp | 16 ++++++--- 4 files changed, 52 insertions(+), 20 deletions(-) diff --git a/rmf_task_sequence/CMakeLists.txt b/rmf_task_sequence/CMakeLists.txt index fbe29d99..d9c819cd 100644 --- a/rmf_task_sequence/CMakeLists.txt +++ b/rmf_task_sequence/CMakeLists.txt @@ -53,7 +53,6 @@ target_include_directories(rmf_task_sequence 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) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp index 9d37d1d6..b4c5b324 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp @@ -47,7 +47,10 @@ class PerformAction::Description : public Event::Description /// Make a PerformAction description. /// - /// \param[in] action + /// \param[in] category + /// A category for this action + /// + /// \param[in] description /// A json description of the action to perform /// /// \param[in] action_duration_estimate @@ -62,16 +65,23 @@ class PerformAction::Description : public Event::Description /// performing the action. Use nullopt to indicate that after performing /// the action, the robot will be at its initial location. static DescriptionPtr make( - nlohmann::json action, + 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 action - const nlohmann::json& action() const; + /// 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 action - Description& action(const nlohmann::json& new_action); + /// Set the description + Description& description(const nlohmann::json& new_description); /// Get the action duration estimate const rmf_traffic::Duration& action_duration_estimate() const; diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp index 5053fb1f..653a6d96 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -110,8 +110,8 @@ State PerformAction::Model::invariant_finish_state() const class PerformAction::Description::Implementation { public: - - nlohmann::json action; + std::string category; + nlohmann::json description; rmf_traffic::Duration action_duration_estimate; bool use_tool_sink; std::optional expected_finish_location; @@ -119,6 +119,7 @@ class PerformAction::Description::Implementation //============================================================================== auto PerformAction::Description::make( + const std::string& category, nlohmann::json action, rmf_traffic::Duration duration, bool use_tool_sink, @@ -128,6 +129,7 @@ auto PerformAction::Description::make( description->_pimpl = rmf_utils::make_impl( Implementation { + std::move(category), std::move(action), std::move(duration), std::move(use_tool_sink), @@ -143,18 +145,33 @@ 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::action() const +PerformAction::Description::description() const { - return _pimpl->action; + return _pimpl->description; } //============================================================================== -auto PerformAction::Description::action( - const nlohmann::json& new_action) -> Description& +auto PerformAction::Description::description( + const nlohmann::json& new_description) -> Description& { - _pimpl->action = new_action; + _pimpl->description = new_description; return *this; } @@ -245,7 +262,7 @@ Header PerformAction::Description::generate_header( return Header( "Perform action", - "Performing action " + _pimpl->action.dump() + + "Performing action " + _pimpl->category + " at waypoint [" + start_name + "]", _pimpl->action_duration_estimate); diff --git a/rmf_task_sequence/test/unit/events/test_PerformAction.cpp b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp index 379f24bc..4122f92e 100644 --- a/rmf_task_sequence/test/unit/events/test_PerformAction.cpp +++ b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp @@ -27,11 +27,13 @@ SCENARIO("Test PerformAction") { using PerformAction = rmf_task_sequence::events::PerformAction; const auto duration = 10s; - const std::string action_name = "test_action"; + const std::string category = "test_action"; + const nlohmann::json desc; const rmf_traffic::agv::Planner::Goal finish_location{1}; auto description = PerformAction::Description::make( - action_name, + category, + desc duration, false, finish_location); @@ -48,7 +50,8 @@ SCENARIO("Test PerformAction") WHEN("Testing getters") { - CHECK(description->action_name() == action_name); + 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()); @@ -57,8 +60,11 @@ SCENARIO("Test PerformAction") WHEN("Testing setters") { - description->action_name("new_name"); - CHECK(description->action_name() == "new_name"); + 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); From 77e7ba7a1437d49a2c616884e8859f9b62c26f27 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 17 Jan 2022 23:11:17 +0800 Subject: [PATCH 4/6] Fix bug in estimate_remaining_time() Signed-off-by: Yadunund --- rmf_task_sequence/src/rmf_task_sequence/Task.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/Task.cpp b/rmf_task_sequence/src/rmf_task_sequence/Task.cpp index 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(); From 28711635614845ad6c4b0838cdececf8132be1df Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 20 Jan 2022 16:45:33 +0800 Subject: [PATCH 5/6] Update utils Signed-off-by: Yadunund --- .../events/PerformAction.hpp | 2 +- .../events/PerformAction.cpp | 4 ++-- .../src/rmf_task_sequence/events/utils.cpp | 21 ++++++++++++------- .../src/rmf_task_sequence/events/utils.hpp | 9 ++++---- 4 files changed, 20 insertions(+), 16 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp index b4c5b324..db1a2758 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp index 653a6d96..6e272a31 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/PerformAction.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -200,7 +200,7 @@ bool PerformAction::Description::use_tool_sink() const auto PerformAction::Description::use_tool_sink( bool use_tool) -> Description& { - _pimpl->use_tool_sink = std::move(use_tool); + _pimpl->use_tool_sink = use_tool; return *this; } diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp index 6bbda90f..0caa54b6 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp @@ -17,24 +17,29 @@ #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) { - const auto& graph = parameters.planner()->get_configuration().graph(); - if (index < graph.num_waypoints()) - { - if (const auto* name = graph.get_waypoint(index).name()) - return *name; - } - - return "#" + std::to_string(index); + 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 index 3e80ff7e..4f66ea0d 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp @@ -26,15 +26,14 @@ namespace rmf_task_sequence { namespace events { namespace utils { +//============================================================================== std::string waypoint_name( const std::size_t index, const rmf_task::Parameters& parameters); -const auto fail = [](const std::string& header, const std::string& msg) - { - throw std::runtime_error( - header + " " + msg); - }; +//============================================================================== +void fail(const std::string& header, const std::string& msg); + } // namespace utils } // namespace events From 66aa1ae4b08300533e42eef186297c20199b71b3 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 20 Jan 2022 16:48:36 +0800 Subject: [PATCH 6/6] Update copyright Signed-off-by: Yadunund --- rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp | 2 +- rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp | 2 +- rmf_task_sequence/test/unit/events/test_PerformAction.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp index 0caa54b6..3163095d 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp index 4f66ea0d..796b4754 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/utils.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/rmf_task_sequence/test/unit/events/test_PerformAction.cpp b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp index 4122f92e..97037b1c 100644 --- a/rmf_task_sequence/test/unit/events/test_PerformAction.cpp +++ b/rmf_task_sequence/test/unit/events/test_PerformAction.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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.