From 82549c83384358dbbbd6d4d042e004e684cfe17b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 3 Aug 2020 14:41:26 +0800 Subject: [PATCH 001/128] Initial commit Signed-off-by: Yadunund --- rmf_battery/CMakeLists.txt | 54 +++++++ .../include/rmf_battery/agv/SystemTraits.hpp | 141 ++++++++++++++++++ rmf_battery/package.xml | 18 +++ 3 files changed, 213 insertions(+) create mode 100644 rmf_battery/CMakeLists.txt create mode 100644 rmf_battery/include/rmf_battery/agv/SystemTraits.hpp create mode 100644 rmf_battery/package.xml diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt new file mode 100644 index 000000000..4edcde015 --- /dev/null +++ b/rmf_battery/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.5) +project(rmf_battery) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include(GNUInstallDirs) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rmf_utils REQUIRED) + + +# ===== Battery modelling library +file(GLOB_RECURSE core_lib_srcs "src/rmf_battery/*.cpp") +add_library(rmf_battery SHARED + ${core_lib_srcs} +) + +target_link_libraries(rmf_battery + PUBLIC + rmf_utils::rmf_utils +) + +target_include_directories(rmf_battery + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) + +ament_export_interfaces(rmf_battery HAS_LIBRARY_TARGET) +ament_export_dependencies(rmf_utils) + +install( + DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install( + TARGETS rmf_battery + EXPORT rmf_battery + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +ament_package() diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp new file mode 100644 index 000000000..800c3e39d --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -0,0 +1,141 @@ +/* + * 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_BATTERY__AGV__SYSTEMTRAITS_HPP +#define RMF_BATTERY__AGV__SYSTEMTRAITS_HPP + +namespace rmf_battery { +namespace agv { + +//============================================================================== +class SystemTraits +{ +public: + class PowerSystem; + using PowerSystems = std::vector; + + class PowerSystem + { + public: + + PowerSystem( + double nominal_power, + double nominal_voltage, + double efficiency = 1.0); + + PowerSystem& set_nominal_power(double nom_power); + double get_nominal_power() const; + + PowerSystem& set_nominal_voltage(double nom_voltage); + double get_nominal_voltage() const; + + PowerSystem& set_efficiency(double efficiency); + double get_efficiency() const; + + /// Returns true if the values of these limits are valid, i.e. greater than + /// zero. + bool valid() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + class BatterySystem + { + public: + enum class BatteryType : uint16_t + { + /// The vehicle is powered by a Lead-Acid battery + LeadAcid, + /// The vehicle is powered by a Lithium-Ion battery + LiIon, + }; + + BatterySystem( + double nominal_voltage, + double nominal_capacity, + double nominal_current, + BatteryType type = BatteryType::LeadAcid; + rmf_utils::optional resistance = rmf_utils::nullopt, + rmf_utils::optional max_voltage = rmf_utils::nullopt, + rmf_utils::optional exp_voltage = rmf_utils::nullopt, + rmf_utils::optional exp_capacity = rmf_utils::nullopt) + + + bool valid() const; + + }; + + class MechanicalSystem + { + public: + + MechanicalSystem( + double mass, + double static_friction, + double dynamic_friction, + double drag_coefficient, + double frontal_area); + + Differential& set_forward(Eigen::Vector2d forward); + + const Eigen::Vector2d& get_forward() const; + + Differential& set_reversible(bool reversible); + bool is_reversible() const; + + /// Returns true if the length of the forward vector is not too close to + /// zero. If it is too close to zero, then the direction of the forward + /// vector cannot be reliably interpreted. Ideally the forward vector should + /// have unit length. + bool valid() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + /// Constructor. + SystemTraits( + MechanicalSystem mechanical_system, + BatterySystem battery_system, + Powersystems power_systems); + + MechanicalSystem& mechanical_system(); + const MechanicalSystem& mechanical_system() const; + + BatterySystem& mechanical_system(); + const BatterySystem& mechanical_system() const; + + Powersystems& mechanical_system(); + const Powersystems& mechanical_system() const; + + /// Returns true if the values of the traits are valid. For example, this + /// means that all power and voltage values are greater than zero. + bool valid() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; + +}; + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__AGV__SYSTEMTRAITS_HPP diff --git a/rmf_battery/package.xml b/rmf_battery/package.xml new file mode 100644 index 000000000..c61d4003a --- /dev/null +++ b/rmf_battery/package.xml @@ -0,0 +1,18 @@ + + + + rmf_battery + 0.0.0 + TODO: Package description + yadu + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From b221cc134ea84a73c1fd22de8a663ab1105e2e3b Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 11 Aug 2020 17:41:33 +0800 Subject: [PATCH 002/128] Added unit tests for PowerSystem --- rmf_battery/CMakeLists.txt | 27 ++++++ .../include/rmf_battery/agv/SystemTraits.hpp | 96 +++++++++++++------ rmf_battery/package.xml | 15 ++- .../src/rmf_battery/agv/SystemTraits.cpp | 84 ++++++++++++++++ rmf_battery/test/main.cpp | 21 ++++ .../test/unit/agv/test_SystemTraits.cpp | 49 ++++++++++ 6 files changed, 256 insertions(+), 36 deletions(-) create mode 100644 rmf_battery/src/rmf_battery/agv/SystemTraits.cpp create mode 100644 rmf_battery/test/main.cpp create mode 100644 rmf_battery/test/unit/agv/test_SystemTraits.cpp diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 4edcde015..26a9d28fe 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -23,6 +23,33 @@ add_library(rmf_battery SHARED ${core_lib_srcs} ) +if(BUILD_TESTING) + find_package(ament_cmake_catch2 REQUIRED) + + file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + + ament_add_catch2( + test_rmf_battery test/main.cpp ${unit_test_srcs} + TIMEOUT 300) + target_link_libraries(test_rmf_battery + rmf_battery + ) + + target_include_directories(test_rmf_battery + PRIVATE + $ + ) + + find_package(rmf_cmake_uncrustify REQUIRED) + find_file(uncrustify_config_file NAMES "share/format/rmf_code_style.cfg") + + rmf_uncrustify( + ARGN include src test + CONFIG_FILE ${uncrustify_config_file} + MAX_LINE_LENGTH 80 + ) +endif() + target_link_libraries(rmf_battery PUBLIC rmf_utils::rmf_utils diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp index 800c3e39d..94fca7f4c 100644 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -18,6 +18,10 @@ #ifndef RMF_BATTERY__AGV__SYSTEMTRAITS_HPP #define RMF_BATTERY__AGV__SYSTEMTRAITS_HPP +#include +#include +#include + namespace rmf_battery { namespace agv { @@ -37,17 +41,16 @@ class SystemTraits double nominal_voltage, double efficiency = 1.0); - PowerSystem& set_nominal_power(double nom_power); - double get_nominal_power() const; + PowerSystem& nominal_power(double nom_power); + double nominal_power() const; - PowerSystem& set_nominal_voltage(double nom_voltage); - double get_nominal_voltage() const; + PowerSystem& nominal_voltage(double nom_voltage); + double nominal_voltage() const; - PowerSystem& set_efficiency(double efficiency); - double get_efficiency() const; + PowerSystem& efficiency(double efficiency); + double efficiency() const; - /// Returns true if the values of these limits are valid, i.e. greater than - /// zero. + /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; class Implementation; @@ -66,19 +69,60 @@ class SystemTraits LiIon, }; + class BatteryProfile + { + public: + BatteryProfile( + double resistance, + double max_voltage, + double exp_voltage, + double exp_capacity); + + BatteryProfile& resistance(double r); + double resistance() const; + + BatteryProfile& max_voltage(double max_voltage); + double max_voltage() const; + + BatteryProfile& exp_voltage(double exp_voltage); + double exp_voltage() const; + + BatteryProfile& exp_capacity(double exp_capacity); + double exp_capacity() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + BatterySystem( double nominal_voltage, double nominal_capacity, - double nominal_current, - BatteryType type = BatteryType::LeadAcid; - rmf_utils::optional resistance = rmf_utils::nullopt, - rmf_utils::optional max_voltage = rmf_utils::nullopt, - rmf_utils::optional exp_voltage = rmf_utils::nullopt, - rmf_utils::optional exp_capacity = rmf_utils::nullopt) + double charging_current, + BatteryType type = BatteryType::LeadAcid, + rmf_utils::optional profile = rmf_utils::nullopt); + + BatterySystem& nominal_voltage(double nominal_voltage); + double nominal_voltage() const; + + BatterySystem& nominal_capacity(double nominal_capacity); + double nominal_capacity() const; + + BatterySystem& charging_current(double charging_current); + double charging_current() const; + BatterySystem& type(uint16_t type); + uint16_t type() const; + BatterySystem& profile(rmf_utils::optional profile); + rmf_utils::optional profile() const; + + /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; }; class MechanicalSystem @@ -87,22 +131,12 @@ class SystemTraits MechanicalSystem( double mass, - double static_friction, double dynamic_friction, double drag_coefficient, double frontal_area); - Differential& set_forward(Eigen::Vector2d forward); - - const Eigen::Vector2d& get_forward() const; - - Differential& set_reversible(bool reversible); - bool is_reversible() const; - /// Returns true if the length of the forward vector is not too close to - /// zero. If it is too close to zero, then the direction of the forward - /// vector cannot be reliably interpreted. Ideally the forward vector should - /// have unit length. + /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; class Implementation; @@ -114,19 +148,19 @@ class SystemTraits SystemTraits( MechanicalSystem mechanical_system, BatterySystem battery_system, - Powersystems power_systems); + PowerSystems power_systems); MechanicalSystem& mechanical_system(); const MechanicalSystem& mechanical_system() const; - BatterySystem& mechanical_system(); - const BatterySystem& mechanical_system() const; + BatterySystem& battery_system(); + const BatterySystem& battery_system() const; - Powersystems& mechanical_system(); - const Powersystems& mechanical_system() const; + PowerSystems& power_systems(); + const PowerSystems& power_systems() const; /// Returns true if the values of the traits are valid. For example, this - /// means that all power and voltage values are greater than zero. + /// means that all system values are greater than zero. bool valid() const; class Implementation; diff --git a/rmf_battery/package.xml b/rmf_battery/package.xml index c61d4003a..d4ec6c7d4 100644 --- a/rmf_battery/package.xml +++ b/rmf_battery/package.xml @@ -2,15 +2,20 @@ rmf_battery - 0.0.0 - TODO: Package description + 1.0.0 + Package for modelling battery life of robots yadu - TODO: License declaration + Apache License 2.0 ament_cmake - ament_lint_auto - ament_lint_common + rmf_utils + rmf_utils + + ament_cmake_catch2 + rmf_cmake_uncrustify + + eigen ament_cmake diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp new file mode 100644 index 000000000..f0e973f8a --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -0,0 +1,84 @@ +/* + * 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 + +namespace rmf_battery { +namespace agv { + +class SystemTraits::PowerSystem::Implementation +{ +public: + + double nominal_power; + double nominal_voltage; + double efficiency; +}; + +//============================================================================== +SystemTraits::PowerSystem::PowerSystem( + const double nominal_power, + const double nominal_voltage, + const double efficiency) +: _pimpl(rmf_utils::make_impl( + Implementation{nominal_power, nominal_voltage, efficiency})) +{ + // Do nothing +} + +//============================================================================== +auto SystemTraits::PowerSystem::nominal_power(double nom_power) ->PowerSystem& +{ + _pimpl->nominal_power = nom_power; + return *this; +} + +//============================================================================== +double SystemTraits::PowerSystem::nominal_power() const +{ + return _pimpl->nominal_power; +} + +//============================================================================== +auto SystemTraits::PowerSystem::nominal_voltage(double nom_voltage) +->PowerSystem& +{ + _pimpl->nominal_voltage = nom_voltage; + return *this; +} + +//============================================================================== +double SystemTraits::PowerSystem::nominal_voltage() const +{ + return _pimpl->nominal_voltage; +} +//============================================================================== +auto SystemTraits::PowerSystem::efficiency(double efficiency) +->PowerSystem& +{ + _pimpl->efficiency = efficiency; + return *this; +} + +//============================================================================== +double SystemTraits::PowerSystem::efficiency() const +{ + return _pimpl->efficiency; +} + +} // namespace agv +} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/test/main.cpp b/rmf_battery/test/main.cpp new file mode 100644 index 000000000..23e837acb --- /dev/null +++ b/rmf_battery/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_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp new file mode 100644 index 000000000..140557f58 --- /dev/null +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +SCENARIO("Test PowerSystem") +{ + rmf_battery::agv::SystemTraits::PowerSystem power_system(60, 12); + REQUIRE(power_system.nominal_power() - 60 == Approx(0.0)); + REQUIRE(power_system.nominal_voltage() - 12 == Approx(0.0)); + REQUIRE(power_system.efficiency() - 1.0 == Approx(0.0)); + WHEN("Nominal power is set") + { + power_system.nominal_power(80); + CHECK(power_system.nominal_power() - 80 == Approx(0.0)); + CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); + CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); + } + WHEN("Nominal voltage is set") + { + power_system.nominal_voltage(24); + CHECK(power_system.nominal_voltage() - 24 == Approx(0.0)); + CHECK(power_system.nominal_power() - 60 == Approx(0.0)); + CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); + } + WHEN("Efficiency is set") + { + power_system.efficiency(0.80); + CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); + CHECK(power_system.nominal_power() - 60 == Approx(0.0)); + CHECK(power_system.efficiency() - 0.80 == Approx(0.0)); + } +} \ No newline at end of file From f434cb075546c2119d7b0fb0e9b281641f522362 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 12 Aug 2020 11:30:01 +0800 Subject: [PATCH 003/128] Completed implementation of SystemTraits Signed-off-by: Yadunund Vijay --- .../include/rmf_battery/agv/SystemTraits.hpp | 34 +- .../src/rmf_battery/agv/SystemTraits.cpp | 375 ++++++++++++++++++ .../test/unit/agv/test_SystemTraits.cpp | 9 + 3 files changed, 408 insertions(+), 10 deletions(-) diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp index 94fca7f4c..91c560b94 100644 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -78,7 +78,7 @@ class SystemTraits double exp_voltage, double exp_capacity); - BatteryProfile& resistance(double r); + BatteryProfile& resistance(double resistance); double resistance() const; BatteryProfile& max_voltage(double max_voltage); @@ -89,6 +89,9 @@ class SystemTraits BatteryProfile& exp_capacity(double exp_capacity); double exp_capacity() const; + + /// Returns true if the values are valid, i.e. greater than zero. + bool valid() const; class Implementation; private: @@ -111,8 +114,8 @@ class SystemTraits BatterySystem& charging_current(double charging_current); double charging_current() const; - BatterySystem& type(uint16_t type); - uint16_t type() const; + BatterySystem& type(BatteryType type); + BatteryType type() const; BatterySystem& profile(rmf_utils::optional profile); rmf_utils::optional profile() const; @@ -131,10 +134,21 @@ class SystemTraits MechanicalSystem( double mass, - double dynamic_friction, + double friction_coefficient, double drag_coefficient, double frontal_area); + MechanicalSystem& mass(double mass); + double mass() const; + + MechanicalSystem& friction_coefficient(double friction_coeff); + double friction_coefficient() const; + + MechanicalSystem& drag_coefficient(double drag_coeff); + double drag_coefficient() const; + + MechanicalSystem& frontal_area(double frontal_area); + double frontal_area() const; /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; @@ -150,14 +164,14 @@ class SystemTraits BatterySystem battery_system, PowerSystems power_systems); - MechanicalSystem& mechanical_system(); - const MechanicalSystem& mechanical_system() const; + SystemTraits& mechanical_system(MechanicalSystem mechanical_system); + const MechanicalSystem mechanical_system() const; - BatterySystem& battery_system(); - const BatterySystem& battery_system() const; + SystemTraits& battery_system(BatterySystem battery_system); + const BatterySystem battery_system() const; - PowerSystems& power_systems(); - const PowerSystems& power_systems() const; + SystemTraits& power_systems(PowerSystems power_systems); + const PowerSystems power_systems() const; /// Returns true if the values of the traits are valid. For example, this /// means that all system values are greater than zero. diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index f0e973f8a..4af797065 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -20,6 +20,9 @@ namespace rmf_battery { namespace agv { +using BatteryProfile = SystemTraits::BatterySystem::BatteryProfile; +using BatteryType = SystemTraits::BatterySystem::BatteryType; + class SystemTraits::PowerSystem::Implementation { public: @@ -80,5 +83,377 @@ double SystemTraits::PowerSystem::efficiency() const return _pimpl->efficiency; } +//============================================================================== +bool SystemTraits::PowerSystem::valid() const +{ + return _pimpl->nominal_power > 0.0 && _pimpl->nominal_voltage > 0.0 && + _pimpl->efficiency > 0.0; +} + +//============================================================================== +class SystemTraits::BatterySystem::BatteryProfile::Implementation +{ +public: + double resistance; + double max_voltage; + double exp_voltage; + double exp_capacity; +}; + +//============================================================================== +SystemTraits::BatterySystem::BatteryProfile::BatteryProfile( + const double resistance, + const double max_voltage, + const double exp_voltage, + const double exp_capacity) +: _pimpl(rmf_utils::make_impl( + Implementation{resistance, max_voltage, exp_voltage, exp_capacity})) +{ + // Do nothing +} + +//============================================================================== +auto BatteryProfile::resistance(double resistance) +-> BatteryProfile& +{ + _pimpl->resistance = resistance; + return *this; +} + +//============================================================================== +double BatteryProfile::resistance() const +{ + return _pimpl->resistance; +} + +//============================================================================== +auto BatteryProfile::max_voltage(double max_voltage) +-> BatteryProfile& +{ + _pimpl->max_voltage = max_voltage; + return *this; +} + +//============================================================================== +double BatteryProfile::max_voltage() const +{ + return _pimpl->max_voltage; +} + +//============================================================================== +auto BatteryProfile::exp_voltage(double exp_voltage) +-> BatteryProfile& +{ + _pimpl->exp_voltage = exp_voltage; + return *this; +} + +//============================================================================== +double BatteryProfile::exp_voltage() const +{ + return _pimpl->exp_voltage; +} + +//============================================================================== +auto BatteryProfile::exp_capacity(double exp_capacity) +-> BatteryProfile& +{ + _pimpl->exp_capacity = exp_capacity; + return *this; +} + +//============================================================================== +double BatteryProfile::exp_capacity() const +{ + return _pimpl->exp_capacity; +} + +//============================================================================== +bool BatteryProfile::valid() const +{ + return _pimpl->resistance > 0.0 && _pimpl->max_voltage > 0.0 && + _pimpl->exp_voltage > 0.0 && _pimpl->exp_capacity > 0.0; +} + +//============================================================================== +class SystemTraits::BatterySystem::Implementation +{ +public: + double nominal_voltage; + double nominal_capacity; + double charging_current; + BatteryType type; + rmf_utils::optional profile; +}; + +//============================================================================== +SystemTraits::BatterySystem::BatterySystem( + const double nominal_voltage, + const double nominal_capacity, + const double charging_current, + BatteryType type, + rmf_utils::optional profile) +: _pimpl(rmf_utils::make_impl( + Implementation{ + nominal_voltage, + nominal_capacity, + charging_current, + type, + std::move(profile) + })) +{ + // Do nothing +} + +//============================================================================== +auto SystemTraits::BatterySystem::nominal_voltage(double nom_voltage) +-> BatterySystem& +{ + _pimpl->nominal_voltage = nom_voltage; + return *this; +} + +//============================================================================== +double SystemTraits::BatterySystem::nominal_voltage() const +{ + return _pimpl->nominal_voltage; +} + +//============================================================================== +auto SystemTraits::BatterySystem::nominal_capacity(double nom_capacity) +-> BatterySystem& +{ + _pimpl->nominal_capacity = nom_capacity; + return *this; +} + +//============================================================================== +double SystemTraits::BatterySystem::nominal_capacity() const +{ + return _pimpl->nominal_capacity; +} + +//============================================================================== +auto SystemTraits::BatterySystem::charging_current(double charging_current) +-> BatterySystem& +{ + _pimpl->charging_current = charging_current; + return *this; +} + +//============================================================================== +double SystemTraits::BatterySystem::charging_current() const +{ + return _pimpl->charging_current; +} + +//============================================================================== +auto SystemTraits::BatterySystem::type(BatteryType type) +-> BatterySystem& +{ + _pimpl->type = type; + return *this; +} + +//============================================================================== +BatteryType SystemTraits::BatterySystem::type() const +{ + return _pimpl->type; +} + +//============================================================================== +auto SystemTraits::BatterySystem::profile( + rmf_utils::optional profile) +-> BatterySystem& +{ + _pimpl->profile = std::move(profile); + return *this; +} + +//============================================================================== +rmf_utils::optional SystemTraits::BatterySystem::profile() const +{ + return _pimpl->profile; +} + +//============================================================================== +bool SystemTraits::BatterySystem::valid() const +{ + bool valid = _pimpl->nominal_voltage > 0.0 && + _pimpl->nominal_capacity > 0.0 && _pimpl->charging_current > 0.0 && + (_pimpl->type == BatteryType::LeadAcid + || _pimpl->type == BatteryType::LiIon); + if (_pimpl->profile) + return valid && _pimpl->profile->valid(); + + return valid; + +} + +//============================================================================== +class SystemTraits::MechanicalSystem::Implementation +{ +public: + double mass; + double friction_coefficient; + double drag_coefficient; + double frontal_area; +}; + +//============================================================================== +SystemTraits::MechanicalSystem::MechanicalSystem( + const double mass, + const double friction_coefficient, + const double drag_coefficient, + const double frontal_area) +: _pimpl(rmf_utils::make_impl( + Implementation{ + mass, + friction_coefficient, + drag_coefficient, + frontal_area + })) +{ + // Do nothing +} + +//============================================================================== +auto SystemTraits::MechanicalSystem::mass(double mass) -> MechanicalSystem& +{ + _pimpl->mass = mass; + return *this; +} + +//============================================================================== +double SystemTraits::MechanicalSystem::mass() const +{ + return _pimpl->mass; +} + +//============================================================================== +auto SystemTraits::MechanicalSystem::friction_coefficient(double friction_coeff) +-> MechanicalSystem& +{ + _pimpl->friction_coefficient = friction_coeff; + return *this; +} + +//============================================================================== +double SystemTraits::MechanicalSystem::friction_coefficient() const +{ + return _pimpl->friction_coefficient; +} + +//============================================================================== +auto SystemTraits::MechanicalSystem::drag_coefficient(double drag_coeff) +-> MechanicalSystem& +{ + _pimpl->drag_coefficient = drag_coeff; + return *this; +} + +//============================================================================== +double SystemTraits::MechanicalSystem::drag_coefficient() const +{ + return _pimpl->drag_coefficient; +} + +//============================================================================== +auto SystemTraits::MechanicalSystem::frontal_area(double frontal_area) +-> MechanicalSystem& +{ + _pimpl->frontal_area = frontal_area; + return *this; +} + +//============================================================================== +double SystemTraits::MechanicalSystem::frontal_area() const +{ + return _pimpl->frontal_area; +} + +//============================================================================== +bool SystemTraits::MechanicalSystem::valid() const +{ + return _pimpl->mass > 0.0 && _pimpl->friction_coefficient > 0.0 && + _pimpl->drag_coefficient > 0.0 && _pimpl->frontal_area > 0.0; +} + +//============================================================================== +class SystemTraits::Implementation +{ +public: + MechanicalSystem mechanical_system; + BatterySystem battery_system; + PowerSystems power_systems; +}; + +//============================================================================== +SystemTraits::SystemTraits( + const MechanicalSystem mechanical_system, + const BatterySystem battery_system, + const PowerSystems power_systems) +: _pimpl(rmf_utils::make_impl( + Implementation + { + std::move(mechanical_system), + std::move(battery_system), + std::move(power_systems) + })) +{ + // Do nothing +} + +//============================================================================== +auto SystemTraits::mechanical_system(MechanicalSystem mechanical_system) +-> SystemTraits& +{ + _pimpl->mechanical_system = std::move(mechanical_system); + return *this; +} + +//============================================================================== +const SystemTraits::MechanicalSystem SystemTraits::mechanical_system() const +{ + return _pimpl->mechanical_system; +} + +//============================================================================== +auto SystemTraits::battery_system(BatterySystem battery_system) +-> SystemTraits& +{ + _pimpl->battery_system = std::move(battery_system); + return *this; +} + +//============================================================================== +const SystemTraits::BatterySystem SystemTraits::battery_system() const +{ + return _pimpl->battery_system; +} + +//============================================================================== +auto SystemTraits::power_systems(PowerSystems power_systems) +-> SystemTraits& +{ + _pimpl->power_systems = std::move(power_systems); + return *this; +} + +//============================================================================== +const SystemTraits::PowerSystems SystemTraits::power_systems() const +{ + return _pimpl->power_systems; +} + +bool SystemTraits::valid() const +{ + bool valid = false; + for (const auto& power_system : _pimpl->power_systems) + valid = valid && power_system.valid(); + return _pimpl->battery_system.valid() && _pimpl->mechanical_system.valid() && + valid; +} + } // namespace agv } // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index 140557f58..c4458b092 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -25,12 +25,14 @@ SCENARIO("Test PowerSystem") REQUIRE(power_system.nominal_power() - 60 == Approx(0.0)); REQUIRE(power_system.nominal_voltage() - 12 == Approx(0.0)); REQUIRE(power_system.efficiency() - 1.0 == Approx(0.0)); + REQUIRE(power_system.valid()); WHEN("Nominal power is set") { power_system.nominal_power(80); CHECK(power_system.nominal_power() - 80 == Approx(0.0)); CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); + CHECK(power_system.valid()); } WHEN("Nominal voltage is set") { @@ -38,6 +40,7 @@ SCENARIO("Test PowerSystem") CHECK(power_system.nominal_voltage() - 24 == Approx(0.0)); CHECK(power_system.nominal_power() - 60 == Approx(0.0)); CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); + CHECK(power_system.valid()); } WHEN("Efficiency is set") { @@ -45,5 +48,11 @@ SCENARIO("Test PowerSystem") CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); CHECK(power_system.nominal_power() - 60 == Approx(0.0)); CHECK(power_system.efficiency() - 0.80 == Approx(0.0)); + CHECK(power_system.valid()); + } + WHEN("A property is negative") + { + power_system.nominal_voltage(-12); + CHECK_FALSE(power_system.valid()); } } \ No newline at end of file From 9328547f3133e4255195a307f5f70c1334bfb879 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 12 Aug 2020 11:48:59 +0800 Subject: [PATCH 004/128] Fixed test Signed-off-by: Yadunund Vijay --- rmf_battery/src/rmf_battery/agv/SystemTraits.cpp | 2 +- rmf_battery/test/unit/agv/test_SystemTraits.cpp | 16 ++++++++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index 4af797065..b1f43de87 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -448,7 +448,7 @@ const SystemTraits::PowerSystems SystemTraits::power_systems() const bool SystemTraits::valid() const { - bool valid = false; + bool valid = true; for (const auto& power_system : _pimpl->power_systems) valid = valid && power_system.valid(); return _pimpl->battery_system.valid() && _pimpl->mechanical_system.valid() && diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index c4458b092..f66c0b686 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -55,4 +55,20 @@ SCENARIO("Test PowerSystem") power_system.nominal_voltage(-12); CHECK_FALSE(power_system.valid()); } +} + +SCENARIO("Test SystemTraits") +{ + using SystemTraits = rmf_battery::agv::SystemTraits; + SystemTraits::BatterySystem battery_system{12, 10, 2}; + REQUIRE(battery_system.valid()); + SystemTraits::MechanicalSystem mechanical_system{60, 0.01, 0.3, 0.4}; + REQUIRE(mechanical_system.valid()); + SystemTraits::PowerSystem power_system{100, 24}; + REQUIRE(power_system.valid()); + SystemTraits system_traits{ + mechanical_system, battery_system, {power_system}}; + REQUIRE(system_traits.valid()); + + // TODO(YV): Tests for getters and setters } \ No newline at end of file From 34e4c085d53cad2debaffd833e2066f7a393f1e8 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 12 Aug 2020 18:29:02 +0800 Subject: [PATCH 005/128] Refactored MechanicalSystem --- rmf_battery/CMakeLists.txt | 4 ++- .../include/rmf_battery/agv/SystemTraits.hpp | 14 +++----- rmf_battery/package.xml | 4 ++- .../src/rmf_battery/agv/SystemTraits.cpp | 35 +++++-------------- .../test/unit/agv/test_SystemTraits.cpp | 2 +- 5 files changed, 21 insertions(+), 38 deletions(-) diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 26a9d28fe..2fcb10eae 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -15,7 +15,7 @@ include(GNUInstallDirs) # find dependencies find_package(ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) - +find_package(rmf_traffic REQUIRED) # ===== Battery modelling library file(GLOB_RECURSE core_lib_srcs "src/rmf_battery/*.cpp") @@ -33,6 +33,7 @@ if(BUILD_TESTING) TIMEOUT 300) target_link_libraries(test_rmf_battery rmf_battery + rmf_traffic::rmf_traffic ) target_include_directories(test_rmf_battery @@ -53,6 +54,7 @@ endif() target_link_libraries(rmf_battery PUBLIC rmf_utils::rmf_utils + rmf_traffic::rmf_traffic ) target_include_directories(rmf_battery diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp index 91c560b94..3849a8ab1 100644 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -134,22 +134,18 @@ class SystemTraits MechanicalSystem( double mass, - double friction_coefficient, - double drag_coefficient, - double frontal_area); + double inertia, + double friction_coefficient); MechanicalSystem& mass(double mass); double mass() const; + MechanicalSystem& inertia(double inertia); + double inertia() const; + MechanicalSystem& friction_coefficient(double friction_coeff); double friction_coefficient() const; - MechanicalSystem& drag_coefficient(double drag_coeff); - double drag_coefficient() const; - - MechanicalSystem& frontal_area(double frontal_area); - double frontal_area() const; - /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; diff --git a/rmf_battery/package.xml b/rmf_battery/package.xml index d4ec6c7d4..dcfdacadd 100644 --- a/rmf_battery/package.xml +++ b/rmf_battery/package.xml @@ -12,9 +12,11 @@ rmf_utils rmf_utils + rmf_traffic + ament_cmake_catch2 rmf_cmake_uncrustify - + eigen diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index b1f43de87..06be3b545 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -295,23 +295,20 @@ class SystemTraits::MechanicalSystem::Implementation { public: double mass; + double inertia; double friction_coefficient; - double drag_coefficient; - double frontal_area; }; //============================================================================== SystemTraits::MechanicalSystem::MechanicalSystem( const double mass, - const double friction_coefficient, - const double drag_coefficient, - const double frontal_area) + const double inertia, + const double friction_coefficient) : _pimpl(rmf_utils::make_impl( Implementation{ mass, + inertia, friction_coefficient, - drag_coefficient, - frontal_area })) { // Do nothing @@ -345,38 +342,24 @@ double SystemTraits::MechanicalSystem::friction_coefficient() const } //============================================================================== -auto SystemTraits::MechanicalSystem::drag_coefficient(double drag_coeff) +auto SystemTraits::MechanicalSystem::inertia(double inertia) -> MechanicalSystem& { - _pimpl->drag_coefficient = drag_coeff; + _pimpl->inertia = inertia; return *this; } //============================================================================== -double SystemTraits::MechanicalSystem::drag_coefficient() const +double SystemTraits::MechanicalSystem::inertia() const { - return _pimpl->drag_coefficient; -} - -//============================================================================== -auto SystemTraits::MechanicalSystem::frontal_area(double frontal_area) --> MechanicalSystem& -{ - _pimpl->frontal_area = frontal_area; - return *this; -} - -//============================================================================== -double SystemTraits::MechanicalSystem::frontal_area() const -{ - return _pimpl->frontal_area; + return _pimpl->inertia; } //============================================================================== bool SystemTraits::MechanicalSystem::valid() const { return _pimpl->mass > 0.0 && _pimpl->friction_coefficient > 0.0 && - _pimpl->drag_coefficient > 0.0 && _pimpl->frontal_area > 0.0; + _pimpl->inertia > 0.0; } //============================================================================== diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index f66c0b686..95da28fdb 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -62,7 +62,7 @@ SCENARIO("Test SystemTraits") using SystemTraits = rmf_battery::agv::SystemTraits; SystemTraits::BatterySystem battery_system{12, 10, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{60, 0.01, 0.3, 0.4}; + SystemTraits::MechanicalSystem mechanical_system{60, 10, 0.3}; REQUIRE(mechanical_system.valid()); SystemTraits::PowerSystem power_system{100, 24}; REQUIRE(power_system.valid()); From bbd6e33145d27fc6e98185d3b4a5bd1652eaf6f4 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 12 Aug 2020 21:52:50 +0800 Subject: [PATCH 006/128] Added EstimateBattery Signed-off-by: Yadunund Vijay --- rmf_battery/CMakeLists.txt | 1 + .../include/rmf_battery/EstimateBattery.hpp | 48 +++++++++++ .../src/rmf_battery/EstimateBattery.cpp | 82 +++++++++++++++++++ .../rmf_battery/EstimateBatteryInternal.hpp | 37 +++++++++ 4 files changed, 168 insertions(+) create mode 100644 rmf_battery/include/rmf_battery/EstimateBattery.hpp create mode 100644 rmf_battery/src/rmf_battery/EstimateBattery.cpp create mode 100644 rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 2fcb10eae..dc0098f91 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -16,6 +16,7 @@ include(GNUInstallDirs) find_package(ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) find_package(rmf_traffic REQUIRED) +find_package(Eigen3 REQUIRED) # ===== Battery modelling library file(GLOB_RECURSE core_lib_srcs "src/rmf_battery/*.cpp") diff --git a/rmf_battery/include/rmf_battery/EstimateBattery.hpp b/rmf_battery/include/rmf_battery/EstimateBattery.hpp new file mode 100644 index 000000000..bf0240082 --- /dev/null +++ b/rmf_battery/include/rmf_battery/EstimateBattery.hpp @@ -0,0 +1,48 @@ +/* + * 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_BATTERY__ESTIMATEBATTERY_HPP +#define RMF_BATTERY__ESTIMATEBATTERY_HPP + +#include + +#include + + +namespace rmf_battery { + + +//============================================================================== +class EstimateBattery +{ +public: + + /// Computes state-of-charge estimate of battery at the end of a trajectory. + /// + /// \return The fraction of remaining charge at the end of the trajectory. + virtual double compute_soc( + const rmf_traffic::Trajectory& trajectory, + const rmf_battery::agv::SystemTraits& system_traits, + const double initial_soc) const = 0; + + + +}; + +} // namespace rmf_traffic + +#endif // RMF_BATTERY__ESTIMATEBATTERY_HPP diff --git a/rmf_battery/src/rmf_battery/EstimateBattery.cpp b/rmf_battery/src/rmf_battery/EstimateBattery.cpp new file mode 100644 index 000000000..692626ce0 --- /dev/null +++ b/rmf_battery/src/rmf_battery/EstimateBattery.cpp @@ -0,0 +1,82 @@ +/* + * 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 "EstimateBatteryInternal.hpp" + +#include + +#include +#include + +namespace rmf_battery { + +double SampleEstimator::compute_soc( + const rmf_traffic::Trajectory& trajectory, + const rmf_battery::agv::SystemTraits& system_traits, + const double initial_soc) const +{ + assert(system_traits.valid()); + std::vector trajectory_soc; + trajectory_soc.reserve(trajectory.size()); + trajectory_soc.push_back(initial_soc); + + double battery_soc = initial_soc; + double nominal_capacity = system_traits.battery_system().nominal_capacity(); + double nominal_voltage = system_traits.battery_system().nominal_voltage(); + const double mass = system_traits.mechanical_system().mass(); + const double inertia = system_traits.mechanical_system().inertia(); + const double friction = + system_traits.mechanical_system().friction_coefficient(); + const double g = 9.81; // ms-1 + const int sim_step = 100; // milliseconds + + for (auto it = trajectory.begin(); it != trajectory.end(); it++) + { + auto next_it = it; ++next_it; + if (next_it == trajectory.end()) + break; + const auto start_time = it->time() + std::chrono::milliseconds(sim_step); + const auto end_time = next_it->time(); + const auto motion = rmf_traffic::Motion::compute_cubic_splines(it, next_it); + + double dE = 0.0; + + for (auto sim_time = start_time; + sim_time <= end_time; sim_time += std::chrono::milliseconds(sim_step)) + { + const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); + const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + const double w = velocity[2]; + // Kinetic energy + const double dKE = 0.5 * (mass*pow(v, 2) + inertia*pow(w, 2)); + // Friction energy + const double dFE = friction * mass * g * 2 * v * (sim_step/1000); + // TODO(YV) energy from power systems + // + dE += dKE + dFE; + } + + // Charge consumed + const double dQ = dE / nominal_voltage; + battery_soc -= dQ / nominal_capacity; + trajectory_soc.push_back(battery_soc); + } + + return trajectory_soc.back(); +} + +} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp b/rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp new file mode 100644 index 000000000..e2273927f --- /dev/null +++ b/rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp @@ -0,0 +1,37 @@ +/* + * 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 SRC__RMF_BATTERY__ESTIMATEBATTERYINTERNAL_HPP +#define SRC__RMF_BATTERY__ESTIMATEBATTERYINTERNAL_HPP + +#include + +namespace rmf_battery { + +class SampleEstimator : public EstimateBattery +{ +public: + double compute_soc( + const rmf_traffic::Trajectory& trajectory, + const rmf_battery::agv::SystemTraits& system_traits, + const double initial_soc) const final; +}; + + +} // namespace rmf_battery + +#endif // SRC__RMF_BATTERY__ESTIMATEBATTERYINTERNAL_HPP \ No newline at end of file From ec6ec27882051b7fd3a458b5335af774034b47b5 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 18 Aug 2020 10:23:10 +0800 Subject: [PATCH 007/128] Moved soc implementation to SimpleBatteryEstimator --- .../include/rmf_battery/EstimateBattery.hpp | 6 +-- .../agv/SimpleBatteryEstimator.hpp} | 29 ++++++++--- .../SimpleBatteryEstimator.cpp} | 48 +++++++++++++++---- .../unit/agv/test_SimpleBatteryEstimator.cpp | 25 ++++++++++ .../test/unit/agv/test_SystemTraits.cpp | 2 +- 5 files changed, 88 insertions(+), 22 deletions(-) rename rmf_battery/{src/rmf_battery/EstimateBatteryInternal.hpp => include/rmf_battery/agv/SimpleBatteryEstimator.hpp} (59%) rename rmf_battery/src/rmf_battery/{EstimateBattery.cpp => agv/SimpleBatteryEstimator.cpp} (64%) create mode 100644 rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp diff --git a/rmf_battery/include/rmf_battery/EstimateBattery.hpp b/rmf_battery/include/rmf_battery/EstimateBattery.hpp index bf0240082..e634c1319 100644 --- a/rmf_battery/include/rmf_battery/EstimateBattery.hpp +++ b/rmf_battery/include/rmf_battery/EstimateBattery.hpp @@ -31,16 +31,14 @@ class EstimateBattery { public: + /// Computes state-of-charge estimate of battery at the end of a trajectory. /// /// \return The fraction of remaining charge at the end of the trajectory. - virtual double compute_soc( + virtual double compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, - const rmf_battery::agv::SystemTraits& system_traits, const double initial_soc) const = 0; - - }; } // namespace rmf_traffic diff --git a/rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp similarity index 59% rename from rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp rename to rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp index e2273927f..ca73d9b24 100644 --- a/rmf_battery/src/rmf_battery/EstimateBatteryInternal.hpp +++ b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp @@ -15,23 +15,38 @@ * */ -#ifndef SRC__RMF_BATTERY__ESTIMATEBATTERYINTERNAL_HPP -#define SRC__RMF_BATTERY__ESTIMATEBATTERYINTERNAL_HPP +#ifndef RMF_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP +#define RMF_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP #include +#include + + namespace rmf_battery { +namespace agv { -class SampleEstimator : public EstimateBattery +class SimpleBatteryEstimator : public EstimateBattery { public: - double compute_soc( + + SimpleBatteryEstimator(SystemTraits& system_traits); + + SimpleBatteryEstimator& system_traits(SystemTraits system_traits); + + const SystemTraits system_traits() const; + + double compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, - const rmf_battery::agv::SystemTraits& system_traits, const double initial_soc) const final; -}; + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; +}; +} // namespace agv } // namespace rmf_battery -#endif // SRC__RMF_BATTERY__ESTIMATEBATTERYINTERNAL_HPP \ No newline at end of file +#endif // RMF_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/EstimateBattery.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp similarity index 64% rename from rmf_battery/src/rmf_battery/EstimateBattery.cpp rename to rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index 692626ce0..ad0021068 100644 --- a/rmf_battery/src/rmf_battery/EstimateBattery.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -15,7 +15,8 @@ * */ -#include "EstimateBatteryInternal.hpp" +#include +#include #include @@ -23,24 +24,50 @@ #include namespace rmf_battery { +namespace agv { -double SampleEstimator::compute_soc( +class SimpleBatteryEstimator::Implementation +{ +public: + SystemTraits system_traits; +}; + +SimpleBatteryEstimator::SimpleBatteryEstimator( + SystemTraits& system_traits) +: _pimpl(rmf_utils::make_impl( + Implementation{std::move(system_traits)})) +{ + // Do nothing +} + +auto SimpleBatteryEstimator::system_traits(const SystemTraits system_traits) +-> SimpleBatteryEstimator& +{ + _pimpl->system_traits = std::move(system_traits); + return *this; +} + +const SystemTraits SimpleBatteryEstimator::system_traits() const +{ + return _pimpl->system_traits; +} + +double SimpleBatteryEstimator::compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, - const rmf_battery::agv::SystemTraits& system_traits, const double initial_soc) const { - assert(system_traits.valid()); + assert(_pimpl->system_traits.valid()); std::vector trajectory_soc; trajectory_soc.reserve(trajectory.size()); trajectory_soc.push_back(initial_soc); double battery_soc = initial_soc; - double nominal_capacity = system_traits.battery_system().nominal_capacity(); - double nominal_voltage = system_traits.battery_system().nominal_voltage(); - const double mass = system_traits.mechanical_system().mass(); - const double inertia = system_traits.mechanical_system().inertia(); + double nominal_capacity = _pimpl->system_traits.battery_system().nominal_capacity(); + double nominal_voltage = _pimpl->system_traits.battery_system().nominal_voltage(); + const double mass = _pimpl->system_traits.mechanical_system().mass(); + const double inertia = _pimpl->system_traits.mechanical_system().inertia(); const double friction = - system_traits.mechanical_system().friction_coefficient(); + _pimpl->system_traits.mechanical_system().friction_coefficient(); const double g = 9.81; // ms-1 const int sim_step = 100; // milliseconds @@ -79,4 +106,5 @@ double SampleEstimator::compute_soc( return trajectory_soc.back(); } -} // namespace rmf_battery \ No newline at end of file +} // namespace agv +} // namespace rmf_battery diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp new file mode 100644 index 000000000..33f51bd5c --- /dev/null +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -0,0 +1,25 @@ +/* + * 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 + +SCENARIO("Test SimpleBatteryEstimator") +{ + +} \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index 95da28fdb..361ea8ba8 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. From ca5ddd9840838848a0c399b8a6164b1321d71c88 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 18 Aug 2020 12:23:46 +0800 Subject: [PATCH 008/128] Debugging soc --- .../agv/SimpleBatteryEstimator.cpp | 58 ++++++++++++++++--- .../unit/agv/test_SimpleBatteryEstimator.cpp | 35 ++++++++++- 2 files changed, 84 insertions(+), 9 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index ad0021068..1a029bb8e 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -23,6 +23,8 @@ #include #include +#include + namespace rmf_battery { namespace agv { @@ -52,6 +54,27 @@ const SystemTraits SimpleBatteryEstimator::system_traits() const return _pimpl->system_traits; } +namespace { +double compute_kinetic_energy( + const double m, + const double v, + const double i, + const double w) +{ + return 0.5 * (m*pow(v, 2) + i*pow(w, 2)); +} + +double compute_friction_energy( + const double f, + const double m, + const double v, + const double dt) +{ + const double g = 9.81; // ms-1 + return f * m * g * 2 * v * dt; +} + +} // namespace anonymous double SimpleBatteryEstimator::compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, const double initial_soc) const @@ -68,7 +91,6 @@ double SimpleBatteryEstimator::compute_state_of_charge( const double inertia = _pimpl->system_traits.mechanical_system().inertia(); const double friction = _pimpl->system_traits.mechanical_system().friction_coefficient(); - const double g = 9.81; // ms-1 const int sim_step = 100; // milliseconds for (auto it = trajectory.begin(); it != trajectory.end(); it++) @@ -76,33 +98,53 @@ double SimpleBatteryEstimator::compute_state_of_charge( auto next_it = it; ++next_it; if (next_it == trajectory.end()) break; - const auto start_time = it->time() + std::chrono::milliseconds(sim_step); + auto start_time = it->time(); const auto end_time = next_it->time(); const auto motion = rmf_traffic::Motion::compute_cubic_splines(it, next_it); - double dE = 0.0; + const Eigen::Vector3d velocity = motion->compute_velocity(start_time); + double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + double w = velocity[2]; + double KE_previous = compute_kinetic_energy(mass, v, inertia, w); + double FE_previous = 0.0; + start_time += std::chrono::milliseconds(sim_step); + double dE = 0.0; + int count = 0; for (auto sim_time = start_time; sim_time <= end_time; sim_time += std::chrono::milliseconds(sim_step)) { + std::cout << " sim time: " << sim_time.time_since_epoch().count() << std:: endl; const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); - const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - const double w = velocity[2]; + std::cout << "Velocity:" << velocity[0] << "," << velocity[1] << std::endl; + v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + w = velocity[2]; + std::cout << " v: " << v << " w: " << w << std::endl; // Kinetic energy - const double dKE = 0.5 * (mass*pow(v, 2) + inertia*pow(w, 2)); + const double KE = compute_kinetic_energy(mass, v, inertia, w); + const double dKE = KE - KE_previous; + KE_previous = KE; // Friction energy - const double dFE = friction * mass * g * 2 * v * (sim_step/1000); + const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); + const double dFE = FE - FE_previous; + FE_previous = FE; + // TODO(YV) energy from power systems // dE += dKE + dFE; + std::cout << " dKE: " << dKE << " dFE: " << dFE << std::endl; + count++; } // Charge consumed const double dQ = dE / nominal_voltage; + std::cout << "count: " << count << std::endl; + std::cout << " dQ: " << dQ << std::endl; battery_soc -= dQ / nominal_capacity; + std::cout << " Battery soc: " << battery_soc << std::endl; trajectory_soc.push_back(battery_soc); } - + std::cout << "Trajectory soc size: " << trajectory_soc.size() << std::endl; return trajectory_soc.back(); } diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 33f51bd5c..62e0b9b8f 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -16,10 +16,43 @@ */ #include +#include + +#include #include +#include + SCENARIO("Test SimpleBatteryEstimator") { - + using SystemTraits = rmf_battery::agv::SystemTraits; + using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; + using namespace std::chrono_literals; + + // Initializing system traits + SystemTraits::BatterySystem battery_system{12, 20, 2}; + REQUIRE(battery_system.valid()); + SystemTraits::MechanicalSystem mechanical_system{60, 10, 0.3}; + REQUIRE(mechanical_system.valid()); + SystemTraits::PowerSystem power_system{100, 24}; + REQUIRE(power_system.valid()); + SystemTraits system_traits{ + mechanical_system, battery_system, {power_system}}; + REQUIRE(system_traits.valid()); + + auto battery_estimator = SimpleBatteryEstimator{system_traits}; + + WHEN("Robot moves 100m along a straight line") + { + rmf_traffic::Trajectory trajectory; + const auto start_time = std::chrono::steady_clock::now(); + trajectory.insert(start_time, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}); + trajectory.insert(start_time + 50s, {50.0, 0.0, 0.0}, {1.5, 0.0, 0.0}); + trajectory.insert(start_time + 100s, {100.0, 0.0, 0.0}, {2.0, 0.0, 0.0}); + REQUIRE(trajectory.size() == 3); + + auto remaining_soc = battery_estimator.compute_state_of_charge(trajectory, 0.9); + std::cout << "Start time: " << start_time.time_since_epoch().count() << " soc: " << remaining_soc << std::endl; + } } \ No newline at end of file From ea8235135514a4f1d4e4492c4da914dc4e0aea77 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 18 Aug 2020 13:16:40 +0800 Subject: [PATCH 009/128] Debuggin messages --- .../agv/SimpleBatteryEstimator.cpp | 21 ++++++++++--------- .../unit/agv/test_SimpleBatteryEstimator.cpp | 4 ++-- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index 1a029bb8e..a0a3f27e8 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -110,41 +110,42 @@ double SimpleBatteryEstimator::compute_state_of_charge( start_time += std::chrono::milliseconds(sim_step); double dE = 0.0; - int count = 0; for (auto sim_time = start_time; sim_time <= end_time; sim_time += std::chrono::milliseconds(sim_step)) { - std::cout << " sim time: " << sim_time.time_since_epoch().count() << std:: endl; + // std::cout << " sim time: " << sim_time.time_since_epoch().count() << std:: endl; const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); - std::cout << "Velocity:" << velocity[0] << "," << velocity[1] << std::endl; + // std::cout << " Velocity:" << velocity[0] << "," << velocity[1] << std::endl; v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); w = velocity[2]; - std::cout << " v: " << v << " w: " << w << std::endl; + // std::cout << " v: " << v << " w: " << w << std::endl; // Kinetic energy + // We assume the robot does not coast nor has regernerative braking const double KE = compute_kinetic_energy(mass, v, inertia, w); - const double dKE = KE - KE_previous; + const double dKE = std::abs(KE - KE_previous); KE_previous = KE; // Friction energy const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); - const double dFE = FE - FE_previous; + const double dFE = std::abs(FE - FE_previous); FE_previous = FE; // TODO(YV) energy from power systems // dE += dKE + dFE; - std::cout << " dKE: " << dKE << " dFE: " << dFE << std::endl; - count++; + // std::cout << " dKE: " << dKE << " dFE: " << dFE << std::endl; } // Charge consumed const double dQ = dE / nominal_voltage; - std::cout << "count: " << count << std::endl; std::cout << " dQ: " << dQ << std::endl; battery_soc -= dQ / nominal_capacity; - std::cout << " Battery soc: " << battery_soc << std::endl; trajectory_soc.push_back(battery_soc); } std::cout << "Trajectory soc size: " << trajectory_soc.size() << std::endl; + for (auto it = trajectory_soc.begin(); it != trajectory_soc.end(); it++) + { + std::cout << " soc: " << *it << std::endl; + } return trajectory_soc.back(); } diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 62e0b9b8f..0c1e89158 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -48,8 +48,8 @@ SCENARIO("Test SimpleBatteryEstimator") rmf_traffic::Trajectory trajectory; const auto start_time = std::chrono::steady_clock::now(); trajectory.insert(start_time, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}); - trajectory.insert(start_time + 50s, {50.0, 0.0, 0.0}, {1.5, 0.0, 0.0}); - trajectory.insert(start_time + 100s, {100.0, 0.0, 0.0}, {2.0, 0.0, 0.0}); + trajectory.insert(start_time + 50s, {50.0, 0.0, 0.0}, {0.5, 0.0, 0.0}); + trajectory.insert(start_time + 100s, {100.0, 0.0, 0.0}, {1.0, 0.0, 0.0}); REQUIRE(trajectory.size() == 3); auto remaining_soc = battery_estimator.compute_state_of_charge(trajectory, 0.9); From e54d6b3718663131e6d4c270bf937f502ae2f1fa Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 19 Aug 2020 12:12:55 +0800 Subject: [PATCH 010/128] More debugging --- .../agv/SimpleBatteryEstimator.cpp | 33 +++++++++++-------- .../unit/agv/test_SimpleBatteryEstimator.cpp | 25 +++++++++----- 2 files changed, 36 insertions(+), 22 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index a0a3f27e8..d3c8d923a 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -91,16 +92,19 @@ double SimpleBatteryEstimator::compute_state_of_charge( const double inertia = _pimpl->system_traits.mechanical_system().inertia(); const double friction = _pimpl->system_traits.mechanical_system().friction_coefficient(); - const int sim_step = 100; // milliseconds + const double sim_step = 0.1; // seconds - for (auto it = trajectory.begin(); it != trajectory.end(); it++) - { - auto next_it = it; ++next_it; - if (next_it == trajectory.end()) + + auto begin_it = it; + if (begin_it != trajectory.begin()) + --begin_it; + auto end_it = it; ++end_it; + if (end_it == trajectory.end()) break; - auto start_time = it->time(); - const auto end_time = next_it->time(); - const auto motion = rmf_traffic::Motion::compute_cubic_splines(it, next_it); + auto start_time = begin_it->time(); + const auto end_time = end_it->time(); + const auto motion = rmf_traffic::Motion::compute_cubic_splines( + begin_it, end_it); const Eigen::Vector3d velocity = motion->compute_velocity(start_time); double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); @@ -108,17 +112,20 @@ double SimpleBatteryEstimator::compute_state_of_charge( double KE_previous = compute_kinetic_energy(mass, v, inertia, w); double FE_previous = 0.0; - start_time += std::chrono::milliseconds(sim_step); + start_time = rmf_traffic::time::apply_offset(start_time, sim_step); + double dE = 0.0; + for (auto sim_time = start_time; - sim_time <= end_time; sim_time += std::chrono::milliseconds(sim_step)) + sim_time <= end_time; + sim_time = rmf_traffic::time::apply_offset(sim_time, sim_step)) { // std::cout << " sim time: " << sim_time.time_since_epoch().count() << std:: endl; const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); - // std::cout << " Velocity:" << velocity[0] << "," << velocity[1] << std::endl; + std::cout << " Velocity:" << velocity[0] << "," << velocity[1] << std::endl; v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); w = velocity[2]; - // std::cout << " v: " << v << " w: " << w << std::endl; + std::cout << " v: " << v << " w: " << w << std::endl; // Kinetic energy // We assume the robot does not coast nor has regernerative braking const double KE = compute_kinetic_energy(mass, v, inertia, w); @@ -140,7 +147,7 @@ double SimpleBatteryEstimator::compute_state_of_charge( std::cout << " dQ: " << dQ << std::endl; battery_soc -= dQ / nominal_capacity; trajectory_soc.push_back(battery_soc); - } + std::cout << "Trajectory soc size: " << trajectory_soc.size() << std::endl; for (auto it = trajectory_soc.begin(); it != trajectory_soc.end(); it++) { diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 0c1e89158..5f01f258e 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include @@ -43,16 +45,21 @@ SCENARIO("Test SimpleBatteryEstimator") auto battery_estimator = SimpleBatteryEstimator{system_traits}; + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {0.7, 0.5}, {0.3, 0.25}, {nullptr, nullptr}); + WHEN("Robot moves 100m along a straight line") { - rmf_traffic::Trajectory trajectory; - const auto start_time = std::chrono::steady_clock::now(); - trajectory.insert(start_time, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}); - trajectory.insert(start_time + 50s, {50.0, 0.0, 0.0}, {0.5, 0.0, 0.0}); - trajectory.insert(start_time + 100s, {100.0, 0.0, 0.0}, {1.0, 0.0, 0.0}); - REQUIRE(trajectory.size() == 3); - - auto remaining_soc = battery_estimator.compute_state_of_charge(trajectory, 0.9); - std::cout << "Start time: " << start_time.time_since_epoch().count() << " soc: " << remaining_soc << std::endl; + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{10, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + auto remaining_soc = battery_estimator.compute_state_of_charge(trajectory, 0.9); + std::cout << "Start time: " << start_time.time_since_epoch().count() << " soc: " << remaining_soc << std::endl; } } \ No newline at end of file From b8dfe0278da2d3cb9729b1ebbf6d4a0eaba09995 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 19 Aug 2020 13:34:40 +0800 Subject: [PATCH 011/128] Initial signs of working discharge model --- .../agv/SimpleBatteryEstimator.cpp | 96 ++++++++----------- .../unit/agv/test_SimpleBatteryEstimator.cpp | 9 +- 2 files changed, 45 insertions(+), 60 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index d3c8d923a..295ec6fb2 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -95,64 +95,48 @@ double SimpleBatteryEstimator::compute_state_of_charge( const double sim_step = 0.1; // seconds - auto begin_it = it; - if (begin_it != trajectory.begin()) - --begin_it; - auto end_it = it; ++end_it; - if (end_it == trajectory.end()) - break; - auto start_time = begin_it->time(); - const auto end_time = end_it->time(); - const auto motion = rmf_traffic::Motion::compute_cubic_splines( - begin_it, end_it); - - const Eigen::Vector3d velocity = motion->compute_velocity(start_time); - double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - double w = velocity[2]; - double KE_previous = compute_kinetic_energy(mass, v, inertia, w); - double FE_previous = 0.0; - - start_time = rmf_traffic::time::apply_offset(start_time, sim_step); - - double dE = 0.0; - - for (auto sim_time = start_time; - sim_time <= end_time; - sim_time = rmf_traffic::time::apply_offset(sim_time, sim_step)) - { - // std::cout << " sim time: " << sim_time.time_since_epoch().count() << std:: endl; - const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); - std::cout << " Velocity:" << velocity[0] << "," << velocity[1] << std::endl; - v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - w = velocity[2]; - std::cout << " v: " << v << " w: " << w << std::endl; - // Kinetic energy - // We assume the robot does not coast nor has regernerative braking - const double KE = compute_kinetic_energy(mass, v, inertia, w); - const double dKE = std::abs(KE - KE_previous); - KE_previous = KE; - // Friction energy - const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); - const double dFE = std::abs(FE - FE_previous); - FE_previous = FE; - - // TODO(YV) energy from power systems - // - dE += dKE + dFE; - // std::cout << " dKE: " << dKE << " dFE: " << dFE << std::endl; - } - - // Charge consumed - const double dQ = dE / nominal_voltage; - std::cout << " dQ: " << dQ << std::endl; - battery_soc -= dQ / nominal_capacity; - trajectory_soc.push_back(battery_soc); - - std::cout << "Trajectory soc size: " << trajectory_soc.size() << std::endl; - for (auto it = trajectory_soc.begin(); it != trajectory_soc.end(); it++) + auto begin_it = trajectory.begin(); + auto end_it = --trajectory.end(); + + auto start_time = begin_it->time(); + const auto end_time = end_it->time(); + const auto motion = rmf_traffic::Motion::compute_cubic_splines( + begin_it, trajectory.end()); + + const Eigen::Vector3d velocity = motion->compute_velocity(start_time); + double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + double w = velocity[2]; + double KE_previous = compute_kinetic_energy(mass, v, inertia, w); + + start_time = rmf_traffic::time::apply_offset(start_time, sim_step); + + double dE = 0.0; + + for (auto sim_time = start_time; + sim_time <= end_time; + sim_time = rmf_traffic::time::apply_offset(sim_time, sim_step)) { - std::cout << " soc: " << *it << std::endl; + const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); + v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + w = velocity[2]; + // Loss through kinetic energy + // We assume the robot does not coast and has regernerative braking + const double KE = compute_kinetic_energy(mass, v, inertia, w); + const double dKE = KE - KE_previous; + KE_previous = KE; + // Loss through friction energy + const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); + + // TODO(YV) energy loss from power systems + // We require a mapping between power systems and active durations + dE += dKE + FE; } + + // Computing the charge consumed + const double dQ = dE / nominal_voltage; + battery_soc -= dQ / nominal_capacity; + trajectory_soc.push_back(battery_soc); + return trajectory_soc.back(); } diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 5f01f258e..92282cda0 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -35,7 +35,7 @@ SCENARIO("Test SimpleBatteryEstimator") // Initializing system traits SystemTraits::BatterySystem battery_system{12, 20, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{60, 10, 0.3}; + SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.03}; REQUIRE(mechanical_system.valid()); SystemTraits::PowerSystem power_system{100, 24}; REQUIRE(power_system.valid()); @@ -54,12 +54,13 @@ SCENARIO("Test SimpleBatteryEstimator") const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{10, 0.0, 0.0}, + Eigen::Vector3d{100, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - auto remaining_soc = battery_estimator.compute_state_of_charge(trajectory, 0.9); - std::cout << "Start time: " << start_time.time_since_epoch().count() << " soc: " << remaining_soc << std::endl; + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0); + REQUIRE(remaining_soc > 0.99); } } \ No newline at end of file From d60192833080bad03699d17f66ff8a71a1a6d568 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 19 Aug 2020 19:41:32 +0800 Subject: [PATCH 012/128] Added more tests --- .../include/rmf_battery/EstimateBattery.hpp | 10 ++++- .../agv/SimpleBatteryEstimator.hpp | 6 ++- .../include/rmf_battery/agv/SystemTraits.hpp | 14 +++++-- .../agv/SimpleBatteryEstimator.cpp | 33 +++++++++++----- .../src/rmf_battery/agv/SystemTraits.cpp | 24 +++++++++--- .../unit/agv/test_SimpleBatteryEstimator.cpp | 38 ++++++++++++++++--- .../test/unit/agv/test_SystemTraits.cpp | 19 ++++++++-- 7 files changed, 112 insertions(+), 32 deletions(-) diff --git a/rmf_battery/include/rmf_battery/EstimateBattery.hpp b/rmf_battery/include/rmf_battery/EstimateBattery.hpp index e634c1319..088ff2e99 100644 --- a/rmf_battery/include/rmf_battery/EstimateBattery.hpp +++ b/rmf_battery/include/rmf_battery/EstimateBattery.hpp @@ -22,6 +22,10 @@ #include +#include + +#include +#include namespace rmf_battery { @@ -31,13 +35,15 @@ class EstimateBattery { public: - + using PowerMap = std::unordered_map; + /// Computes state-of-charge estimate of battery at the end of a trajectory. /// /// \return The fraction of remaining charge at the end of the trajectory. virtual double compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, - const double initial_soc) const = 0; + const double initial_soc, + rmf_utils::optional power_map = rmf_utils::nullopt) const = 0; }; diff --git a/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp index ca73d9b24..3a4aa5d6d 100644 --- a/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp +++ b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp @@ -20,7 +20,7 @@ #include -#include +#include namespace rmf_battery { @@ -38,7 +38,9 @@ class SimpleBatteryEstimator : public EstimateBattery double compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, - const double initial_soc) const final; + const double initial_soc, + rmf_utils::optional power_map = rmf_utils::nullopt + ) const final; class Implementation; diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp index 3849a8ab1..ac3ebe5b0 100644 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -18,9 +18,11 @@ #ifndef RMF_BATTERY__AGV__SYSTEMTRAITS_HPP #define RMF_BATTERY__AGV__SYSTEMTRAITS_HPP -#include -#include -#include +#include +#include + +#include +#include namespace rmf_battery { namespace agv { @@ -30,17 +32,21 @@ class SystemTraits { public: class PowerSystem; - using PowerSystems = std::vector; + using PowerSystems = std::unordered_map; class PowerSystem { public: PowerSystem( + std::string name, double nominal_power, double nominal_voltage, double efficiency = 1.0); + PowerSystem& name(std::string name); + std::string name() const; + PowerSystem& nominal_power(double nom_power); double nominal_power() const; diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index 295ec6fb2..c532411b7 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -23,7 +23,6 @@ #include #include - #include namespace rmf_battery { @@ -78,7 +77,8 @@ double compute_friction_energy( } // namespace anonymous double SimpleBatteryEstimator::compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, - const double initial_soc) const + const double initial_soc, + rmf_utils::optional power_map) const { assert(_pimpl->system_traits.valid()); std::vector trajectory_soc; @@ -86,14 +86,14 @@ double SimpleBatteryEstimator::compute_state_of_charge( trajectory_soc.push_back(initial_soc); double battery_soc = initial_soc; - double nominal_capacity = _pimpl->system_traits.battery_system().nominal_capacity(); + double nominal_capacity = + _pimpl->system_traits.battery_system().nominal_capacity(); double nominal_voltage = _pimpl->system_traits.battery_system().nominal_voltage(); const double mass = _pimpl->system_traits.mechanical_system().mass(); const double inertia = _pimpl->system_traits.mechanical_system().inertia(); const double friction = _pimpl->system_traits.mechanical_system().friction_coefficient(); - const double sim_step = 0.1; // seconds - + const auto& power_systems = _pimpl->system_traits.power_systems(); auto begin_it = trajectory.begin(); auto end_it = --trajectory.end(); @@ -108,6 +108,7 @@ double SimpleBatteryEstimator::compute_state_of_charge( double w = velocity[2]; double KE_previous = compute_kinetic_energy(mass, v, inertia, w); + const double sim_step = 0.1; // seconds start_time = rmf_traffic::time::apply_offset(start_time, sim_step); double dE = 0.0; @@ -127,16 +128,28 @@ double SimpleBatteryEstimator::compute_state_of_charge( // Loss through friction energy const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); - // TODO(YV) energy loss from power systems - // We require a mapping between power systems and active durations - dE += dKE + FE; + double PE = 0.0; + // Loss through power systems + if (power_map.has_value()) + { + for (const auto& item : power_map.value()) + { + if (item.second.find(sim_time) != item.second.end()) + { + const auto it = power_systems.find(item.first); + assert(it != power_systems.end()); + PE += it->second.nominal_power() * sim_step; + } + } + } + dE += dKE + FE + PE; } // Computing the charge consumed const double dQ = dE / nominal_voltage; - battery_soc -= dQ / nominal_capacity; + battery_soc -= dQ / (nominal_capacity * 3600); trajectory_soc.push_back(battery_soc); - + return trajectory_soc.back(); } diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index 06be3b545..8ccad4f95 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -26,7 +26,7 @@ using BatteryType = SystemTraits::BatterySystem::BatteryType; class SystemTraits::PowerSystem::Implementation { public: - + std::string name; double nominal_power; double nominal_voltage; double efficiency; @@ -34,15 +34,29 @@ class SystemTraits::PowerSystem::Implementation //============================================================================== SystemTraits::PowerSystem::PowerSystem( + const std::string name, const double nominal_power, const double nominal_voltage, const double efficiency) : _pimpl(rmf_utils::make_impl( - Implementation{nominal_power, nominal_voltage, efficiency})) + Implementation{std::move(name), nominal_power, nominal_voltage, efficiency})) { // Do nothing } +//============================================================================== +auto SystemTraits::PowerSystem::name(std::string name) -> PowerSystem& +{ + _pimpl->name = name; + return *this; +} + +//============================================================================== +std::string SystemTraits::PowerSystem::name() const +{ + return _pimpl->name; +} + //============================================================================== auto SystemTraits::PowerSystem::nominal_power(double nom_power) ->PowerSystem& { @@ -86,8 +100,8 @@ double SystemTraits::PowerSystem::efficiency() const //============================================================================== bool SystemTraits::PowerSystem::valid() const { - return _pimpl->nominal_power > 0.0 && _pimpl->nominal_voltage > 0.0 && - _pimpl->efficiency > 0.0; + return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0 + && _pimpl->nominal_voltage > 0.0 && _pimpl->efficiency > 0.0; } //============================================================================== @@ -433,7 +447,7 @@ bool SystemTraits::valid() const { bool valid = true; for (const auto& power_system : _pimpl->power_systems) - valid = valid && power_system.valid(); + valid = valid && power_system.second.valid(); return _pimpl->battery_system.valid() && _pimpl->mechanical_system.valid() && valid; } diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 92282cda0..e940e7bf6 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -30,17 +30,20 @@ SCENARIO("Test SimpleBatteryEstimator") { using SystemTraits = rmf_battery::agv::SystemTraits; using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; + using PowerMap = rmf_battery::EstimateBattery::PowerMap; using namespace std::chrono_literals; // Initializing system traits - SystemTraits::BatterySystem battery_system{12, 20, 2}; + SystemTraits::BatterySystem battery_system{24, 24, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.03}; + SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.3}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system{100, 24}; - REQUIRE(power_system.valid()); + SystemTraits::PowerSystem power_system_1{"processor", 10, 5}; + REQUIRE(power_system_1.valid()); + SystemTraits::PowerSystems power_systems; + power_systems.insert({power_system_1.name(), power_system_1}); SystemTraits system_traits{ - mechanical_system, battery_system, {power_system}}; + mechanical_system, battery_system, power_systems}; REQUIRE(system_traits.valid()); auto battery_estimator = SimpleBatteryEstimator{system_traits}; @@ -61,6 +64,29 @@ SCENARIO("Test SimpleBatteryEstimator") auto remaining_soc = battery_estimator.compute_state_of_charge( trajectory, 1.0); - REQUIRE(remaining_soc > 0.99); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; + CHECK(ok); + } + + WHEN("Robot moves 20km along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{20000, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + CHECK(remaining_soc == Approx(0.0)); } } \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index 361ea8ba8..d1f46ae69 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -21,11 +21,22 @@ SCENARIO("Test PowerSystem") { - rmf_battery::agv::SystemTraits::PowerSystem power_system(60, 12); + rmf_battery::agv::SystemTraits::PowerSystem power_system( + "cleaning_system", 60, 12); + REQUIRE(power_system.name() == "cleaning_system"); REQUIRE(power_system.nominal_power() - 60 == Approx(0.0)); REQUIRE(power_system.nominal_voltage() - 12 == Approx(0.0)); REQUIRE(power_system.efficiency() - 1.0 == Approx(0.0)); REQUIRE(power_system.valid()); + WHEN("Name is set") + { + power_system.name("vacuuming_system"); + CHECK(power_system.name() == "vacuuming_system"); + CHECK(power_system.nominal_power() - 60 == Approx(0.0)); + CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); + CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); + CHECK(power_system.valid()); + } WHEN("Nominal power is set") { power_system.nominal_power(80); @@ -64,10 +75,12 @@ SCENARIO("Test SystemTraits") REQUIRE(battery_system.valid()); SystemTraits::MechanicalSystem mechanical_system{60, 10, 0.3}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system{100, 24}; + SystemTraits::PowerSystem power_system{"cleaning_system", 100, 24}; REQUIRE(power_system.valid()); + SystemTraits::PowerSystems power_systems; + power_systems.insert({power_system.name(), power_system}); SystemTraits system_traits{ - mechanical_system, battery_system, {power_system}}; + mechanical_system, battery_system, power_systems}; REQUIRE(system_traits.valid()); // TODO(YV): Tests for getters and setters From acfbdb16eacde1ea9adbadd18f27bcd011471819 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 19 Aug 2020 20:39:13 +0800 Subject: [PATCH 013/128] Fixed tests --- .../agv/SimpleBatteryEstimator.cpp | 8 +- .../unit/agv/test_SimpleBatteryEstimator.cpp | 87 +++++++++++++++++-- 2 files changed, 84 insertions(+), 11 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index c532411b7..b93d51cd2 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -106,7 +106,7 @@ double SimpleBatteryEstimator::compute_state_of_charge( const Eigen::Vector3d velocity = motion->compute_velocity(start_time); double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); double w = velocity[2]; - double KE_previous = compute_kinetic_energy(mass, v, inertia, w); + // double KE_previous = compute_kinetic_energy(mass, v, inertia, w); const double sim_step = 0.1; // seconds start_time = rmf_traffic::time::apply_offset(start_time, sim_step); @@ -123,8 +123,8 @@ double SimpleBatteryEstimator::compute_state_of_charge( // Loss through kinetic energy // We assume the robot does not coast and has regernerative braking const double KE = compute_kinetic_energy(mass, v, inertia, w); - const double dKE = KE - KE_previous; - KE_previous = KE; + // const double dKE = KE - KE_previous; + // KE_previous = KE; // Loss through friction energy const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); @@ -142,7 +142,7 @@ double SimpleBatteryEstimator::compute_state_of_charge( } } } - dE += dKE + FE + PE; + dE += KE + FE + PE; } // Computing the charge consumed diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index e940e7bf6..71607ee20 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -26,7 +26,7 @@ #include -SCENARIO("Test SimpleBatteryEstimator") +SCENARIO("Test SimpleBatteryEstimator with RobotA") { using SystemTraits = rmf_battery::agv::SystemTraits; using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; @@ -34,11 +34,11 @@ SCENARIO("Test SimpleBatteryEstimator") using namespace std::chrono_literals; // Initializing system traits - SystemTraits::BatterySystem battery_system{24, 24, 2}; + SystemTraits::BatterySystem battery_system{12, 24, 2}; REQUIRE(battery_system.valid()); SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.3}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 10, 5}; + SystemTraits::PowerSystem power_system_1{"processor", 5, 5}; REQUIRE(power_system_1.valid()); SystemTraits::PowerSystems power_systems; power_systems.insert({power_system_1.name(), power_system_1}); @@ -62,20 +62,92 @@ SCENARIO("Test SimpleBatteryEstimator") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0); + trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } + WHEN("Robot moves 15km along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{15000, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > -1 && remaining_soc < 0.05; + CHECK(ok); + } +} + +SCENARIO("Test SimpleBatteryEstimator with RobotB") +{ + using SystemTraits = rmf_battery::agv::SystemTraits; + using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; + using PowerMap = rmf_battery::EstimateBattery::PowerMap; + using namespace std::chrono_literals; + + // Initializing system traits + SystemTraits::BatterySystem battery_system{24, 40, 2}; + REQUIRE(battery_system.valid()); + SystemTraits::MechanicalSystem mechanical_system{70, 30, 0.3}; + REQUIRE(mechanical_system.valid()); + SystemTraits::PowerSystem power_system_1{"processor", 10, 5}; + REQUIRE(power_system_1.valid()); + SystemTraits::PowerSystems power_systems; + power_systems.insert({power_system_1.name(), power_system_1}); + SystemTraits system_traits{ + mechanical_system, battery_system, power_systems}; + REQUIRE(system_traits.valid()); + + auto battery_estimator = SimpleBatteryEstimator{system_traits}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {1.0, 0.7}, {0.6, 0.5}, {nullptr, nullptr}); + + WHEN("Robot moves 100m along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{100, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; + CHECK(ok); + } + WHEN("Robot moves 20km along a straight line") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{20000, 0.0, 0.0}, + Eigen::Vector3d{10000, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -87,6 +159,7 @@ SCENARIO("Test SimpleBatteryEstimator") trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; - CHECK(remaining_soc == Approx(0.0)); + const bool ok = remaining_soc > -1.0 && remaining_soc < 0.05; + CHECK(ok); } -} \ No newline at end of file +} From f38c52d9f28b8449eaa2ec58369542b98f298455 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 20 Aug 2020 17:31:00 +0800 Subject: [PATCH 014/128] Removed message printouts --- .../src/rmf_battery/agv/SimpleBatteryEstimator.cpp | 6 +----- rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index b93d51cd2..a575fb738 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -23,7 +23,6 @@ #include #include -#include namespace rmf_battery { namespace agv { @@ -106,7 +105,6 @@ double SimpleBatteryEstimator::compute_state_of_charge( const Eigen::Vector3d velocity = motion->compute_velocity(start_time); double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); double w = velocity[2]; - // double KE_previous = compute_kinetic_energy(mass, v, inertia, w); const double sim_step = 0.1; // seconds start_time = rmf_traffic::time::apply_offset(start_time, sim_step); @@ -121,10 +119,8 @@ double SimpleBatteryEstimator::compute_state_of_charge( v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); w = velocity[2]; // Loss through kinetic energy - // We assume the robot does not coast and has regernerative braking const double KE = compute_kinetic_energy(mass, v, inertia, w); - // const double dKE = KE - KE_previous; - // KE_previous = KE; + // Loss through friction energy const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 71607ee20..9ce2442cd 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -68,7 +68,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") auto remaining_soc = battery_estimator.compute_state_of_charge( trajectory, 1.0, power_map); - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } @@ -89,7 +89,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") auto remaining_soc = battery_estimator.compute_state_of_charge( trajectory, 1.0, power_map); - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1 && remaining_soc < 0.05; CHECK(ok); } @@ -137,7 +137,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") auto remaining_soc = battery_estimator.compute_state_of_charge( trajectory, 1.0, power_map); - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; CHECK(ok); } @@ -158,7 +158,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") auto remaining_soc = battery_estimator.compute_state_of_charge( trajectory, 1.0, power_map); - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1.0 && remaining_soc < 0.05; CHECK(ok); } From be47d46d961a62a46455a3ad5fe04893a7a071e8 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 20 Aug 2020 22:27:43 +0800 Subject: [PATCH 015/128] Replaced KE with energy for acceleration --- .../agv/SimpleBatteryEstimator.cpp | 39 ++- .../unit/agv/test_SimpleBatteryEstimator.cpp | 323 +++++++++++------- 2 files changed, 217 insertions(+), 145 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index a575fb738..85f6d470e 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -24,6 +24,8 @@ #include #include +#include + namespace rmf_battery { namespace agv { @@ -70,7 +72,7 @@ double compute_friction_energy( const double dt) { const double g = 9.81; // ms-1 - return f * m * g * 2 * v * dt; + return f * m * g * v * dt; } } // namespace anonymous @@ -102,12 +104,12 @@ double SimpleBatteryEstimator::compute_state_of_charge( const auto motion = rmf_traffic::Motion::compute_cubic_splines( begin_it, trajectory.end()); - const Eigen::Vector3d velocity = motion->compute_velocity(start_time); - double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - double w = velocity[2]; + // const Eigen::Vector3d velocity = motion->compute_velocity(start_time); + // double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + // double w = velocity[2]; const double sim_step = 0.1; // seconds - start_time = rmf_traffic::time::apply_offset(start_time, sim_step); + // start_time = rmf_traffic::time::apply_offset(start_time, sim_step); double dE = 0.0; @@ -116,15 +118,20 @@ double SimpleBatteryEstimator::compute_state_of_charge( sim_time = rmf_traffic::time::apply_offset(sim_time, sim_step)) { const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); - v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - w = velocity[2]; - // Loss through kinetic energy - const double KE = compute_kinetic_energy(mass, v, inertia, w); - - // Loss through friction energy - const double FE = compute_friction_energy(friction, mass, v, (sim_step/1000.0)); - - double PE = 0.0; + const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); + const double w = velocity[2]; + + const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time); + const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2)); + const double alpha = acceleration[2]; + + // Loss through acceleration + const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; + // std::cout << "EA: " << EA << " alpha: " << alpha << " w: " << w << " v: " << v << std::endl; + // Loss through friction + const double EF = compute_friction_energy(friction, mass, v, sim_step); + + double EP = 0.0; // Loss through power systems if (power_map.has_value()) { @@ -134,11 +141,11 @@ double SimpleBatteryEstimator::compute_state_of_charge( { const auto it = power_systems.find(item.first); assert(it != power_systems.end()); - PE += it->second.nominal_power() * sim_step; + EP += it->second.nominal_power() * sim_step; } } } - dE += KE + FE + PE; + dE += EA + EF + EP; } // Computing the charge consumed diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 9ce2442cd..f5a12c7ed 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include @@ -28,138 +30,201 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") { - using SystemTraits = rmf_battery::agv::SystemTraits; - using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; - using PowerMap = rmf_battery::EstimateBattery::PowerMap; - using namespace std::chrono_literals; - - // Initializing system traits - SystemTraits::BatterySystem battery_system{12, 24, 2}; - REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.3}; - REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 5, 5}; - REQUIRE(power_system_1.valid()); - SystemTraits::PowerSystems power_systems; - power_systems.insert({power_system_1.name(), power_system_1}); - SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; - REQUIRE(system_traits.valid()); - - auto battery_estimator = SimpleBatteryEstimator{system_traits}; - - // Initializing vehicle traits - const rmf_traffic::agv::VehicleTraits traits( - {0.7, 0.5}, {0.3, 0.25}, {nullptr, nullptr}); - - WHEN("Robot moves 100m along a straight line") - { - const auto start_time = std::chrono::steady_clock::now(); - const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{100, 0.0, 0.0}, - }; - rmf_traffic::Trajectory trajectory = - rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); - - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; - const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; - CHECK(ok); - } - - WHEN("Robot moves 15km along a straight line") - { - const auto start_time = std::chrono::steady_clock::now(); - const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{15000, 0.0, 0.0}, - }; - rmf_traffic::Trajectory trajectory = - rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); - - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; - const bool ok = remaining_soc > -1 && remaining_soc < 0.05; - CHECK(ok); - } + using SystemTraits = rmf_battery::agv::SystemTraits; + using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; + using PowerMap = rmf_battery::EstimateBattery::PowerMap; + using namespace std::chrono_literals; + + // Initializing system traits + SystemTraits::BatterySystem battery_system{12, 24, 2}; + REQUIRE(battery_system.valid()); + SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.3}; + REQUIRE(mechanical_system.valid()); + SystemTraits::PowerSystem power_system_1{"processor", 15, 5}; + REQUIRE(power_system_1.valid()); + SystemTraits::PowerSystems power_systems; + power_systems.insert({power_system_1.name(), power_system_1}); + SystemTraits system_traits{ + mechanical_system, battery_system, power_systems}; + REQUIRE(system_traits.valid()); + + auto battery_estimator = SimpleBatteryEstimator{system_traits}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {0.7, 0.5}, {0.3, 0.25}, {nullptr, nullptr}); + + WHEN("Robot moves 100m along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{100, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; + CHECK(ok); + } + + WHEN("Robot moves 15km along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{15000, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > -1 && remaining_soc < 0.05; + CHECK(ok); + } } SCENARIO("Test SimpleBatteryEstimator with RobotB") { - using SystemTraits = rmf_battery::agv::SystemTraits; - using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; - using PowerMap = rmf_battery::EstimateBattery::PowerMap; - using namespace std::chrono_literals; - - // Initializing system traits - SystemTraits::BatterySystem battery_system{24, 40, 2}; - REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{70, 30, 0.3}; - REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 10, 5}; - REQUIRE(power_system_1.valid()); - SystemTraits::PowerSystems power_systems; - power_systems.insert({power_system_1.name(), power_system_1}); - SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; - REQUIRE(system_traits.valid()); - - auto battery_estimator = SimpleBatteryEstimator{system_traits}; - - // Initializing vehicle traits - const rmf_traffic::agv::VehicleTraits traits( - {1.0, 0.7}, {0.6, 0.5}, {nullptr, nullptr}); + using SystemTraits = rmf_battery::agv::SystemTraits; + using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; + using PowerMap = rmf_battery::EstimateBattery::PowerMap; + using namespace std::chrono_literals; + + // Initializing system traits + SystemTraits::BatterySystem battery_system{24, 40, 2}; + REQUIRE(battery_system.valid()); + SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.4}; + REQUIRE(mechanical_system.valid()); + SystemTraits::PowerSystem power_system_1{"processor", 60, 5}; + REQUIRE(power_system_1.valid()); + SystemTraits::PowerSystems power_systems; + power_systems.insert({power_system_1.name(), power_system_1}); + SystemTraits system_traits{ + mechanical_system, battery_system, power_systems}; + REQUIRE(system_traits.valid()); + + auto battery_estimator = SimpleBatteryEstimator{system_traits}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {1.0, 0.7}, {0.6, 0.5}, {nullptr, nullptr}); + + WHEN("Robot moves 100m along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{100, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; + CHECK(ok); + } + + WHEN("Robot moves 20km along a straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{10000, 0.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > -1.0 && remaining_soc < 0.05; + CHECK(ok); + } + + WHEN("Robot moves 20km along a square perimeter") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{4000, 0.0, 0.0}, + Eigen::Vector3d{4000, 4000.0, 0.0}, + Eigen::Vector3d{0.0, 4000.0, 0.0}, + Eigen::Vector3d{0.0, 0.0, 0.0} + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + } + + WHEN("Testing turning on a spot") + { + const auto start_time = std::chrono::steady_clock::now(); + rmf_traffic::Trajectory trajectory; + trajectory.insert( + start_time, + {0, 0, 0}, + {0, 0, 0}); + trajectory.insert( + start_time + 5s, + {5, 0, M_PI}, + {0, 0, 0}); + REQUIRE(trajectory.size() == 2); + // const auto motion = rmf_traffic::Motion::compute_cubic_splines( + // trajectory.begin(), trajectory.end()); + // auto begin_it = trajectory.begin(); + // auto end_it = --trajectory.end(); + + // for (auto sim_time = start_time; + // sim_time <= end_it->time(); + // sim_time = rmf_traffic::time::apply_offset(sim_time, 0.1)) - WHEN("Robot moves 100m along a straight line") - { - const auto start_time = std::chrono::steady_clock::now(); - const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{100, 0.0, 0.0}, - }; - rmf_traffic::Trajectory trajectory = - rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); - - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; - const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; - CHECK(ok); - } - - WHEN("Robot moves 20km along a straight line") - { - const auto start_time = std::chrono::steady_clock::now(); - const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{10000, 0.0, 0.0}, - }; - rmf_traffic::Trajectory trajectory = - rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); - - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; - const bool ok = remaining_soc > -1.0 && remaining_soc < 0.05; - CHECK(ok); - } + // { + // const auto a = motion->compute_acceleration(sim_time); + // std::cout << "ax: " << a[0] << " ay: " << a[1] << " aa: " << a[2] < power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; + CHECK(ok); + } + + } + From 73a832ef1a7b661ccc44d91552cac621d212ed73 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 21 Aug 2020 13:10:03 +0800 Subject: [PATCH 016/128] Added documentation and linted --- .../include/rmf_battery/EstimateBattery.hpp | 18 +++++- .../agv/SimpleBatteryEstimator.hpp | 22 ++++++- .../include/rmf_battery/agv/SystemTraits.hpp | 12 ++-- .../agv/SimpleBatteryEstimator.cpp | 31 ++++------ .../src/rmf_battery/agv/SystemTraits.cpp | 51 ++++++++-------- .../unit/agv/test_SimpleBatteryEstimator.cpp | 59 +++++++++---------- .../test/unit/agv/test_SystemTraits.cpp | 4 +- 7 files changed, 108 insertions(+), 89 deletions(-) diff --git a/rmf_battery/include/rmf_battery/EstimateBattery.hpp b/rmf_battery/include/rmf_battery/EstimateBattery.hpp index 088ff2e99..4dbffe77b 100644 --- a/rmf_battery/include/rmf_battery/EstimateBattery.hpp +++ b/rmf_battery/include/rmf_battery/EstimateBattery.hpp @@ -22,7 +22,7 @@ #include -#include +#include #include #include @@ -36,10 +36,22 @@ class EstimateBattery public: using PowerMap = std::unordered_map; - + /// Computes state-of-charge estimate of battery at the end of a trajectory. /// - /// \return The fraction of remaining charge at the end of the trajectory. + /// \param[in] trajectory + /// A valid rmf_traffic:::Trajectory + /// + /// \param[in] initial_soc + /// The initial state of charge of the robot at the beginning of the + /// trajectory as a fraction of total battery capacity + /// + /// \param[in] power_map + /// An optional unordered map with keys representing names of power systems + /// and values of trajectories during which the power system is active. + /// + /// \return The remaining charge at the end of the trajectory as a fraction of + /// total battery capacity virtual double compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, const double initial_soc, diff --git a/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp index 3a4aa5d6d..9d86a5ff2 100644 --- a/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp +++ b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp @@ -30,17 +30,33 @@ class SimpleBatteryEstimator : public EstimateBattery { public: + /// Constructor SimpleBatteryEstimator(SystemTraits& system_traits); SimpleBatteryEstimator& system_traits(SystemTraits system_traits); - const SystemTraits system_traits() const; + const SystemTraits system_traits() const; + /// Computes state-of-charge estimate of battery at the end of a trajectory. + /// + /// \param[in] trajectory + /// A valid rmf_traffic:::Trajectory + /// + /// \param[in] initial_soc + /// The initial state of charge of the robot at the beginning of the + /// trajectory as a fraction of total battery capacity + /// + /// \param[in] power_map + /// An optional unordered map with keys representing names of power systems + /// and values of trajectories during which the power system is active. + /// + /// \return The remaining charge at the end of the trajectory as a fraction of + /// total battery capacity double compute_state_of_charge( const rmf_traffic::Trajectory& trajectory, const double initial_soc, rmf_utils::optional power_map = rmf_utils::nullopt - ) const final; + ) const final; class Implementation; @@ -50,5 +66,5 @@ class SimpleBatteryEstimator : public EstimateBattery } // namespace agv } // namespace rmf_battery - + #endif // RMF_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP \ No newline at end of file diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp index ac3ebe5b0..b6fd711b0 100644 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -46,7 +46,7 @@ class SystemTraits PowerSystem& name(std::string name); std::string name() const; - + PowerSystem& nominal_power(double nom_power); double nominal_power() const; @@ -69,10 +69,10 @@ class SystemTraits public: enum class BatteryType : uint16_t { - /// The vehicle is powered by a Lead-Acid battery - LeadAcid, - /// The vehicle is powered by a Lithium-Ion battery - LiIon, + /// The vehicle is powered by a Lead-Acid battery + LeadAcid, + /// The vehicle is powered by a Lithium-Ion battery + LiIon, }; class BatteryProfile @@ -98,7 +98,7 @@ class SystemTraits /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; - + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index 85f6d470e..9b1c92fe8 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -15,8 +15,8 @@ * */ -#include -#include +#include +#include #include #include @@ -38,7 +38,7 @@ class SimpleBatteryEstimator::Implementation SimpleBatteryEstimator::SimpleBatteryEstimator( SystemTraits& system_traits) : _pimpl(rmf_utils::make_impl( - Implementation{std::move(system_traits)})) + Implementation{std::move(system_traits)})) { // Do nothing } @@ -62,7 +62,7 @@ double compute_kinetic_energy( const double i, const double w) { - return 0.5 * (m*pow(v, 2) + i*pow(w, 2)); + return 0.5 * (m*pow(v, 2) + i*pow(w, 2)); } double compute_friction_energy( @@ -82,14 +82,12 @@ double SimpleBatteryEstimator::compute_state_of_charge( rmf_utils::optional power_map) const { assert(_pimpl->system_traits.valid()); - std::vector trajectory_soc; - trajectory_soc.reserve(trajectory.size()); - trajectory_soc.push_back(initial_soc); double battery_soc = initial_soc; - double nominal_capacity = + double nominal_capacity = _pimpl->system_traits.battery_system().nominal_capacity(); - double nominal_voltage = _pimpl->system_traits.battery_system().nominal_voltage(); + double nominal_voltage = + _pimpl->system_traits.battery_system().nominal_voltage(); const double mass = _pimpl->system_traits.mechanical_system().mass(); const double inertia = _pimpl->system_traits.mechanical_system().inertia(); const double friction = @@ -102,17 +100,13 @@ double SimpleBatteryEstimator::compute_state_of_charge( auto start_time = begin_it->time(); const auto end_time = end_it->time(); const auto motion = rmf_traffic::Motion::compute_cubic_splines( - begin_it, trajectory.end()); - - // const Eigen::Vector3d velocity = motion->compute_velocity(start_time); - // double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - // double w = velocity[2]; + begin_it, trajectory.end()); const double sim_step = 0.1; // seconds - // start_time = rmf_traffic::time::apply_offset(start_time, sim_step); double dE = 0.0; + // TODO explore analytical solutions as opposed to numerical integration for (auto sim_time = start_time; sim_time <= end_time; sim_time = rmf_traffic::time::apply_offset(sim_time, sim_step)) @@ -120,7 +114,7 @@ double SimpleBatteryEstimator::compute_state_of_charge( const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); const double w = velocity[2]; - + const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time); const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2)); const double alpha = acceleration[2]; @@ -144,16 +138,15 @@ double SimpleBatteryEstimator::compute_state_of_charge( EP += it->second.nominal_power() * sim_step; } } - } + } dE += EA + EF + EP; } // Computing the charge consumed const double dQ = dE / nominal_voltage; battery_soc -= dQ / (nominal_capacity * 3600); - trajectory_soc.push_back(battery_soc); - return trajectory_soc.back(); + return battery_soc; } } // namespace agv diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index 8ccad4f95..46c76f4aa 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -39,7 +39,8 @@ SystemTraits::PowerSystem::PowerSystem( const double nominal_voltage, const double efficiency) : _pimpl(rmf_utils::make_impl( - Implementation{std::move(name), nominal_power, nominal_voltage, efficiency})) + Implementation{std::move(name), nominal_power, nominal_voltage, + efficiency})) { // Do nothing } @@ -100,7 +101,7 @@ double SystemTraits::PowerSystem::efficiency() const //============================================================================== bool SystemTraits::PowerSystem::valid() const { - return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0 + return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0 && _pimpl->nominal_voltage > 0.0 && _pimpl->efficiency > 0.0; } @@ -121,7 +122,7 @@ SystemTraits::BatterySystem::BatteryProfile::BatteryProfile( const double exp_voltage, const double exp_capacity) : _pimpl(rmf_utils::make_impl( - Implementation{resistance, max_voltage, exp_voltage, exp_capacity})) + Implementation{resistance, max_voltage, exp_voltage, exp_capacity})) { // Do nothing } @@ -208,13 +209,13 @@ SystemTraits::BatterySystem::BatterySystem( BatteryType type, rmf_utils::optional profile) : _pimpl(rmf_utils::make_impl( - Implementation{ - nominal_voltage, - nominal_capacity, - charging_current, - type, - std::move(profile) - })) + Implementation{ + nominal_voltage, + nominal_capacity, + charging_current, + type, + std::move(profile) + })) { // Do nothing } @@ -295,13 +296,13 @@ bool SystemTraits::BatterySystem::valid() const { bool valid = _pimpl->nominal_voltage > 0.0 && _pimpl->nominal_capacity > 0.0 && _pimpl->charging_current > 0.0 && - (_pimpl->type == BatteryType::LeadAcid - || _pimpl->type == BatteryType::LiIon); + (_pimpl->type == BatteryType::LeadAcid + || _pimpl->type == BatteryType::LiIon); if (_pimpl->profile) return valid && _pimpl->profile->valid(); - + return valid; - + } //============================================================================== @@ -319,11 +320,11 @@ SystemTraits::MechanicalSystem::MechanicalSystem( const double inertia, const double friction_coefficient) : _pimpl(rmf_utils::make_impl( - Implementation{ - mass, - inertia, - friction_coefficient, - })) + Implementation{ + mass, + inertia, + friction_coefficient, + })) { // Do nothing } @@ -391,12 +392,12 @@ SystemTraits::SystemTraits( const BatterySystem battery_system, const PowerSystems power_systems) : _pimpl(rmf_utils::make_impl( - Implementation - { - std::move(mechanical_system), - std::move(battery_system), - std::move(power_systems) - })) + Implementation + { + std::move(mechanical_system), + std::move(battery_system), + std::move(power_systems) + })) { // Do nothing } diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index f5a12c7ed..4782de509 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -40,26 +40,26 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") REQUIRE(battery_system.valid()); SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.3}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 15, 5}; + SystemTraits::PowerSystem power_system_1{"processor", 10, 5}; REQUIRE(power_system_1.valid()); SystemTraits::PowerSystems power_systems; power_systems.insert({power_system_1.name(), power_system_1}); SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; + mechanical_system, battery_system, power_systems}; REQUIRE(system_traits.valid()); auto battery_estimator = SimpleBatteryEstimator{system_traits}; // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( - {0.7, 0.5}, {0.3, 0.25}, {nullptr, nullptr}); - + {0.7, 0.5}, {0.3, 0.25}, {nullptr, nullptr}); + WHEN("Robot moves 100m along a straight line") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{100, 0.0, 0.0}, + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{100, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -68,7 +68,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") power_map.value().insert({"processor", trajectory}); auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; @@ -79,8 +79,8 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{15000, 0.0, 0.0}, + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{15000, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -89,7 +89,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") power_map.value().insert({"processor", trajectory}); auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1 && remaining_soc < 0.05; @@ -114,21 +114,21 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") SystemTraits::PowerSystems power_systems; power_systems.insert({power_system_1.name(), power_system_1}); SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; + mechanical_system, battery_system, power_systems}; REQUIRE(system_traits.valid()); auto battery_estimator = SimpleBatteryEstimator{system_traits}; // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( - {1.0, 0.7}, {0.6, 0.5}, {nullptr, nullptr}); - + {1.0, 0.7}, {0.6, 0.5}, {nullptr, nullptr}); + WHEN("Robot moves 100m along a straight line") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{100, 0.0, 0.0}, + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{100, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -137,7 +137,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") power_map.value().insert({"processor", trajectory}); auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; @@ -148,8 +148,8 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{10000, 0.0, 0.0}, + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{10000, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -158,7 +158,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") power_map.value().insert({"processor", trajectory}); auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1.0 && remaining_soc < 0.05; @@ -169,11 +169,11 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { - Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{4000, 0.0, 0.0}, - Eigen::Vector3d{4000, 4000.0, 0.0}, - Eigen::Vector3d{0.0, 4000.0, 0.0}, - Eigen::Vector3d{0.0, 0.0, 0.0} + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{4000, 0.0, 0.0}, + Eigen::Vector3d{4000, 4000.0, 0.0}, + Eigen::Vector3d{0.0, 4000.0, 0.0}, + Eigen::Vector3d{0.0, 0.0, 0.0} }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -182,7 +182,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") power_map.value().insert({"processor", trajectory}); auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; } @@ -204,11 +204,11 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") // trajectory.begin(), trajectory.end()); // auto begin_it = trajectory.begin(); // auto end_it = --trajectory.end(); - + // for (auto sim_time = start_time; // sim_time <= end_it->time(); // sim_time = rmf_traffic::time::apply_offset(sim_time, 0.1)) - + // { // const auto a = motion->compute_acceleration(sim_time); // std::cout << "ax: " << a[0] << " ay: " << a[1] << " aa: " << a[2] < 0.99 && remaining_soc < 1.0; CHECK(ok); } - - } - diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index d1f46ae69..6694617f7 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -83,5 +83,5 @@ SCENARIO("Test SystemTraits") mechanical_system, battery_system, power_systems}; REQUIRE(system_traits.valid()); - // TODO(YV): Tests for getters and setters -} \ No newline at end of file + // TODO(YV): Tests for getters and setters +} From 257ff632beb452207d4487ef10acd2f08eb95455 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 21 Aug 2020 17:19:01 +0800 Subject: [PATCH 017/128] Added CleaningTaskPlanner --- .../rmf_battery/agv/CleaningTaskPlanner.hpp | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp diff --git a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp new file mode 100644 index 000000000..4e9b8e69c --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp @@ -0,0 +1,98 @@ +/* + * 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_BATTERY__AGV__CLEANINGTASKPLANNER_HPP +#define RMF_BATTERY__AGV__CLEANINGTASKPLANNER_HPP + +#include +#include + +#include + +#include + +#include + +namespace rmf_battery { +namespace agv { + +class CleaningTaskPlanner +{ +public: + using Planner = rmf_traffic::agv::Planner; + /// Constructor + /// + /// \param[in] system_traits + /// An instance of rmf_battery::agv::SystemTraits. + /// + /// \param[in] planner + /// An instance of rmf_traffic::agv::Planner. A valid reference is to be + /// maintained by the user. + CleaningTaskPlanner( + SystemTraits& system_traits, + Planner& planner); + + /// Get a const reference to the rmf_battery::agv::SystemTraits instance + const SystemTraits& system_traits() const; + + /// Get a const reference to the rmf_traffic::agv::Planner instance + const Planner& planner() const; + + /// Produce a set of valid trajectories if the robot can successfully travel + /// from start location to the cleaning waypoint, complete the cleaning + /// process and has sufficent battery to return to its nearest charging + /// waypoint if required. If no feasible plan is found, an empty set is + /// returned + /// + /// \param[in] start + /// The starting conditions + /// + /// \param[in] cleaning_start_waypoint + /// The waypoint where the robot needs to reach to begin cleaning + /// + /// \param[in] cleaning_trajectory + /// The trajectory to be followed by the robot while cleaning + /// + /// \param[in] cleaning_system + /// The name of the cleaning system. The name must match one of the power + /// systems of the robot as configured in SystemTraits. + /// + /// \param[in] cleaning_end_waypoint + /// The waypoint where the robot stops after cleaning + /// + /// \param[in] charging_station_waypoint + /// The nearest charging station waypoint for the robot + /// + std::vector plan( + const Planner::Start& start, + const std::size_t cleaning_start_waypoint, + const rmf_traffic::Trajectory& cleaning_trajectory, + const std::string& cleaning_system, + const std::size_t cleaning_end_waypoint, + const std::size_t charging_station_waypoint); + + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; +}; + + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__AGV__CLEANINGTASKPLANNER_HPP From 6a0a84ff21664522883fc64637d19c534684b686 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 21 Aug 2020 17:56:23 +0800 Subject: [PATCH 018/128] Added CleaningTaskPlanner.cpp and corrected unit test --- .../rmf_battery/agv/CleaningTaskPlanner.cpp | 27 ++++++++++ .../unit/agv/test_SimpleBatteryEstimator.cpp | 50 +++++++++++-------- 2 files changed, 56 insertions(+), 21 deletions(-) create mode 100644 rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp diff --git a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp new file mode 100644 index 000000000..5be7d9ce4 --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp @@ -0,0 +1,27 @@ +/* + * 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 + +namespace rmf_battery { +namespace agv { + + + + +} // namespace agv +} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index 4782de509..c09dfc620 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -107,9 +107,9 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") // Initializing system traits SystemTraits::BatterySystem battery_system{24, 40, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.4}; + SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.2}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 60, 5}; + SystemTraits::PowerSystem power_system_1{"processor", 20, 5}; REQUIRE(power_system_1.valid()); SystemTraits::PowerSystems power_systems; power_systems.insert({power_system_1.name(), power_system_1}); @@ -144,12 +144,12 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") CHECK(ok); } - WHEN("Robot moves 20km along a straight line") + WHEN("Robot moves 20km along a horizontal straight line") { const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{10000, 0.0, 0.0}, + Eigen::Vector3d{20000, 0.0, 0.0}, }; rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); @@ -161,7 +161,28 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") trajectory, 1.0, power_map); std::cout << "Remaining soc: " << remaining_soc << std::endl; - const bool ok = remaining_soc > -1.0 && remaining_soc < 0.05; + const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; + CHECK(ok); + } + + WHEN("Robot moves 20km along a vertical straight line") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{0.0, 0.0, 0.0}, + Eigen::Vector3d{0.0, 20000.0, 0.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + rmf_utils::optional power_map = PowerMap(); + power_map.value().insert({"processor", trajectory}); + + auto remaining_soc = battery_estimator.compute_state_of_charge( + trajectory, 1.0, power_map); + + std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; CHECK(ok); } @@ -170,9 +191,9 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") const auto start_time = std::chrono::steady_clock::now(); const std::vector positions = { Eigen::Vector3d{0.0, 0.0, 0.0}, - Eigen::Vector3d{4000, 0.0, 0.0}, - Eigen::Vector3d{4000, 4000.0, 0.0}, - Eigen::Vector3d{0.0, 4000.0, 0.0}, + Eigen::Vector3d{5000, 0.0, 0.0}, + Eigen::Vector3d{5000, 5000.0, 0.0}, + Eigen::Vector3d{0.0, 5000.0, 0.0}, Eigen::Vector3d{0.0, 0.0, 0.0} }; rmf_traffic::Trajectory trajectory = @@ -200,20 +221,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") {5, 0, M_PI}, {0, 0, 0}); REQUIRE(trajectory.size() == 2); - // const auto motion = rmf_traffic::Motion::compute_cubic_splines( - // trajectory.begin(), trajectory.end()); - // auto begin_it = trajectory.begin(); - // auto end_it = --trajectory.end(); - - // for (auto sim_time = start_time; - // sim_time <= end_it->time(); - // sim_time = rmf_traffic::time::apply_offset(sim_time, 0.1)) - - // { - // const auto a = motion->compute_acceleration(sim_time); - // std::cout << "ax: " << a[0] << " ay: " << a[1] << " aa: " << a[2] < power_map = PowerMap(); power_map.value().insert({"processor", trajectory}); From d7fd00b840f3c3aaa2651109fcef2f102646769e Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 22 Aug 2020 19:09:33 +0800 Subject: [PATCH 019/128] Added CleaningTaskPlanner Signed-off-by: Yadunund Vijay --- .../rmf_battery/agv/CleaningTaskPlanner.hpp | 27 ++- .../rmf_battery/agv/CleaningTaskPlanner.cpp | 163 ++++++++++++++++++ .../src/rmf_battery/agv/SystemTraits.cpp | 1 + .../unit/agv/test_SimpleBatteryEstimator.cpp | 2 +- 4 files changed, 186 insertions(+), 7 deletions(-) diff --git a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp index 4e9b8e69c..a66b51b1a 100644 --- a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp +++ b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp @@ -42,9 +42,19 @@ class CleaningTaskPlanner /// \param[in] planner /// An instance of rmf_traffic::agv::Planner. A valid reference is to be /// maintained by the user. + /// + /// \param[in] cleaning_system + /// The name of the cleaning system. The name must match one of the power + /// system names as configured in SystemTraits. + /// + /// \param[in] battery_threshold + /// The minimum allowable state of charge of the battery as a fraction the + /// total charge capacity CleaningTaskPlanner( SystemTraits& system_traits, - Planner& planner); + Planner& planner, + std::string& cleaning_system, + double battery_threshold = 0.0); /// Get a const reference to the rmf_battery::agv::SystemTraits instance const SystemTraits& system_traits() const; @@ -52,6 +62,12 @@ class CleaningTaskPlanner /// Get a const reference to the rmf_traffic::agv::Planner instance const Planner& planner() const; + /// Get a const reference to the name of the cleaning system + const std::string& cleaning_system() const; + + /// Get a const reference to the battery threshold + const double battery_threshold() const; + /// Produce a set of valid trajectories if the robot can successfully travel /// from start location to the cleaning waypoint, complete the cleaning /// process and has sufficent battery to return to its nearest charging @@ -61,16 +77,15 @@ class CleaningTaskPlanner /// \param[in] start /// The starting conditions /// + /// \param[in] initial_soc + /// The state of charge of the robot at its start location + /// /// \param[in] cleaning_start_waypoint /// The waypoint where the robot needs to reach to begin cleaning /// /// \param[in] cleaning_trajectory /// The trajectory to be followed by the robot while cleaning /// - /// \param[in] cleaning_system - /// The name of the cleaning system. The name must match one of the power - /// systems of the robot as configured in SystemTraits. - /// /// \param[in] cleaning_end_waypoint /// The waypoint where the robot stops after cleaning /// @@ -79,9 +94,9 @@ class CleaningTaskPlanner /// std::vector plan( const Planner::Start& start, + const double initial_soc, const std::size_t cleaning_start_waypoint, const rmf_traffic::Trajectory& cleaning_trajectory, - const std::string& cleaning_system, const std::size_t cleaning_end_waypoint, const std::size_t charging_station_waypoint); diff --git a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp index 5be7d9ce4..8e10cb303 100644 --- a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp +++ b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp @@ -16,12 +16,175 @@ */ #include +#include + +#include + +#include namespace rmf_battery { namespace agv { +class CleaningTaskPlanner::Implementation +{ +public: + SystemTraits system_traits; + Planner planner; + std::string cleaning_system; + double battery_threshold; +}; + +//============================================================================== +CleaningTaskPlanner::CleaningTaskPlanner( + SystemTraits& system_traits, + Planner& planner, + std::string& cleaning_system, + double battery_threshold) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(system_traits), + std::move(planner), + std::move(cleaning_system), + battery_threshold + })) +{ + assert(battery_threshold >= 0.0 && battery_threshold <= 1.0); +} + +//============================================================================== +const SystemTraits& CleaningTaskPlanner::system_traits() const +{ + return _pimpl->system_traits; +} + +//============================================================================== +const rmf_traffic::agv::Planner& CleaningTaskPlanner::planner() const +{ + return _pimpl->planner; +} + +//============================================================================== +const std::string& CleaningTaskPlanner::cleaning_system() const +{ + return _pimpl->cleaning_system; +} + +//============================================================================== +const double CleaningTaskPlanner::battery_threshold() const +{ + return _pimpl->battery_threshold; +} +//============================================================================== +namespace { +double get_battery_soc_after_itinerary( + SimpleBatteryEstimator& battery_estimator, + std::vector itinerary, + SystemTraits& system_traits, + std::string& cleaning_system, + double initial_soc) +{ + double battery_soc = initial_soc; + for (const auto& route : itinerary) + { + // TODO(YV): allow users to state which power systems are active during non- + // cleaning routes + rmf_utils::optional power_map = + SimpleBatteryEstimator::PowerMap(); + for (const auto& power_system : system_traits.power_systems()) + { + if (power_system.first == cleaning_system) + continue; + + power_map.value().insert({power_system.first, route.trajectory()}); + } + battery_soc = battery_estimator.compute_state_of_charge( + route.trajectory(), battery_soc, power_map); + } + + return battery_soc; +} + +} // anonymous + +//============================================================================== +std::vector CleaningTaskPlanner::plan( + const Planner::Start& start, + const double initial_soc, + const std::size_t cleaning_start_waypoint, + const rmf_traffic::Trajectory& cleaning_trajectory, + const std::size_t cleaning_end_waypoint, + const std::size_t charging_station_waypoint) +{ + // Ensure that the waypoint indices are valid + const auto& graph = _pimpl->planner.get_configuration().graph(); + std::size_t num_waypoints = graph.num_waypoints(); + assert(start.waypoint() < num_waypoints); + assert(cleaning_start_waypoint < num_waypoints); + assert(cleaning_end_waypoint < num_waypoints); + assert(charging_station_waypoint < num_waypoints); + assert(_pimpl->system_traits.power_systems().find(_pimpl->cleaning_system) + != _pimpl->system_traits.power_systems().end()); + + std::vector cleaning_plan; + double battery_soc = initial_soc; + SimpleBatteryEstimator battery_estimator(_pimpl->system_traits); + + Planner::Goal goal_1{cleaning_start_waypoint}; + const auto plan_to_cleaning_start = _pimpl->planner.plan(start, goal_1); + // Stage 1: check if robot has charge to reach cleaning start waypoint + if (!plan_to_cleaning_start.success()) + { + std::cout << "Failed to find plan from start to cleaning start waypoint" + << std::endl; + return cleaning_plan; + } + auto itinerary_1 = plan_to_cleaning_start->get_itinerary(); + // Compute battery drain for each route + battery_soc = get_battery_soc_after_itinerary(battery_estimator, + itinerary_1, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); + + if (battery_soc < _pimpl->battery_threshold) + return cleaning_plan; + + // Stage 2: check if robot has charge to finish cleaning + rmf_utils::optional power_map = + SimpleBatteryEstimator::PowerMap(); + for (const auto& system : _pimpl->system_traits.power_systems()) + power_map.value().insert({system.first, cleaning_trajectory}); + battery_soc = battery_estimator.compute_state_of_charge( + cleaning_trajectory, battery_soc, power_map); + + if (battery_soc < _pimpl->battery_threshold) + return cleaning_plan; + + // Stage 3: check if robot has charge to theoretically travel to its charger + auto end_it = cleaning_trajectory.end(); --end_it; + Planner::Start start_2{end_it->time(), + cleaning_end_waypoint, end_it->position()[2]}; + Planner::Goal goal_2{charging_station_waypoint}; + const auto plan_to_charger = _pimpl->planner.plan(start_2, goal_2); + + if (!plan_to_charger.success()) + { + std::cout << "Failed to find plan from cleaning end to charger" + << std::endl; + return cleaning_plan; + } + + auto itinerary_2 = plan_to_charger->get_itinerary(); + battery_soc = get_battery_soc_after_itinerary(battery_estimator, + itinerary_2, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); + + if (battery_soc < _pimpl->battery_threshold) + return cleaning_plan; + // A successful can be produced + for (const auto& route : itinerary_1) + cleaning_plan.push_back(route.trajectory()); + cleaning_plan.push_back(cleaning_trajectory); + return cleaning_plan; +} } // namespace agv } // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index 46c76f4aa..52b612dec 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -444,6 +444,7 @@ const SystemTraits::PowerSystems SystemTraits::power_systems() const return _pimpl->power_systems; } +//============================================================================== bool SystemTraits::valid() const { bool valid = true; diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp index c09dfc620..153c32c25 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp @@ -107,7 +107,7 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") // Initializing system traits SystemTraits::BatterySystem battery_system{24, 40, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.2}; + SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.22}; REQUIRE(mechanical_system.valid()); SystemTraits::PowerSystem power_system_1{"processor", 20, 5}; REQUIRE(power_system_1.valid()); From 05ab68d9337808b7cc11e0aa019ae6a2f2e9e089 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 24 Aug 2020 12:11:30 +0800 Subject: [PATCH 020/128] Added virtual destructor Signed-off-by: Yadunund Vijay --- rmf_battery/include/rmf_battery/EstimateBattery.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_battery/include/rmf_battery/EstimateBattery.hpp b/rmf_battery/include/rmf_battery/EstimateBattery.hpp index 4dbffe77b..0537a4a81 100644 --- a/rmf_battery/include/rmf_battery/EstimateBattery.hpp +++ b/rmf_battery/include/rmf_battery/EstimateBattery.hpp @@ -57,6 +57,7 @@ class EstimateBattery const double initial_soc, rmf_utils::optional power_map = rmf_utils::nullopt) const = 0; + virtual ~EstimateBattery() = default; }; } // namespace rmf_traffic From 0f1a1bf5f67224f4d977eeedff8582ee3d862040 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 24 Aug 2020 14:20:38 +0800 Subject: [PATCH 021/128] Debugging Signed-off-by: Yadunund Vijay --- .../rmf_battery/agv/CleaningTaskPlanner.hpp | 4 +- .../include/rmf_battery/agv/SystemTraits.hpp | 8 +- .../rmf_battery/agv/CleaningTaskPlanner.cpp | 23 ++-- .../src/rmf_battery/agv/SystemTraits.cpp | 10 +- .../unit/agv/test_CleaningTaskPlanner.cpp | 127 ++++++++++++++++++ 5 files changed, 152 insertions(+), 20 deletions(-) create mode 100644 rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp diff --git a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp index a66b51b1a..207c0aa34 100644 --- a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp +++ b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp @@ -53,8 +53,8 @@ class CleaningTaskPlanner CleaningTaskPlanner( SystemTraits& system_traits, Planner& planner, - std::string& cleaning_system, - double battery_threshold = 0.0); + const std::string& cleaning_system, + const double battery_threshold = 0.0); /// Get a const reference to the rmf_battery::agv::SystemTraits instance const SystemTraits& system_traits() const; diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp index b6fd711b0..9df273708 100644 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp @@ -45,7 +45,7 @@ class SystemTraits double efficiency = 1.0); PowerSystem& name(std::string name); - std::string name() const; + const std::string& name() const; PowerSystem& nominal_power(double nom_power); double nominal_power() const; @@ -167,13 +167,13 @@ class SystemTraits PowerSystems power_systems); SystemTraits& mechanical_system(MechanicalSystem mechanical_system); - const MechanicalSystem mechanical_system() const; + const MechanicalSystem& mechanical_system() const; SystemTraits& battery_system(BatterySystem battery_system); - const BatterySystem battery_system() const; + const BatterySystem& battery_system() const; SystemTraits& power_systems(PowerSystems power_systems); - const PowerSystems power_systems() const; + const PowerSystems& power_systems() const; /// Returns true if the values of the traits are valid. For example, this /// means that all system values are greater than zero. diff --git a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp index 8e10cb303..f23d82fe8 100644 --- a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp +++ b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp @@ -38,8 +38,8 @@ class CleaningTaskPlanner::Implementation CleaningTaskPlanner::CleaningTaskPlanner( SystemTraits& system_traits, Planner& planner, - std::string& cleaning_system, - double battery_threshold) + const std::string& cleaning_system, + const double battery_threshold) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(system_traits), @@ -77,11 +77,11 @@ const double CleaningTaskPlanner::battery_threshold() const //============================================================================== namespace { double get_battery_soc_after_itinerary( - SimpleBatteryEstimator& battery_estimator, - std::vector itinerary, - SystemTraits& system_traits, - std::string& cleaning_system, - double initial_soc) + const SimpleBatteryEstimator& battery_estimator, + const std::vector& itinerary, + const SystemTraits& system_traits, + const std::string& cleaning_system, + const double initial_soc) { double battery_soc = initial_soc; for (const auto& route : itinerary) @@ -90,7 +90,7 @@ double get_battery_soc_after_itinerary( // cleaning routes rmf_utils::optional power_map = SimpleBatteryEstimator::PowerMap(); - for (const auto& power_system : system_traits.power_systems()) + for (const auto power_system : system_traits.power_systems()) { if (power_system.first == cleaning_system) continue; @@ -143,6 +143,7 @@ std::vector CleaningTaskPlanner::plan( battery_soc = get_battery_soc_after_itinerary(battery_estimator, itinerary_1, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); + std::cout << "Battery soc after traversing to cleaning start: " << battery_soc; if (battery_soc < _pimpl->battery_threshold) return cleaning_plan; @@ -154,6 +155,8 @@ std::vector CleaningTaskPlanner::plan( battery_soc = battery_estimator.compute_state_of_charge( cleaning_trajectory, battery_soc, power_map); + std::cout << "Battery soc after cleaning: " << battery_soc; + if (battery_soc < _pimpl->battery_threshold) return cleaning_plan; @@ -175,10 +178,12 @@ std::vector CleaningTaskPlanner::plan( battery_soc = get_battery_soc_after_itinerary(battery_estimator, itinerary_2, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); + std::cout << "Battery after returning to charger: " << battery_soc; + if (battery_soc < _pimpl->battery_threshold) return cleaning_plan; - // A successful can be produced + // A successful plan can be produced for (const auto& route : itinerary_1) cleaning_plan.push_back(route.trajectory()); cleaning_plan.push_back(cleaning_trajectory); diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp index 52b612dec..81245f606 100644 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp @@ -39,7 +39,7 @@ SystemTraits::PowerSystem::PowerSystem( const double nominal_voltage, const double efficiency) : _pimpl(rmf_utils::make_impl( - Implementation{std::move(name), nominal_power, nominal_voltage, + Implementation{name, nominal_power, nominal_voltage, efficiency})) { // Do nothing @@ -53,7 +53,7 @@ auto SystemTraits::PowerSystem::name(std::string name) -> PowerSystem& } //============================================================================== -std::string SystemTraits::PowerSystem::name() const +const std::string& SystemTraits::PowerSystem::name() const { return _pimpl->name; } @@ -411,7 +411,7 @@ auto SystemTraits::mechanical_system(MechanicalSystem mechanical_system) } //============================================================================== -const SystemTraits::MechanicalSystem SystemTraits::mechanical_system() const +const SystemTraits::MechanicalSystem& SystemTraits::mechanical_system() const { return _pimpl->mechanical_system; } @@ -425,7 +425,7 @@ auto SystemTraits::battery_system(BatterySystem battery_system) } //============================================================================== -const SystemTraits::BatterySystem SystemTraits::battery_system() const +const SystemTraits::BatterySystem& SystemTraits::battery_system() const { return _pimpl->battery_system; } @@ -439,7 +439,7 @@ auto SystemTraits::power_systems(PowerSystems power_systems) } //============================================================================== -const SystemTraits::PowerSystems SystemTraits::power_systems() const +const SystemTraits::PowerSystems& SystemTraits::power_systems() const { return _pimpl->power_systems; } diff --git a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp new file mode 100644 index 000000000..4b74f4cab --- /dev/null +++ b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp @@ -0,0 +1,127 @@ +/* + * 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 + +#include +#include +#include + + +#include +#include + +rmf_utils::clone_ptr +make_test_schedule_validator( + const rmf_traffic::schedule::Viewer& viewer, + rmf_traffic::Profile profile) +{ + return rmf_utils::make_clone( + viewer, + std::numeric_limits::max(), + std::move(profile)); +} + +SCENARIO("Test CleaningTaskPlanner") +{ + using namespace std::chrono_literals; + using Graph = rmf_traffic::agv::Graph; + using VehicleTraits = rmf_traffic::agv::VehicleTraits; + using Interpolate = rmf_traffic::agv::Interpolate; + using Planner = rmf_traffic::agv::Planner; + using CleaningTaskPlanner = rmf_battery::agv::CleaningTaskPlanner; + using SystemTraits = rmf_battery::agv::SystemTraits; + + 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_options = rmf_traffic::agv::Planner::Options{ + make_test_schedule_validator(database, profile)}; + + // Setup graph + Graph graph; + // 3--------0------1 + // | + // | + // 2 + const std::string map_name = "L1"; + graph.add_waypoint(map_name, {0, 0}).set_holding_point(true); // 0 + graph.add_waypoint(map_name, {10000, 0}).set_holding_point(true); // 1 + graph.add_waypoint(map_name, {0, -10000}).set_holding_point(true); // 2 + graph.add_waypoint(map_name, {-20000, 0}).set_holding_point(true); // 3 + + graph.add_lane(0, 1); + graph.add_lane(1, 0); + graph.add_lane(0, 2); + graph.add_lane(2, 0); + graph.add_lane(0, 3); + graph.add_lane(3, 0); + + + rmf_traffic::agv::Planner planner{ + rmf_traffic::agv::Planner::Configuration{graph, traits}, + default_options + }; + + SystemTraits::BatterySystem battery_system{24, 40, 2}; + SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.22}; + SystemTraits::PowerSystem power_system_1{"processor", 20, 5}; + SystemTraits::PowerSystem power_system_2{"cleaning", 30, 5}; + SystemTraits::PowerSystems power_systems; + power_systems.insert({power_system_1.name(), power_system_1}); + power_systems.insert({power_system_2.name(), power_system_2}); + SystemTraits system_traits{ + mechanical_system, battery_system, power_systems}; + REQUIRE(system_traits.valid()); + + CleaningTaskPlanner cleaning_planner{ + system_traits, planner, power_system_2.name()}; + + const auto start_time = std::chrono::steady_clock::now(); + + WHEN("Cleaning zone 1") + { + Planner::Start start{start_time, 0, 0.0}; // Start at 0 + rmf_traffic::Trajectory cleaning_trajectory = + rmf_traffic::agv::Interpolate::positions( + traits, + start_time + 15000s, + {{15000, 0, 0}, {15000, 100, 0}, {15000, 0, 0}} + ); + + const auto itinerary = cleaning_planner.plan( + start, + 1.0, + 1, + cleaning_trajectory, + 1, + 0 + ); + + CHECK(itinerary.size() > 0); + + + } + +} \ No newline at end of file From 8f51274600d1307c44a9d4d49d52d58aefdb25bf Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 24 Aug 2020 15:17:28 +0800 Subject: [PATCH 022/128] Fixed test for CleaningTaskPlanner Signed-off-by: Yadunund Vijay --- rmf_battery/CMakeLists.txt | 3 +++ .../src/rmf_battery/agv/CleaningTaskPlanner.cpp | 11 +++++++---- .../src/rmf_battery/agv/SimpleBatteryEstimator.cpp | 2 +- .../test/unit/agv/test_CleaningTaskPlanner.cpp | 6 +++--- rmf_battery/test/unit/agv/test_SystemTraits.cpp | 6 ++++++ 5 files changed, 20 insertions(+), 8 deletions(-) diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index dc0098f91..4f2d12663 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -10,6 +10,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") + include(GNUInstallDirs) # find dependencies diff --git a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp index f23d82fe8..9d84ed977 100644 --- a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp +++ b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp @@ -42,7 +42,7 @@ CleaningTaskPlanner::CleaningTaskPlanner( const double battery_threshold) : _pimpl(rmf_utils::make_impl( Implementation{ - std::move(system_traits), + system_traits, std::move(planner), std::move(cleaning_system), battery_threshold @@ -143,7 +143,8 @@ std::vector CleaningTaskPlanner::plan( battery_soc = get_battery_soc_after_itinerary(battery_estimator, itinerary_1, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); - std::cout << "Battery soc after traversing to cleaning start: " << battery_soc; + std::cout << "Battery soc after traversing to cleaning start: " + << battery_soc << std::endl; if (battery_soc < _pimpl->battery_threshold) return cleaning_plan; @@ -155,7 +156,8 @@ std::vector CleaningTaskPlanner::plan( battery_soc = battery_estimator.compute_state_of_charge( cleaning_trajectory, battery_soc, power_map); - std::cout << "Battery soc after cleaning: " << battery_soc; + std::cout << "Battery soc after cleaning: " << battery_soc + << std::endl; if (battery_soc < _pimpl->battery_threshold) return cleaning_plan; @@ -178,7 +180,8 @@ std::vector CleaningTaskPlanner::plan( battery_soc = get_battery_soc_after_itinerary(battery_estimator, itinerary_2, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); - std::cout << "Battery after returning to charger: " << battery_soc; + std::cout << "Battery after returning to charger: " << battery_soc + <battery_threshold) return cleaning_plan; diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index 9b1c92fe8..9277c65a7 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -38,7 +38,7 @@ class SimpleBatteryEstimator::Implementation SimpleBatteryEstimator::SimpleBatteryEstimator( SystemTraits& system_traits) : _pimpl(rmf_utils::make_impl( - Implementation{std::move(system_traits)})) + Implementation{system_traits})) { // Do nothing } diff --git a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp index 4b74f4cab..f0736de0f 100644 --- a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp +++ b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp @@ -67,8 +67,8 @@ SCENARIO("Test CleaningTaskPlanner") // 2 const std::string map_name = "L1"; graph.add_waypoint(map_name, {0, 0}).set_holding_point(true); // 0 - graph.add_waypoint(map_name, {10000, 0}).set_holding_point(true); // 1 - graph.add_waypoint(map_name, {0, -10000}).set_holding_point(true); // 2 + graph.add_waypoint(map_name, {8000, 0}).set_holding_point(true); // 1 + graph.add_waypoint(map_name, {0, -8000}).set_holding_point(true); // 2 graph.add_waypoint(map_name, {-20000, 0}).set_holding_point(true); // 3 graph.add_lane(0, 1); @@ -107,7 +107,7 @@ SCENARIO("Test CleaningTaskPlanner") rmf_traffic::agv::Interpolate::positions( traits, start_time + 15000s, - {{15000, 0, 0}, {15000, 100, 0}, {15000, 0, 0}} + {{8000, 0, 0}, {8000, 100, 0}, {8000, 0, 0}} ); const auto itinerary = cleaning_planner.plan( diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_SystemTraits.cpp index 6694617f7..ca293c5c0 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_SystemTraits.cpp @@ -84,4 +84,10 @@ SCENARIO("Test SystemTraits") REQUIRE(system_traits.valid()); // TODO(YV): Tests for getters and setters + WHEN("Getting power systems") + { + const auto& power_systems = system_traits.power_systems(); + CHECK(power_systems.size() > 0); + } + } From 7d6f9de458e9204c933444912a19641e0d978758 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 24 Aug 2020 17:32:42 +0800 Subject: [PATCH 023/128] More tests Signed-off-by: Yadunund Vijay --- .../unit/agv/test_CleaningTaskPlanner.cpp | 52 +++++++++++++++++-- 1 file changed, 47 insertions(+), 5 deletions(-) diff --git a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp index f0736de0f..240837dc6 100644 --- a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp +++ b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp @@ -51,7 +51,7 @@ SCENARIO("Test CleaningTaskPlanner") rmf_traffic::geometry::Circle>(1.0); const rmf_traffic::Profile profile{shape, shape}; - const rmf_traffic::agv::VehicleTraits traits( + const VehicleTraits traits( {1.0, 0.7}, {0.6, 0.5}, profile); rmf_traffic::schedule::Database database; @@ -61,14 +61,14 @@ SCENARIO("Test CleaningTaskPlanner") // Setup graph Graph graph; - // 3--------0------1 + // 3--------0----1 // | // | // 2 const std::string map_name = "L1"; graph.add_waypoint(map_name, {0, 0}).set_holding_point(true); // 0 graph.add_waypoint(map_name, {8000, 0}).set_holding_point(true); // 1 - graph.add_waypoint(map_name, {0, -8000}).set_holding_point(true); // 2 + graph.add_waypoint(map_name, {0, -10000}).set_holding_point(true); // 2 graph.add_waypoint(map_name, {-20000, 0}).set_holding_point(true); // 3 graph.add_lane(0, 1); @@ -100,11 +100,11 @@ SCENARIO("Test CleaningTaskPlanner") const auto start_time = std::chrono::steady_clock::now(); - WHEN("Cleaning zone 1") + WHEN("Robot has sufficient charge to clean a zone and return to charger") { Planner::Start start{start_time, 0, 0.0}; // Start at 0 rmf_traffic::Trajectory cleaning_trajectory = - rmf_traffic::agv::Interpolate::positions( + Interpolate::positions( traits, start_time + 15000s, {{8000, 0, 0}, {8000, 100, 0}, {8000, 0, 0}} @@ -120,8 +120,50 @@ SCENARIO("Test CleaningTaskPlanner") ); CHECK(itinerary.size() > 0); + } + + WHEN("Robot can reach a zone but has insufficient charge to finish cleaning") + { + Planner::Start start{start_time, 0, 0.0}; // Start at 0 + rmf_traffic::Trajectory cleaning_trajectory = + Interpolate::positions( + traits, + start_time + 15000s, + {{20000, 0, 0}, {20000, 100, 0}, {20000, 0, 0}} + ); + + const auto itinerary = cleaning_planner.plan( + start, + 1.0, + 3, + cleaning_trajectory, + 3, + 0 + ); + CHECK(itinerary.size() == 0); + } + + WHEN("Robot can clean a zone but has insufficient charge to return to charger") + { + Planner::Start start{start_time, 0, 0.0}; // Start at 0 + rmf_traffic::Trajectory cleaning_trajectory = + Interpolate::positions( + traits, + start_time + 15000s, + {{0, -10000, 0}, {-100, -10000, 0}, {0, -10000, 0}} + ); + + const auto itinerary = cleaning_planner.plan( + start, + 1.0, + 2, + cleaning_trajectory, + 2, + 0 + ); + CHECK(itinerary.size() == 0); } } \ No newline at end of file From 771ef0dbe7374f942ba1b9d437e3f4b7a221f60f Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 25 Aug 2020 19:35:14 +0800 Subject: [PATCH 024/128] Restructured CMakeLists and fixed warnings --- rmf_battery/CMakeLists.txt | 33 +++++++++---------- .../rmf_battery/agv/CleaningTaskPlanner.hpp | 2 +- .../rmf_battery/agv/CleaningTaskPlanner.cpp | 2 +- .../agv/SimpleBatteryEstimator.cpp | 8 ----- 4 files changed, 18 insertions(+), 27 deletions(-) diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 4f2d12663..77cdaa651 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -27,6 +27,22 @@ add_library(rmf_battery SHARED ${core_lib_srcs} ) +target_link_libraries(rmf_battery + PUBLIC + rmf_utils::rmf_utils + rmf_traffic::rmf_traffic +) + +target_include_directories(rmf_battery + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) + +ament_export_interfaces(rmf_battery HAS_LIBRARY_TARGET) +ament_export_dependencies(rmf_utils) + if(BUILD_TESTING) find_package(ament_cmake_catch2 REQUIRED) @@ -37,7 +53,6 @@ if(BUILD_TESTING) TIMEOUT 300) target_link_libraries(test_rmf_battery rmf_battery - rmf_traffic::rmf_traffic ) target_include_directories(test_rmf_battery @@ -55,22 +70,6 @@ if(BUILD_TESTING) ) endif() -target_link_libraries(rmf_battery - PUBLIC - rmf_utils::rmf_utils - rmf_traffic::rmf_traffic -) - -target_include_directories(rmf_battery - PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIRS} -) - -ament_export_interfaces(rmf_battery HAS_LIBRARY_TARGET) -ament_export_dependencies(rmf_utils) - install( DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} diff --git a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp index 207c0aa34..f92551174 100644 --- a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp +++ b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp @@ -66,7 +66,7 @@ class CleaningTaskPlanner const std::string& cleaning_system() const; /// Get a const reference to the battery threshold - const double battery_threshold() const; + double battery_threshold() const; /// Produce a set of valid trajectories if the robot can successfully travel /// from start location to the cleaning waypoint, complete the cleaning diff --git a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp index 9d84ed977..c1bf60647 100644 --- a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp +++ b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp @@ -70,7 +70,7 @@ const std::string& CleaningTaskPlanner::cleaning_system() const } //============================================================================== -const double CleaningTaskPlanner::battery_threshold() const +double CleaningTaskPlanner::battery_threshold() const { return _pimpl->battery_threshold; } diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp index 9277c65a7..6cbdcd9e9 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp @@ -56,14 +56,6 @@ const SystemTraits SimpleBatteryEstimator::system_traits() const } namespace { -double compute_kinetic_energy( - const double m, - const double v, - const double i, - const double w) -{ - return 0.5 * (m*pow(v, 2) + i*pow(w, 2)); -} double compute_friction_energy( const double f, From de34ba657a3e535350f48ba895887994fc69631f Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 27 Aug 2020 16:53:29 +0800 Subject: [PATCH 025/128] Refactored --- .../include/rmf_battery/DevicePowerSink.hpp | 45 ++ .../include/rmf_battery/EstimateBattery.hpp | 65 --- .../include/rmf_battery/MotionPowerSink.hpp | 47 ++ .../include/rmf_battery/agv/BatterySystem.hpp | 138 ++++++ .../rmf_battery/agv/CleaningTaskPlanner.hpp | 113 ----- .../rmf_battery/agv/MechanicalSystem.hpp | 59 +++ .../include/rmf_battery/agv/PowerSystem.hpp | 69 +++ .../agv/SimpleBatteryEstimator.hpp | 70 --- .../rmf_battery/agv/SimpleDevicePowerSink.hpp | 72 +++ .../rmf_battery/agv/SimpleMotionPowerSink.hpp | 72 +++ .../include/rmf_battery/agv/SystemTraits.hpp | 191 -------- .../src/rmf_battery/agv/BatterySystem.cpp | 218 +++++++++ .../rmf_battery/agv/CleaningTaskPlanner.cpp | 198 -------- .../src/rmf_battery/agv/MechanicalSystem.cpp | 96 ++++ .../src/rmf_battery/agv/PowerSystem.cpp | 91 ++++ .../rmf_battery/agv/SimpleDevicePowerSink.cpp | 76 +++ ...stimator.cpp => SimpleMotionPowerSink.cpp} | 82 ++-- .../src/rmf_battery/agv/SystemTraits.cpp | 458 ------------------ .../test/unit/agv/test_BatterySystem.cpp | 27 ++ .../unit/agv/test_CleaningTaskPlanner.cpp | 169 ------- .../test/unit/agv/test_MechanicalSystem.cpp | 65 +++ ..._SystemTraits.cpp => test_PowerSystem.cpp} | 48 +- ...ryEstimator.cpp => test_battery_drain.cpp} | 141 +++--- 23 files changed, 1191 insertions(+), 1419 deletions(-) create mode 100644 rmf_battery/include/rmf_battery/DevicePowerSink.hpp delete mode 100644 rmf_battery/include/rmf_battery/EstimateBattery.hpp create mode 100644 rmf_battery/include/rmf_battery/MotionPowerSink.hpp create mode 100644 rmf_battery/include/rmf_battery/agv/BatterySystem.hpp delete mode 100644 rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp create mode 100644 rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp create mode 100644 rmf_battery/include/rmf_battery/agv/PowerSystem.hpp delete mode 100644 rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp create mode 100644 rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp create mode 100644 rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp delete mode 100644 rmf_battery/include/rmf_battery/agv/SystemTraits.hpp create mode 100644 rmf_battery/src/rmf_battery/agv/BatterySystem.cpp delete mode 100644 rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp create mode 100644 rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp create mode 100644 rmf_battery/src/rmf_battery/agv/PowerSystem.cpp create mode 100644 rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp rename rmf_battery/src/rmf_battery/agv/{SimpleBatteryEstimator.cpp => SimpleMotionPowerSink.cpp} (56%) delete mode 100644 rmf_battery/src/rmf_battery/agv/SystemTraits.cpp create mode 100644 rmf_battery/test/unit/agv/test_BatterySystem.cpp delete mode 100644 rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp create mode 100644 rmf_battery/test/unit/agv/test_MechanicalSystem.cpp rename rmf_battery/test/unit/agv/{test_SystemTraits.cpp => test_PowerSystem.cpp} (52%) rename rmf_battery/test/unit/agv/{test_SimpleBatteryEstimator.cpp => test_battery_drain.cpp} (53%) diff --git a/rmf_battery/include/rmf_battery/DevicePowerSink.hpp b/rmf_battery/include/rmf_battery/DevicePowerSink.hpp new file mode 100644 index 000000000..7e1eb3554 --- /dev/null +++ b/rmf_battery/include/rmf_battery/DevicePowerSink.hpp @@ -0,0 +1,45 @@ +/* + * 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_BATTERY__DEVICEPOWERSINK_HPP +#define RMF_BATTERY__DEVICEPOWERSINK_HPP + +namespace rmf_battery { + + +//============================================================================== +class DevicePowerSink +{ +public: + + /// Compute change in state-of-charge of the battery due to an onboard + /// device over a time period. + /// + /// \param[in] run_time + /// The duration in seconds over which the power system drains charge from + /// the battery + /// + /// \return The charge depleted as a fraction of the total battery capacity + virtual double compute_change_in_charge( + const double run_time) const = 0; + + virtual ~DevicePowerSink() = default; +}; + +} // namespace rmf_battery + +#endif // RMF_BATTERY__DEVICEPOWERSINK_HPP diff --git a/rmf_battery/include/rmf_battery/EstimateBattery.hpp b/rmf_battery/include/rmf_battery/EstimateBattery.hpp deleted file mode 100644 index 0537a4a81..000000000 --- a/rmf_battery/include/rmf_battery/EstimateBattery.hpp +++ /dev/null @@ -1,65 +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_BATTERY__ESTIMATEBATTERY_HPP -#define RMF_BATTERY__ESTIMATEBATTERY_HPP - -#include - -#include - -#include - -#include -#include - -namespace rmf_battery { - - -//============================================================================== -class EstimateBattery -{ -public: - - using PowerMap = std::unordered_map; - - /// Computes state-of-charge estimate of battery at the end of a trajectory. - /// - /// \param[in] trajectory - /// A valid rmf_traffic:::Trajectory - /// - /// \param[in] initial_soc - /// The initial state of charge of the robot at the beginning of the - /// trajectory as a fraction of total battery capacity - /// - /// \param[in] power_map - /// An optional unordered map with keys representing names of power systems - /// and values of trajectories during which the power system is active. - /// - /// \return The remaining charge at the end of the trajectory as a fraction of - /// total battery capacity - virtual double compute_state_of_charge( - const rmf_traffic::Trajectory& trajectory, - const double initial_soc, - rmf_utils::optional power_map = rmf_utils::nullopt) const = 0; - - virtual ~EstimateBattery() = default; -}; - -} // namespace rmf_traffic - -#endif // RMF_BATTERY__ESTIMATEBATTERY_HPP diff --git a/rmf_battery/include/rmf_battery/MotionPowerSink.hpp b/rmf_battery/include/rmf_battery/MotionPowerSink.hpp new file mode 100644 index 000000000..a492411f0 --- /dev/null +++ b/rmf_battery/include/rmf_battery/MotionPowerSink.hpp @@ -0,0 +1,47 @@ +/* + * 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_BATTERY__MOTIONPOWERSINK_HPP +#define RMF_BATTERY__MOTIONPOWERSINK_HPP + +#include + +namespace rmf_battery { + + +//============================================================================== +class MotionPowerSink +{ +public: + + /// Compute change in state-of-charge of the battery due to locomotion + /// of the robot along a trajectory. + /// + /// \param[in] trajectory + /// A valid rmf_traffic:::Trajectory over which the change in charge has to + /// to be computed + /// + /// \return The charge depleted as a fraction of the total battery capacity + virtual double compute_change_in_charge( + const rmf_traffic::Trajectory& trajectory) const = 0; + + virtual ~MotionPowerSink() = default; +}; + +} // namespace rmf_battery + +#endif // RMF_BATTERY__MOTIONPOWERSINK_HPP diff --git a/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp b/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp new file mode 100644 index 000000000..6c300456f --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp @@ -0,0 +1,138 @@ + +/* + * 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_BATTERY__AGV__BATTERYSYSTEM_HPP +#define RMF_BATTERY__AGV__BATTERYSYSTEM_HPP + +#include +#include + +namespace rmf_battery { +namespace agv { + +class BatterySystem +{ +public: + enum class BatteryType : uint16_t + { + /// The vehicle is powered by a Lead-Acid battery + LeadAcid, + /// The vehicle is powered by a Lithium-Ion battery + LiIon, + /// The vehicle is powered by a Nickel-Metal-Hydride battery + NiMH + }; + + class BatteryProfile + { + public: + /// The parameters required to construct a generic battery model to map + /// state of charge of the battery to its voltage. + /// + /// \param[in] resistance + /// The internal resistance of the battery in ohms + /// + /// \param[in] max_voltage + /// The maximum voltage in Volts of the battery + /// + /// \param[in] exp_voltage + /// The voltage of the battery in Volts at the end of the exponential + /// zone in its discharge profile + /// + /// \param[in] exp_capacity + /// The capacity of the battery in Ah at the end of the exponential zone + /// in its discharge profile + BatteryProfile( + double resistance, + double max_voltage, + double exp_voltage, + double exp_capacity); + + BatteryProfile& resistance(double resistance); + double resistance() const; + + BatteryProfile& max_voltage(double max_voltage); + double max_voltage() const; + + BatteryProfile& exp_voltage(double exp_voltage); + double exp_voltage() const; + + BatteryProfile& exp_capacity(double exp_capacity); + double exp_capacity() const; + + /// Returns true if the values are valid, i.e. greater than zero. + bool valid() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + /// Constructor + /// + /// \param[in] nominal_voltage + /// The nominal voltage of the battery in volts + /// + /// \param[in] nominal_capacity + /// The nominal capacity of the battery in Ah + /// + /// \param[in] charging_current + /// The rated current in A for charging the battery + /// + /// \param[in] type + /// The chemisty type of the battery + /// + /// \param[in] profile + /// The battery profile for this battery + BatterySystem( + double nominal_voltage, + double nominal_capacity, + double charging_current, + BatteryType type = BatteryType::LeadAcid, + rmf_utils::optional profile = rmf_utils::nullopt); + + BatterySystem& nominal_voltage(double nominal_voltage); + double nominal_voltage() const; + + BatterySystem& nominal_capacity(double nominal_capacity); + double nominal_capacity() const; + + BatterySystem& charging_current(double charging_current); + double charging_current() const; + + BatterySystem& type(BatteryType type); + BatteryType type() const; + + BatterySystem& profile(rmf_utils::optional profile); + rmf_utils::optional profile() const; + + /// Returns true if the values are valid, i.e. greater than zero. + bool valid() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +using BatterySystemPtr = std::shared_ptr; +using ConstBatterySystemPtr = std::shared_ptr; + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__AGV__BATTERYSYSTEM_HPP diff --git a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp b/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp deleted file mode 100644 index f92551174..000000000 --- a/rmf_battery/include/rmf_battery/agv/CleaningTaskPlanner.hpp +++ /dev/null @@ -1,113 +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_BATTERY__AGV__CLEANINGTASKPLANNER_HPP -#define RMF_BATTERY__AGV__CLEANINGTASKPLANNER_HPP - -#include -#include - -#include - -#include - -#include - -namespace rmf_battery { -namespace agv { - -class CleaningTaskPlanner -{ -public: - using Planner = rmf_traffic::agv::Planner; - /// Constructor - /// - /// \param[in] system_traits - /// An instance of rmf_battery::agv::SystemTraits. - /// - /// \param[in] planner - /// An instance of rmf_traffic::agv::Planner. A valid reference is to be - /// maintained by the user. - /// - /// \param[in] cleaning_system - /// The name of the cleaning system. The name must match one of the power - /// system names as configured in SystemTraits. - /// - /// \param[in] battery_threshold - /// The minimum allowable state of charge of the battery as a fraction the - /// total charge capacity - CleaningTaskPlanner( - SystemTraits& system_traits, - Planner& planner, - const std::string& cleaning_system, - const double battery_threshold = 0.0); - - /// Get a const reference to the rmf_battery::agv::SystemTraits instance - const SystemTraits& system_traits() const; - - /// Get a const reference to the rmf_traffic::agv::Planner instance - const Planner& planner() const; - - /// Get a const reference to the name of the cleaning system - const std::string& cleaning_system() const; - - /// Get a const reference to the battery threshold - double battery_threshold() const; - - /// Produce a set of valid trajectories if the robot can successfully travel - /// from start location to the cleaning waypoint, complete the cleaning - /// process and has sufficent battery to return to its nearest charging - /// waypoint if required. If no feasible plan is found, an empty set is - /// returned - /// - /// \param[in] start - /// The starting conditions - /// - /// \param[in] initial_soc - /// The state of charge of the robot at its start location - /// - /// \param[in] cleaning_start_waypoint - /// The waypoint where the robot needs to reach to begin cleaning - /// - /// \param[in] cleaning_trajectory - /// The trajectory to be followed by the robot while cleaning - /// - /// \param[in] cleaning_end_waypoint - /// The waypoint where the robot stops after cleaning - /// - /// \param[in] charging_station_waypoint - /// The nearest charging station waypoint for the robot - /// - std::vector plan( - const Planner::Start& start, - const double initial_soc, - const std::size_t cleaning_start_waypoint, - const rmf_traffic::Trajectory& cleaning_trajectory, - const std::size_t cleaning_end_waypoint, - const std::size_t charging_station_waypoint); - - class Implementation; - -private: - rmf_utils::impl_ptr _pimpl; -}; - - -} // namespace agv -} // namespace rmf_battery - -#endif // RMF_BATTERY__AGV__CLEANINGTASKPLANNER_HPP diff --git a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp new file mode 100644 index 000000000..e209e2293 --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp @@ -0,0 +1,59 @@ +/* + * 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_BATTERY__AGV__MECHANICALSYSTEM_HPP +#define RMF_BATTERY__AGV__MECHANICALSYSTEM_HPP + +#include + +namespace rmf_battery { +namespace agv { + + +class MechanicalSystem +{ +public: + + MechanicalSystem( + double mass, + double inertia, + double friction_coefficient); + + MechanicalSystem& mass(double mass); + double mass() const; + + MechanicalSystem& inertia(double inertia); + double inertia() const; + + MechanicalSystem& friction_coefficient(double friction_coeff); + double friction_coefficient() const; + + /// Returns true if the values are valid, i.e. greater than zero. + bool valid() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +using MechanicalSystemPtr = std::shared_ptr; +using ConstMechanicalSystemPtr = std::shared_ptr; + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__AGV__MECHANICALSYSTEM_HPP diff --git a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp new file mode 100644 index 000000000..7e53c33e9 --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp @@ -0,0 +1,69 @@ +/* + * 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_BATTERY__AGV__POWERSYSTEM_HPP +#define RMF_BATTERY__AGV__POWERSYSTEM_HPP + +#include + +namespace rmf_battery { +namespace agv { + + +class PowerSystem +{ +public: + + /// Constructor + /// + /// \param[in] name + /// A string representing the name of this power system + /// + /// \param[in] nominal_power + /// The rated nominal power consumption in Watts for this power system + /// + /// \param[in] efficiency + /// The efficiency of this power system + PowerSystem( + std::string name, + double nominal_power, + double efficiency = 1.0); + + PowerSystem& name(std::string name); + const std::string& name() const; + + PowerSystem& nominal_power(double nom_power); + double nominal_power() const; + + PowerSystem& efficiency(double efficiency); + double efficiency() const; + + /// Returns true if the values are valid, i.e. greater than zero. + bool valid() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +using PowerSystemPtr = std::shared_ptr; +using ConstPowerSystemPtr = std::shared_ptr; + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__AGV__POWERSYSTEM_HPP diff --git a/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp b/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp deleted file mode 100644 index 9d86a5ff2..000000000 --- a/rmf_battery/include/rmf_battery/agv/SimpleBatteryEstimator.hpp +++ /dev/null @@ -1,70 +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_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP -#define RMF_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP - -#include - -#include - - -namespace rmf_battery { -namespace agv { - -class SimpleBatteryEstimator : public EstimateBattery -{ -public: - - /// Constructor - SimpleBatteryEstimator(SystemTraits& system_traits); - - SimpleBatteryEstimator& system_traits(SystemTraits system_traits); - - const SystemTraits system_traits() const; - - /// Computes state-of-charge estimate of battery at the end of a trajectory. - /// - /// \param[in] trajectory - /// A valid rmf_traffic:::Trajectory - /// - /// \param[in] initial_soc - /// The initial state of charge of the robot at the beginning of the - /// trajectory as a fraction of total battery capacity - /// - /// \param[in] power_map - /// An optional unordered map with keys representing names of power systems - /// and values of trajectories during which the power system is active. - /// - /// \return The remaining charge at the end of the trajectory as a fraction of - /// total battery capacity - double compute_state_of_charge( - const rmf_traffic::Trajectory& trajectory, - const double initial_soc, - rmf_utils::optional power_map = rmf_utils::nullopt - ) const final; - - class Implementation; - -private: - rmf_utils::impl_ptr _pimpl; -}; - -} // namespace agv -} // namespace rmf_battery - -#endif // RMF_BATTERY__AGV__SIMPLEBATTERYESTIMATOR_HPP \ No newline at end of file diff --git a/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp b/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp new file mode 100644 index 000000000..7dacc600f --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp @@ -0,0 +1,72 @@ +/* + * 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_BATTERY__SIMPLEDEVICEPOWERSINK_HPP +#define RMF_BATTERY__SIMPLEDEVICEPOWERSINK_HPP + +#include +#include +#include + +#include + +namespace rmf_battery { +namespace agv { + +//============================================================================== +class SimpleDevicePowerSink : public DevicePowerSink +{ +public: + + /// Constructor + /// + /// \param[in] battery_system + /// The BatterySystem of the robot + /// + /// \param[in] power_system + /// The PowerSystem for this device + SimpleDevicePowerSink( + BatterySystem& battery_system, + PowerSystem& power_system); + + /// Get a constant reference to the battery system + const BatterySystem& battery_system() const; + + /// Get a constant reference to the power system + const PowerSystem& power_system() const; + + /// Compute change in state-of-charge of the battery due to an onboard + /// device over a time period. + /// + /// \param[in] run_time + /// The duration in seconds over which the power system drains charge from + /// the battery + /// + /// \return The charge depleted as a fraction of the total battery capacity + virtual double compute_change_in_charge( + const double run_time) const final; + + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__SIMPLEDEVICEPOWERSINK_HPP diff --git a/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp b/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp new file mode 100644 index 000000000..56ac2246b --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp @@ -0,0 +1,72 @@ +/* + * 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_BATTERY__SIMPLEMOTIONPOWERSINK_HPP +#define RMF_BATTERY__SIMPLEMOTIONPOWERSINK_HPP + +#include +#include +#include + +#include + +namespace rmf_battery { +namespace agv { + +//============================================================================== +class SimpleMotionPowerSink : public MotionPowerSink +{ +public: + + /// Constructor + /// + /// \param[in] battery_system + /// The BatterySystem of the robot + /// + /// \param[in] mechanical_system + /// The MechanicalSystem of the robot + SimpleMotionPowerSink( + BatterySystem& battery_system, + MechanicalSystem& mechanical_system); + + /// Get a constant reference to the battery system + const BatterySystem& battery_system() const; + + /// Get a constant reference to the mechanical system + const MechanicalSystem& mechanical_system() const; + + /// Compute change in state-of-charge estimate of battery due to locomotion + /// of the robot along a trajectory. + /// + /// \param[in] trajectory + /// A valid rmf_traffic:::Trajectory over which the change in charge has to + /// to be computed + /// + /// \return The charge depleted as a fraction of the total battery capacity + virtual double compute_change_in_charge( + const rmf_traffic::Trajectory& trajectory) const final; + + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_battery + +#endif // RMF_BATTERY__SIMPLEMOTIONPOWERSINK_HPP diff --git a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp b/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp deleted file mode 100644 index 9df273708..000000000 --- a/rmf_battery/include/rmf_battery/agv/SystemTraits.hpp +++ /dev/null @@ -1,191 +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_BATTERY__AGV__SYSTEMTRAITS_HPP -#define RMF_BATTERY__AGV__SYSTEMTRAITS_HPP - -#include -#include - -#include -#include - -namespace rmf_battery { -namespace agv { - -//============================================================================== -class SystemTraits -{ -public: - class PowerSystem; - using PowerSystems = std::unordered_map; - - class PowerSystem - { - public: - - PowerSystem( - std::string name, - double nominal_power, - double nominal_voltage, - double efficiency = 1.0); - - PowerSystem& name(std::string name); - const std::string& name() const; - - PowerSystem& nominal_power(double nom_power); - double nominal_power() const; - - PowerSystem& nominal_voltage(double nom_voltage); - double nominal_voltage() const; - - PowerSystem& efficiency(double efficiency); - double efficiency() const; - - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - class BatterySystem - { - public: - enum class BatteryType : uint16_t - { - /// The vehicle is powered by a Lead-Acid battery - LeadAcid, - /// The vehicle is powered by a Lithium-Ion battery - LiIon, - }; - - class BatteryProfile - { - public: - BatteryProfile( - double resistance, - double max_voltage, - double exp_voltage, - double exp_capacity); - - BatteryProfile& resistance(double resistance); - double resistance() const; - - BatteryProfile& max_voltage(double max_voltage); - double max_voltage() const; - - BatteryProfile& exp_voltage(double exp_voltage); - double exp_voltage() const; - - BatteryProfile& exp_capacity(double exp_capacity); - double exp_capacity() const; - - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - BatterySystem( - double nominal_voltage, - double nominal_capacity, - double charging_current, - BatteryType type = BatteryType::LeadAcid, - rmf_utils::optional profile = rmf_utils::nullopt); - - BatterySystem& nominal_voltage(double nominal_voltage); - double nominal_voltage() const; - - BatterySystem& nominal_capacity(double nominal_capacity); - double nominal_capacity() const; - - BatterySystem& charging_current(double charging_current); - double charging_current() const; - - BatterySystem& type(BatteryType type); - BatteryType type() const; - - BatterySystem& profile(rmf_utils::optional profile); - rmf_utils::optional profile() const; - - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - class MechanicalSystem - { - public: - - MechanicalSystem( - double mass, - double inertia, - double friction_coefficient); - - MechanicalSystem& mass(double mass); - double mass() const; - - MechanicalSystem& inertia(double inertia); - double inertia() const; - - MechanicalSystem& friction_coefficient(double friction_coeff); - double friction_coefficient() const; - - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - /// Constructor. - SystemTraits( - MechanicalSystem mechanical_system, - BatterySystem battery_system, - PowerSystems power_systems); - - SystemTraits& mechanical_system(MechanicalSystem mechanical_system); - const MechanicalSystem& mechanical_system() const; - - SystemTraits& battery_system(BatterySystem battery_system); - const BatterySystem& battery_system() const; - - SystemTraits& power_systems(PowerSystems power_systems); - const PowerSystems& power_systems() const; - - /// Returns true if the values of the traits are valid. For example, this - /// means that all system values are greater than zero. - bool valid() const; - - class Implementation; -private: - rmf_utils::impl_ptr _pimpl; - -}; - -} // namespace agv -} // namespace rmf_battery - -#endif // RMF_BATTERY__AGV__SYSTEMTRAITS_HPP diff --git a/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp new file mode 100644 index 000000000..6a92efd3f --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp @@ -0,0 +1,218 @@ +/* + * 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 + +namespace rmf_battery { +namespace agv { + +using BatteryProfile = BatterySystem::BatteryProfile; +using BatteryType = BatterySystem::BatteryType; + +class BatterySystem::BatteryProfile::Implementation +{ +public: + double resistance; + double max_voltage; + double exp_voltage; + double exp_capacity; +}; + +//============================================================================== +BatterySystem::BatteryProfile::BatteryProfile( + const double resistance, + const double max_voltage, + const double exp_voltage, + const double exp_capacity) +: _pimpl(rmf_utils::make_impl( + Implementation{resistance, max_voltage, exp_voltage, exp_capacity})) +{ + // Do nothing +} + +//============================================================================== +auto BatteryProfile::resistance(double resistance) -> BatteryProfile& +{ + _pimpl->resistance = resistance; + return *this; +} + +//============================================================================== +double BatteryProfile::resistance() const +{ + return _pimpl->resistance; +} + +//============================================================================== +auto BatteryProfile::max_voltage(double max_voltage) -> BatteryProfile& +{ + _pimpl->max_voltage = max_voltage; + return *this; +} + +//============================================================================== +double BatteryProfile::max_voltage() const +{ + return _pimpl->max_voltage; +} + +//============================================================================== +auto BatteryProfile::exp_voltage(double exp_voltage) -> BatteryProfile& +{ + _pimpl->exp_voltage = exp_voltage; + return *this; +} + +//============================================================================== +double BatteryProfile::exp_voltage() const +{ + return _pimpl->exp_voltage; +} + +//============================================================================== +auto BatteryProfile::exp_capacity(double exp_capacity) -> BatteryProfile& +{ + _pimpl->exp_capacity = exp_capacity; + return *this; +} + +//============================================================================== +double BatteryProfile::exp_capacity() const +{ + return _pimpl->exp_capacity; +} + +//============================================================================== +bool BatteryProfile::valid() const +{ + return _pimpl->resistance > 0.0 && _pimpl->max_voltage > 0.0 && + _pimpl->exp_voltage > 0.0 && _pimpl->exp_capacity > 0.0; +} + +//============================================================================== +class BatterySystem::Implementation +{ +public: + double nominal_voltage; + double nominal_capacity; + double charging_current; + BatteryType type; + rmf_utils::optional profile; +}; + +//============================================================================== +BatterySystem::BatterySystem( + const double nominal_voltage, + const double nominal_capacity, + const double charging_current, + BatteryType type, + rmf_utils::optional profile) +: _pimpl(rmf_utils::make_impl( + Implementation{ + nominal_voltage, + nominal_capacity, + charging_current, + type, + std::move(profile) + })) +{ + // Do nothing +} + +//============================================================================== +auto BatterySystem::nominal_voltage(double nom_voltage) -> BatterySystem& +{ + _pimpl->nominal_voltage = nom_voltage; + return *this; +} + +//============================================================================== +double BatterySystem::nominal_voltage() const +{ + return _pimpl->nominal_voltage; +} + +//============================================================================== +auto BatterySystem::nominal_capacity(double nom_capacity) -> BatterySystem& +{ + _pimpl->nominal_capacity = nom_capacity; + return *this; +} + +//============================================================================== +double BatterySystem::nominal_capacity() const +{ + return _pimpl->nominal_capacity; +} + +//============================================================================== +auto BatterySystem::charging_current(double charging_current) -> BatterySystem& +{ + _pimpl->charging_current = charging_current; + return *this; +} + +//============================================================================== +double BatterySystem::charging_current() const +{ + return _pimpl->charging_current; +} + +//============================================================================== +auto BatterySystem::type(BatteryType type) +-> BatterySystem& +{ + _pimpl->type = type; + return *this; +} + +//============================================================================== +BatteryType BatterySystem::type() const +{ + return _pimpl->type; +} + +//============================================================================== +auto BatterySystem::profile( + rmf_utils::optional profile) -> BatterySystem& +{ + _pimpl->profile = std::move(profile); + return *this; +} + +//============================================================================== +rmf_utils::optional BatterySystem::profile() const +{ + return _pimpl->profile; +} + +//============================================================================== +bool BatterySystem::valid() const +{ + bool valid = _pimpl->nominal_voltage > 0.0 && + _pimpl->nominal_capacity > 0.0 && _pimpl->charging_current > 0.0 && + (_pimpl->type == BatteryType::LeadAcid + || _pimpl->type == BatteryType::LiIon); + if (_pimpl->profile) + return valid && _pimpl->profile->valid(); + + return valid; + +} + +} // namespace agv +} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp b/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp deleted file mode 100644 index c1bf60647..000000000 --- a/rmf_battery/src/rmf_battery/agv/CleaningTaskPlanner.cpp +++ /dev/null @@ -1,198 +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 - -#include - -#include - -namespace rmf_battery { -namespace agv { - -class CleaningTaskPlanner::Implementation -{ -public: - SystemTraits system_traits; - Planner planner; - std::string cleaning_system; - double battery_threshold; -}; - -//============================================================================== -CleaningTaskPlanner::CleaningTaskPlanner( - SystemTraits& system_traits, - Planner& planner, - const std::string& cleaning_system, - const double battery_threshold) -: _pimpl(rmf_utils::make_impl( - Implementation{ - system_traits, - std::move(planner), - std::move(cleaning_system), - battery_threshold - })) -{ - assert(battery_threshold >= 0.0 && battery_threshold <= 1.0); -} - -//============================================================================== -const SystemTraits& CleaningTaskPlanner::system_traits() const -{ - return _pimpl->system_traits; -} - -//============================================================================== -const rmf_traffic::agv::Planner& CleaningTaskPlanner::planner() const -{ - return _pimpl->planner; -} - -//============================================================================== -const std::string& CleaningTaskPlanner::cleaning_system() const -{ - return _pimpl->cleaning_system; -} - -//============================================================================== -double CleaningTaskPlanner::battery_threshold() const -{ - return _pimpl->battery_threshold; -} -//============================================================================== -namespace { -double get_battery_soc_after_itinerary( - const SimpleBatteryEstimator& battery_estimator, - const std::vector& itinerary, - const SystemTraits& system_traits, - const std::string& cleaning_system, - const double initial_soc) -{ - double battery_soc = initial_soc; - for (const auto& route : itinerary) - { - // TODO(YV): allow users to state which power systems are active during non- - // cleaning routes - rmf_utils::optional power_map = - SimpleBatteryEstimator::PowerMap(); - for (const auto power_system : system_traits.power_systems()) - { - if (power_system.first == cleaning_system) - continue; - - power_map.value().insert({power_system.first, route.trajectory()}); - } - battery_soc = battery_estimator.compute_state_of_charge( - route.trajectory(), battery_soc, power_map); - } - - return battery_soc; -} - -} // anonymous - -//============================================================================== -std::vector CleaningTaskPlanner::plan( - const Planner::Start& start, - const double initial_soc, - const std::size_t cleaning_start_waypoint, - const rmf_traffic::Trajectory& cleaning_trajectory, - const std::size_t cleaning_end_waypoint, - const std::size_t charging_station_waypoint) -{ - // Ensure that the waypoint indices are valid - const auto& graph = _pimpl->planner.get_configuration().graph(); - std::size_t num_waypoints = graph.num_waypoints(); - assert(start.waypoint() < num_waypoints); - assert(cleaning_start_waypoint < num_waypoints); - assert(cleaning_end_waypoint < num_waypoints); - assert(charging_station_waypoint < num_waypoints); - assert(_pimpl->system_traits.power_systems().find(_pimpl->cleaning_system) - != _pimpl->system_traits.power_systems().end()); - - std::vector cleaning_plan; - double battery_soc = initial_soc; - SimpleBatteryEstimator battery_estimator(_pimpl->system_traits); - - Planner::Goal goal_1{cleaning_start_waypoint}; - const auto plan_to_cleaning_start = _pimpl->planner.plan(start, goal_1); - // Stage 1: check if robot has charge to reach cleaning start waypoint - if (!plan_to_cleaning_start.success()) - { - std::cout << "Failed to find plan from start to cleaning start waypoint" - << std::endl; - return cleaning_plan; - } - auto itinerary_1 = plan_to_cleaning_start->get_itinerary(); - // Compute battery drain for each route - battery_soc = get_battery_soc_after_itinerary(battery_estimator, - itinerary_1, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); - - std::cout << "Battery soc after traversing to cleaning start: " - << battery_soc << std::endl; - if (battery_soc < _pimpl->battery_threshold) - return cleaning_plan; - - // Stage 2: check if robot has charge to finish cleaning - rmf_utils::optional power_map = - SimpleBatteryEstimator::PowerMap(); - for (const auto& system : _pimpl->system_traits.power_systems()) - power_map.value().insert({system.first, cleaning_trajectory}); - battery_soc = battery_estimator.compute_state_of_charge( - cleaning_trajectory, battery_soc, power_map); - - std::cout << "Battery soc after cleaning: " << battery_soc - << std::endl; - - if (battery_soc < _pimpl->battery_threshold) - return cleaning_plan; - - // Stage 3: check if robot has charge to theoretically travel to its charger - auto end_it = cleaning_trajectory.end(); --end_it; - Planner::Start start_2{end_it->time(), - cleaning_end_waypoint, end_it->position()[2]}; - Planner::Goal goal_2{charging_station_waypoint}; - const auto plan_to_charger = _pimpl->planner.plan(start_2, goal_2); - - if (!plan_to_charger.success()) - { - std::cout << "Failed to find plan from cleaning end to charger" - << std::endl; - return cleaning_plan; - } - - auto itinerary_2 = plan_to_charger->get_itinerary(); - battery_soc = get_battery_soc_after_itinerary(battery_estimator, - itinerary_2, _pimpl->system_traits, _pimpl->cleaning_system, battery_soc); - - std::cout << "Battery after returning to charger: " << battery_soc - <battery_threshold) - return cleaning_plan; - - // A successful plan can be produced - for (const auto& route : itinerary_1) - cleaning_plan.push_back(route.trajectory()); - cleaning_plan.push_back(cleaning_trajectory); - - return cleaning_plan; -} - -} // namespace agv -} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp new file mode 100644 index 000000000..872a718ac --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp @@ -0,0 +1,96 @@ +/* + * 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 + +namespace rmf_battery { +namespace agv { + +//============================================================================== +class MechanicalSystem::Implementation +{ +public: + double mass; + double inertia; + double friction_coefficient; +}; + +//============================================================================== +MechanicalSystem::MechanicalSystem( + const double mass, + const double inertia, + const double friction_coefficient) +: _pimpl(rmf_utils::make_impl( + Implementation{ + mass, + inertia, + friction_coefficient, + })) +{ + // Do nothing +} + +//============================================================================== +auto MechanicalSystem::mass(double mass) -> MechanicalSystem& +{ + _pimpl->mass = mass; + return *this; +} + +//============================================================================== +double MechanicalSystem::mass() const +{ + return _pimpl->mass; +} + +//============================================================================== +auto MechanicalSystem::friction_coefficient(double friction_coeff) +-> MechanicalSystem& +{ + _pimpl->friction_coefficient = friction_coeff; + return *this; +} + +//============================================================================== +double MechanicalSystem::friction_coefficient() const +{ + return _pimpl->friction_coefficient; +} + +//============================================================================== +auto MechanicalSystem::inertia(double inertia) +-> MechanicalSystem& +{ + _pimpl->inertia = inertia; + return *this; +} + +//============================================================================== +double MechanicalSystem::inertia() const +{ + return _pimpl->inertia; +} + +//============================================================================== +bool MechanicalSystem::valid() const +{ + return _pimpl->mass > 0.0 && _pimpl->friction_coefficient > 0.0 && + _pimpl->inertia > 0.0; +} + +} // namespace agv +} // namespace rmf_battery diff --git a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp new file mode 100644 index 000000000..01ababe2a --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -0,0 +1,91 @@ +/* + * 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 + +namespace rmf_battery { +namespace agv { + +class PowerSystem::Implementation +{ +public: + std::string name; + double nominal_power; + double efficiency; +}; + +//============================================================================== +PowerSystem::PowerSystem( + const std::string name, + const double nominal_power, + const double efficiency) +: _pimpl(rmf_utils::make_impl( + Implementation{name, nominal_power, efficiency})) +{ + // Do nothing +} + +//============================================================================== +auto PowerSystem::name(std::string name) -> PowerSystem& +{ + _pimpl->name = name; + return *this; +} + +//============================================================================== +const std::string& PowerSystem::name() const +{ + return _pimpl->name; +} + +//============================================================================== +auto PowerSystem::nominal_power(double nom_power) ->PowerSystem& +{ + _pimpl->nominal_power = nom_power; + return *this; +} + +//============================================================================== +double PowerSystem::nominal_power() const +{ + return _pimpl->nominal_power; +} + +//============================================================================== +auto PowerSystem::efficiency(double efficiency) +->PowerSystem& +{ + _pimpl->efficiency = efficiency; + return *this; +} + +//============================================================================== +double PowerSystem::efficiency() const +{ + return _pimpl->efficiency; +} + +//============================================================================== +bool PowerSystem::valid() const +{ + return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0 + && _pimpl->efficiency > 0.0; +} + + +} // namespace agv +} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp new file mode 100644 index 000000000..e469ca20a --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp @@ -0,0 +1,76 @@ +/* + * 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 + +#include + +namespace rmf_battery { +namespace agv { + +class SimpleDevicePowerSink::Implementation +{ +public: + BatterySystem battery_system; + PowerSystem power_system; +}; + +//============================================================================== +SimpleDevicePowerSink::SimpleDevicePowerSink( + BatterySystem& battery_system, + PowerSystem& power_system) +: _pimpl(rmf_utils::make_impl( + Implementation{battery_system, power_system})) +{ + // Do nothing +} + +//============================================================================== +const BatterySystem& SimpleDevicePowerSink::battery_system() const +{ + return _pimpl->battery_system; +} + +//============================================================================== +const PowerSystem& SimpleDevicePowerSink::power_system() const +{ + return _pimpl->power_system; +} + + +//============================================================================== +double SimpleDevicePowerSink::compute_change_in_charge( + const double run_time) const +{ + assert(_pimpl->battery_system.valid()); + assert(_pimpl->power_system.valid()); + + const double nominal_capacity = _pimpl->battery_system.nominal_capacity(); + const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); + const double nominal_power = _pimpl->power_system.nominal_power(); + + const double dE = nominal_power * run_time; + const double dQ = dE / nominal_voltage; + const double dSOC = dQ / (nominal_capacity * 3600.0); + + return dSOC; +} + +} // namespace agv +} // namespace rmf_battery diff --git a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp similarity index 56% rename from rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp rename to rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 6cbdcd9e9..d49401d3c 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleBatteryEstimator.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -15,8 +15,7 @@ * */ -#include -#include +#include #include #include @@ -29,32 +28,36 @@ namespace rmf_battery { namespace agv { -class SimpleBatteryEstimator::Implementation +class SimpleMotionPowerSink::Implementation { public: - SystemTraits system_traits; + BatterySystem battery_system; + MechanicalSystem mechanical_system; }; -SimpleBatteryEstimator::SimpleBatteryEstimator( - SystemTraits& system_traits) +//============================================================================== +SimpleMotionPowerSink::SimpleMotionPowerSink( + BatterySystem& battery_system, + MechanicalSystem& mechanical_system) : _pimpl(rmf_utils::make_impl( - Implementation{system_traits})) + Implementation{battery_system, mechanical_system})) { // Do nothing } -auto SimpleBatteryEstimator::system_traits(const SystemTraits system_traits) --> SimpleBatteryEstimator& +//============================================================================== +const BatterySystem& SimpleMotionPowerSink::battery_system() const { - _pimpl->system_traits = std::move(system_traits); - return *this; + return _pimpl->battery_system; } -const SystemTraits SimpleBatteryEstimator::system_traits() const +//============================================================================== +const MechanicalSystem& SimpleMotionPowerSink::mechanical_system() const { - return _pimpl->system_traits; + return _pimpl->mechanical_system; } +//============================================================================== namespace { double compute_friction_energy( @@ -68,23 +71,19 @@ double compute_friction_energy( } } // namespace anonymous -double SimpleBatteryEstimator::compute_state_of_charge( - const rmf_traffic::Trajectory& trajectory, - const double initial_soc, - rmf_utils::optional power_map) const + +//============================================================================== +double SimpleMotionPowerSink::compute_change_in_charge( + const rmf_traffic::Trajectory& trajectory) const { - assert(_pimpl->system_traits.valid()); - - double battery_soc = initial_soc; - double nominal_capacity = - _pimpl->system_traits.battery_system().nominal_capacity(); - double nominal_voltage = - _pimpl->system_traits.battery_system().nominal_voltage(); - const double mass = _pimpl->system_traits.mechanical_system().mass(); - const double inertia = _pimpl->system_traits.mechanical_system().inertia(); - const double friction = - _pimpl->system_traits.mechanical_system().friction_coefficient(); - const auto& power_systems = _pimpl->system_traits.power_systems(); + assert(_pimpl->battery_system.valid()); + assert(_pimpl->mechanical_system.valid()); + + const double nominal_capacity = _pimpl->battery_system.nominal_capacity(); + const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); + const double mass = _pimpl->mechanical_system.mass(); + const double inertia = _pimpl->mechanical_system.inertia(); + const double friction = _pimpl->mechanical_system.friction_coefficient(); auto begin_it = trajectory.begin(); auto end_it = --trajectory.end(); @@ -96,6 +95,7 @@ double SimpleBatteryEstimator::compute_state_of_charge( const double sim_step = 0.1; // seconds + // Change in energy double dE = 0.0; // TODO explore analytical solutions as opposed to numerical integration @@ -117,28 +117,14 @@ double SimpleBatteryEstimator::compute_state_of_charge( // Loss through friction const double EF = compute_friction_energy(friction, mass, v, sim_step); - double EP = 0.0; - // Loss through power systems - if (power_map.has_value()) - { - for (const auto& item : power_map.value()) - { - if (item.second.find(sim_time) != item.second.end()) - { - const auto it = power_systems.find(item.first); - assert(it != power_systems.end()); - EP += it->second.nominal_power() * sim_step; - } - } - } - dE += EA + EF + EP; + dE += EA + EF; } - // Computing the charge consumed + // The charge consumed const double dQ = dE / nominal_voltage; - battery_soc -= dQ / (nominal_capacity * 3600); - - return battery_soc; + // The depleted state of charge + const double dSOC = dQ / (nominal_capacity * 3600.0); + return dSOC; } } // namespace agv diff --git a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp b/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp deleted file mode 100644 index 81245f606..000000000 --- a/rmf_battery/src/rmf_battery/agv/SystemTraits.cpp +++ /dev/null @@ -1,458 +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 - -namespace rmf_battery { -namespace agv { - -using BatteryProfile = SystemTraits::BatterySystem::BatteryProfile; -using BatteryType = SystemTraits::BatterySystem::BatteryType; - -class SystemTraits::PowerSystem::Implementation -{ -public: - std::string name; - double nominal_power; - double nominal_voltage; - double efficiency; -}; - -//============================================================================== -SystemTraits::PowerSystem::PowerSystem( - const std::string name, - const double nominal_power, - const double nominal_voltage, - const double efficiency) -: _pimpl(rmf_utils::make_impl( - Implementation{name, nominal_power, nominal_voltage, - efficiency})) -{ - // Do nothing -} - -//============================================================================== -auto SystemTraits::PowerSystem::name(std::string name) -> PowerSystem& -{ - _pimpl->name = name; - return *this; -} - -//============================================================================== -const std::string& SystemTraits::PowerSystem::name() const -{ - return _pimpl->name; -} - -//============================================================================== -auto SystemTraits::PowerSystem::nominal_power(double nom_power) ->PowerSystem& -{ - _pimpl->nominal_power = nom_power; - return *this; -} - -//============================================================================== -double SystemTraits::PowerSystem::nominal_power() const -{ - return _pimpl->nominal_power; -} - -//============================================================================== -auto SystemTraits::PowerSystem::nominal_voltage(double nom_voltage) -->PowerSystem& -{ - _pimpl->nominal_voltage = nom_voltage; - return *this; -} - -//============================================================================== -double SystemTraits::PowerSystem::nominal_voltage() const -{ - return _pimpl->nominal_voltage; -} -//============================================================================== -auto SystemTraits::PowerSystem::efficiency(double efficiency) -->PowerSystem& -{ - _pimpl->efficiency = efficiency; - return *this; -} - -//============================================================================== -double SystemTraits::PowerSystem::efficiency() const -{ - return _pimpl->efficiency; -} - -//============================================================================== -bool SystemTraits::PowerSystem::valid() const -{ - return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0 - && _pimpl->nominal_voltage > 0.0 && _pimpl->efficiency > 0.0; -} - -//============================================================================== -class SystemTraits::BatterySystem::BatteryProfile::Implementation -{ -public: - double resistance; - double max_voltage; - double exp_voltage; - double exp_capacity; -}; - -//============================================================================== -SystemTraits::BatterySystem::BatteryProfile::BatteryProfile( - const double resistance, - const double max_voltage, - const double exp_voltage, - const double exp_capacity) -: _pimpl(rmf_utils::make_impl( - Implementation{resistance, max_voltage, exp_voltage, exp_capacity})) -{ - // Do nothing -} - -//============================================================================== -auto BatteryProfile::resistance(double resistance) --> BatteryProfile& -{ - _pimpl->resistance = resistance; - return *this; -} - -//============================================================================== -double BatteryProfile::resistance() const -{ - return _pimpl->resistance; -} - -//============================================================================== -auto BatteryProfile::max_voltage(double max_voltage) --> BatteryProfile& -{ - _pimpl->max_voltage = max_voltage; - return *this; -} - -//============================================================================== -double BatteryProfile::max_voltage() const -{ - return _pimpl->max_voltage; -} - -//============================================================================== -auto BatteryProfile::exp_voltage(double exp_voltage) --> BatteryProfile& -{ - _pimpl->exp_voltage = exp_voltage; - return *this; -} - -//============================================================================== -double BatteryProfile::exp_voltage() const -{ - return _pimpl->exp_voltage; -} - -//============================================================================== -auto BatteryProfile::exp_capacity(double exp_capacity) --> BatteryProfile& -{ - _pimpl->exp_capacity = exp_capacity; - return *this; -} - -//============================================================================== -double BatteryProfile::exp_capacity() const -{ - return _pimpl->exp_capacity; -} - -//============================================================================== -bool BatteryProfile::valid() const -{ - return _pimpl->resistance > 0.0 && _pimpl->max_voltage > 0.0 && - _pimpl->exp_voltage > 0.0 && _pimpl->exp_capacity > 0.0; -} - -//============================================================================== -class SystemTraits::BatterySystem::Implementation -{ -public: - double nominal_voltage; - double nominal_capacity; - double charging_current; - BatteryType type; - rmf_utils::optional profile; -}; - -//============================================================================== -SystemTraits::BatterySystem::BatterySystem( - const double nominal_voltage, - const double nominal_capacity, - const double charging_current, - BatteryType type, - rmf_utils::optional profile) -: _pimpl(rmf_utils::make_impl( - Implementation{ - nominal_voltage, - nominal_capacity, - charging_current, - type, - std::move(profile) - })) -{ - // Do nothing -} - -//============================================================================== -auto SystemTraits::BatterySystem::nominal_voltage(double nom_voltage) --> BatterySystem& -{ - _pimpl->nominal_voltage = nom_voltage; - return *this; -} - -//============================================================================== -double SystemTraits::BatterySystem::nominal_voltage() const -{ - return _pimpl->nominal_voltage; -} - -//============================================================================== -auto SystemTraits::BatterySystem::nominal_capacity(double nom_capacity) --> BatterySystem& -{ - _pimpl->nominal_capacity = nom_capacity; - return *this; -} - -//============================================================================== -double SystemTraits::BatterySystem::nominal_capacity() const -{ - return _pimpl->nominal_capacity; -} - -//============================================================================== -auto SystemTraits::BatterySystem::charging_current(double charging_current) --> BatterySystem& -{ - _pimpl->charging_current = charging_current; - return *this; -} - -//============================================================================== -double SystemTraits::BatterySystem::charging_current() const -{ - return _pimpl->charging_current; -} - -//============================================================================== -auto SystemTraits::BatterySystem::type(BatteryType type) --> BatterySystem& -{ - _pimpl->type = type; - return *this; -} - -//============================================================================== -BatteryType SystemTraits::BatterySystem::type() const -{ - return _pimpl->type; -} - -//============================================================================== -auto SystemTraits::BatterySystem::profile( - rmf_utils::optional profile) --> BatterySystem& -{ - _pimpl->profile = std::move(profile); - return *this; -} - -//============================================================================== -rmf_utils::optional SystemTraits::BatterySystem::profile() const -{ - return _pimpl->profile; -} - -//============================================================================== -bool SystemTraits::BatterySystem::valid() const -{ - bool valid = _pimpl->nominal_voltage > 0.0 && - _pimpl->nominal_capacity > 0.0 && _pimpl->charging_current > 0.0 && - (_pimpl->type == BatteryType::LeadAcid - || _pimpl->type == BatteryType::LiIon); - if (_pimpl->profile) - return valid && _pimpl->profile->valid(); - - return valid; - -} - -//============================================================================== -class SystemTraits::MechanicalSystem::Implementation -{ -public: - double mass; - double inertia; - double friction_coefficient; -}; - -//============================================================================== -SystemTraits::MechanicalSystem::MechanicalSystem( - const double mass, - const double inertia, - const double friction_coefficient) -: _pimpl(rmf_utils::make_impl( - Implementation{ - mass, - inertia, - friction_coefficient, - })) -{ - // Do nothing -} - -//============================================================================== -auto SystemTraits::MechanicalSystem::mass(double mass) -> MechanicalSystem& -{ - _pimpl->mass = mass; - return *this; -} - -//============================================================================== -double SystemTraits::MechanicalSystem::mass() const -{ - return _pimpl->mass; -} - -//============================================================================== -auto SystemTraits::MechanicalSystem::friction_coefficient(double friction_coeff) --> MechanicalSystem& -{ - _pimpl->friction_coefficient = friction_coeff; - return *this; -} - -//============================================================================== -double SystemTraits::MechanicalSystem::friction_coefficient() const -{ - return _pimpl->friction_coefficient; -} - -//============================================================================== -auto SystemTraits::MechanicalSystem::inertia(double inertia) --> MechanicalSystem& -{ - _pimpl->inertia = inertia; - return *this; -} - -//============================================================================== -double SystemTraits::MechanicalSystem::inertia() const -{ - return _pimpl->inertia; -} - -//============================================================================== -bool SystemTraits::MechanicalSystem::valid() const -{ - return _pimpl->mass > 0.0 && _pimpl->friction_coefficient > 0.0 && - _pimpl->inertia > 0.0; -} - -//============================================================================== -class SystemTraits::Implementation -{ -public: - MechanicalSystem mechanical_system; - BatterySystem battery_system; - PowerSystems power_systems; -}; - -//============================================================================== -SystemTraits::SystemTraits( - const MechanicalSystem mechanical_system, - const BatterySystem battery_system, - const PowerSystems power_systems) -: _pimpl(rmf_utils::make_impl( - Implementation - { - std::move(mechanical_system), - std::move(battery_system), - std::move(power_systems) - })) -{ - // Do nothing -} - -//============================================================================== -auto SystemTraits::mechanical_system(MechanicalSystem mechanical_system) --> SystemTraits& -{ - _pimpl->mechanical_system = std::move(mechanical_system); - return *this; -} - -//============================================================================== -const SystemTraits::MechanicalSystem& SystemTraits::mechanical_system() const -{ - return _pimpl->mechanical_system; -} - -//============================================================================== -auto SystemTraits::battery_system(BatterySystem battery_system) --> SystemTraits& -{ - _pimpl->battery_system = std::move(battery_system); - return *this; -} - -//============================================================================== -const SystemTraits::BatterySystem& SystemTraits::battery_system() const -{ - return _pimpl->battery_system; -} - -//============================================================================== -auto SystemTraits::power_systems(PowerSystems power_systems) --> SystemTraits& -{ - _pimpl->power_systems = std::move(power_systems); - return *this; -} - -//============================================================================== -const SystemTraits::PowerSystems& SystemTraits::power_systems() const -{ - return _pimpl->power_systems; -} - -//============================================================================== -bool SystemTraits::valid() const -{ - bool valid = true; - for (const auto& power_system : _pimpl->power_systems) - valid = valid && power_system.second.valid(); - return _pimpl->battery_system.valid() && _pimpl->mechanical_system.valid() && - valid; -} - -} // namespace agv -} // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_BatterySystem.cpp b/rmf_battery/test/unit/agv/test_BatterySystem.cpp new file mode 100644 index 000000000..edaa6c2f9 --- /dev/null +++ b/rmf_battery/test/unit/agv/test_BatterySystem.cpp @@ -0,0 +1,27 @@ +/* + * 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 + + + +SCENARIO("Test BatterySystem") +{ + // TODO +} diff --git a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp b/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp deleted file mode 100644 index 240837dc6..000000000 --- a/rmf_battery/test/unit/agv/test_CleaningTaskPlanner.cpp +++ /dev/null @@ -1,169 +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 - -#include -#include -#include - - -#include -#include - -rmf_utils::clone_ptr -make_test_schedule_validator( - const rmf_traffic::schedule::Viewer& viewer, - rmf_traffic::Profile profile) -{ - return rmf_utils::make_clone( - viewer, - std::numeric_limits::max(), - std::move(profile)); -} - -SCENARIO("Test CleaningTaskPlanner") -{ - using namespace std::chrono_literals; - using Graph = rmf_traffic::agv::Graph; - using VehicleTraits = rmf_traffic::agv::VehicleTraits; - using Interpolate = rmf_traffic::agv::Interpolate; - using Planner = rmf_traffic::agv::Planner; - using CleaningTaskPlanner = rmf_battery::agv::CleaningTaskPlanner; - using SystemTraits = rmf_battery::agv::SystemTraits; - - const auto shape = rmf_traffic::geometry::make_final_convex< - rmf_traffic::geometry::Circle>(1.0); - const rmf_traffic::Profile profile{shape, shape}; - - const VehicleTraits traits( - {1.0, 0.7}, {0.6, 0.5}, profile); - - rmf_traffic::schedule::Database database; - - const auto default_options = rmf_traffic::agv::Planner::Options{ - make_test_schedule_validator(database, profile)}; - - // Setup graph - Graph graph; - // 3--------0----1 - // | - // | - // 2 - const std::string map_name = "L1"; - graph.add_waypoint(map_name, {0, 0}).set_holding_point(true); // 0 - graph.add_waypoint(map_name, {8000, 0}).set_holding_point(true); // 1 - graph.add_waypoint(map_name, {0, -10000}).set_holding_point(true); // 2 - graph.add_waypoint(map_name, {-20000, 0}).set_holding_point(true); // 3 - - graph.add_lane(0, 1); - graph.add_lane(1, 0); - graph.add_lane(0, 2); - graph.add_lane(2, 0); - graph.add_lane(0, 3); - graph.add_lane(3, 0); - - - rmf_traffic::agv::Planner planner{ - rmf_traffic::agv::Planner::Configuration{graph, traits}, - default_options - }; - - SystemTraits::BatterySystem battery_system{24, 40, 2}; - SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.22}; - SystemTraits::PowerSystem power_system_1{"processor", 20, 5}; - SystemTraits::PowerSystem power_system_2{"cleaning", 30, 5}; - SystemTraits::PowerSystems power_systems; - power_systems.insert({power_system_1.name(), power_system_1}); - power_systems.insert({power_system_2.name(), power_system_2}); - SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; - REQUIRE(system_traits.valid()); - - CleaningTaskPlanner cleaning_planner{ - system_traits, planner, power_system_2.name()}; - - const auto start_time = std::chrono::steady_clock::now(); - - WHEN("Robot has sufficient charge to clean a zone and return to charger") - { - Planner::Start start{start_time, 0, 0.0}; // Start at 0 - rmf_traffic::Trajectory cleaning_trajectory = - Interpolate::positions( - traits, - start_time + 15000s, - {{8000, 0, 0}, {8000, 100, 0}, {8000, 0, 0}} - ); - - const auto itinerary = cleaning_planner.plan( - start, - 1.0, - 1, - cleaning_trajectory, - 1, - 0 - ); - - CHECK(itinerary.size() > 0); - } - - WHEN("Robot can reach a zone but has insufficient charge to finish cleaning") - { - Planner::Start start{start_time, 0, 0.0}; // Start at 0 - rmf_traffic::Trajectory cleaning_trajectory = - Interpolate::positions( - traits, - start_time + 15000s, - {{20000, 0, 0}, {20000, 100, 0}, {20000, 0, 0}} - ); - - const auto itinerary = cleaning_planner.plan( - start, - 1.0, - 3, - cleaning_trajectory, - 3, - 0 - ); - - CHECK(itinerary.size() == 0); - } - - WHEN("Robot can clean a zone but has insufficient charge to return to charger") - { - Planner::Start start{start_time, 0, 0.0}; // Start at 0 - rmf_traffic::Trajectory cleaning_trajectory = - Interpolate::positions( - traits, - start_time + 15000s, - {{0, -10000, 0}, {-100, -10000, 0}, {0, -10000, 0}} - ); - - const auto itinerary = cleaning_planner.plan( - start, - 1.0, - 2, - cleaning_trajectory, - 2, - 0 - ); - - CHECK(itinerary.size() == 0); - } - -} \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp b/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp new file mode 100644 index 000000000..12719aef5 --- /dev/null +++ b/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp @@ -0,0 +1,65 @@ +/* + * 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 + + + +SCENARIO("Test MechanicalSystem") +{ + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + MechanicalSystem mechanical_system{70.0, 30.0, 0.3}; + REQUIRE(mechanical_system.valid()); + CHECK(mechanical_system.mass() - 70.0 == Approx(0.0)); + CHECK(mechanical_system.inertia() - 30.0 == Approx(0.0)); + CHECK(mechanical_system.friction_coefficient() - 0.3 == Approx(0.0)); + + WHEN("Mass is set") + { + mechanical_system.mass(75.0); + CHECK(mechanical_system.valid()); + CHECK(mechanical_system.mass() - 75.0 == Approx(0.0)); + CHECK(mechanical_system.inertia() - 30.0 == Approx(0.0)); + CHECK(mechanical_system.friction_coefficient() - 0.3 == Approx(0.0)); + } + + WHEN("Inertia is set") + { + mechanical_system.inertia(35.0); + CHECK(mechanical_system.valid()); + CHECK(mechanical_system.mass() - 70.0 == Approx(0.0)); + CHECK(mechanical_system.inertia() - 35.0 == Approx(0.0)); + CHECK(mechanical_system.friction_coefficient() - 0.3 == Approx(0.0)); + } + + WHEN("Friction coefficient is set") + { + mechanical_system.friction_coefficient(0.4); + CHECK(mechanical_system.valid()); + CHECK(mechanical_system.mass() - 70.0 == Approx(0.0)); + CHECK(mechanical_system.inertia() - 30.0 == Approx(0.0)); + CHECK(mechanical_system.friction_coefficient() - 0.4 == Approx(0.0)); + } + + WHEN("A negative value is set") + { + mechanical_system.mass(-10.0); + CHECK_FALSE(mechanical_system.valid()); + } +} diff --git a/rmf_battery/test/unit/agv/test_SystemTraits.cpp b/rmf_battery/test/unit/agv/test_PowerSystem.cpp similarity index 52% rename from rmf_battery/test/unit/agv/test_SystemTraits.cpp rename to rmf_battery/test/unit/agv/test_PowerSystem.cpp index ca293c5c0..8f46dd1d8 100644 --- a/rmf_battery/test/unit/agv/test_SystemTraits.cpp +++ b/rmf_battery/test/unit/agv/test_PowerSystem.cpp @@ -15,25 +15,24 @@ * */ -#include +#include #include SCENARIO("Test PowerSystem") { - rmf_battery::agv::SystemTraits::PowerSystem power_system( - "cleaning_system", 60, 12); + rmf_battery::agv::PowerSystem power_system( + "cleaning_system", 60); REQUIRE(power_system.name() == "cleaning_system"); REQUIRE(power_system.nominal_power() - 60 == Approx(0.0)); - REQUIRE(power_system.nominal_voltage() - 12 == Approx(0.0)); REQUIRE(power_system.efficiency() - 1.0 == Approx(0.0)); REQUIRE(power_system.valid()); + WHEN("Name is set") { power_system.name("vacuuming_system"); CHECK(power_system.name() == "vacuuming_system"); CHECK(power_system.nominal_power() - 60 == Approx(0.0)); - CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); CHECK(power_system.valid()); } @@ -41,53 +40,20 @@ SCENARIO("Test PowerSystem") { power_system.nominal_power(80); CHECK(power_system.nominal_power() - 80 == Approx(0.0)); - CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); - CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); - CHECK(power_system.valid()); - } - WHEN("Nominal voltage is set") - { - power_system.nominal_voltage(24); - CHECK(power_system.nominal_voltage() - 24 == Approx(0.0)); - CHECK(power_system.nominal_power() - 60 == Approx(0.0)); CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); CHECK(power_system.valid()); } + WHEN("Efficiency is set") { power_system.efficiency(0.80); - CHECK(power_system.nominal_voltage() - 12 == Approx(0.0)); CHECK(power_system.nominal_power() - 60 == Approx(0.0)); CHECK(power_system.efficiency() - 0.80 == Approx(0.0)); CHECK(power_system.valid()); } WHEN("A property is negative") { - power_system.nominal_voltage(-12); + power_system.nominal_power(-12); CHECK_FALSE(power_system.valid()); } -} - -SCENARIO("Test SystemTraits") -{ - using SystemTraits = rmf_battery::agv::SystemTraits; - SystemTraits::BatterySystem battery_system{12, 10, 2}; - REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{60, 10, 0.3}; - REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system{"cleaning_system", 100, 24}; - REQUIRE(power_system.valid()); - SystemTraits::PowerSystems power_systems; - power_systems.insert({power_system.name(), power_system}); - SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; - REQUIRE(system_traits.valid()); - - // TODO(YV): Tests for getters and setters - WHEN("Getting power systems") - { - const auto& power_systems = system_traits.power_systems(); - CHECK(power_systems.size() > 0); - } - -} +} \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp similarity index 53% rename from rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp rename to rmf_battery/test/unit/agv/test_battery_drain.cpp index 153c32c25..61b6a6bcd 100644 --- a/rmf_battery/test/unit/agv/test_SimpleBatteryEstimator.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -15,45 +15,48 @@ * */ -#include -#include +#include +#include +#include +#include +#include + #include #include #include -#include #include #include #include -SCENARIO("Test SimpleBatteryEstimator with RobotA") +SCENARIO("Test battery drain with RobotA") { - using SystemTraits = rmf_battery::agv::SystemTraits; - using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; - using PowerMap = rmf_battery::EstimateBattery::PowerMap; + 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; using namespace std::chrono_literals; // Initializing system traits - SystemTraits::BatterySystem battery_system{12, 24, 2}; + BatterySystem battery_system{12, 24, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{20, 10, 0.3}; + MechanicalSystem mechanical_system{20, 10, 0.3}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 10, 5}; + PowerSystem power_system_1{"processor", 10}; REQUIRE(power_system_1.valid()); - SystemTraits::PowerSystems power_systems; - power_systems.insert({power_system_1.name(), power_system_1}); - SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; - REQUIRE(system_traits.valid()); - - auto battery_estimator = SimpleBatteryEstimator{system_traits}; + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( {0.7, 0.5}, {0.3, 0.25}, {nullptr, nullptr}); + const double initial_soc = 1.0; + WHEN("Robot moves 100m along a straight line") { const auto start_time = std::chrono::steady_clock::now(); @@ -64,13 +67,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } @@ -85,13 +89,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1 && remaining_soc < 0.05; CHECK(ok); } @@ -99,30 +104,29 @@ SCENARIO("Test SimpleBatteryEstimator with RobotA") SCENARIO("Test SimpleBatteryEstimator with RobotB") { - using SystemTraits = rmf_battery::agv::SystemTraits; - using SimpleBatteryEstimator = rmf_battery::agv::SimpleBatteryEstimator; - using PowerMap = rmf_battery::EstimateBattery::PowerMap; + 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; using namespace std::chrono_literals; // Initializing system traits - SystemTraits::BatterySystem battery_system{24, 40, 2}; + BatterySystem battery_system{24, 40, 2}; REQUIRE(battery_system.valid()); - SystemTraits::MechanicalSystem mechanical_system{70, 40, 0.22}; + MechanicalSystem mechanical_system{70, 40, 0.22}; REQUIRE(mechanical_system.valid()); - SystemTraits::PowerSystem power_system_1{"processor", 20, 5}; + PowerSystem power_system_1{"processor", 20}; REQUIRE(power_system_1.valid()); - SystemTraits::PowerSystems power_systems; - power_systems.insert({power_system_1.name(), power_system_1}); - SystemTraits system_traits{ - mechanical_system, battery_system, power_systems}; - REQUIRE(system_traits.valid()); - - auto battery_estimator = SimpleBatteryEstimator{system_traits}; - + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( {1.0, 0.7}, {0.6, 0.5}, {nullptr, nullptr}); + const double initial_soc = 1.0; + WHEN("Robot moves 100m along a straight line") { const auto start_time = std::chrono::steady_clock::now(); @@ -133,13 +137,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; CHECK(ok); } @@ -154,13 +159,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; CHECK(ok); } @@ -175,13 +181,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; CHECK(ok); } @@ -199,13 +206,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::Trajectory trajectory = rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; } WHEN("Testing turning on a spot") @@ -222,13 +230,14 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") {0, 0, 0}); REQUIRE(trajectory.size() == 2); - rmf_utils::optional power_map = PowerMap(); - power_map.value().insert({"processor", trajectory}); + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); - auto remaining_soc = battery_estimator.compute_state_of_charge( - trajectory, 1.0, power_map); + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - std::cout << "Remaining soc: " << remaining_soc << std::endl; + // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } From 4616684116a9ba787a3c19b5a33c3936ee8fcd7d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 16 Sep 2020 17:46:05 +0800 Subject: [PATCH 026/128] Setting up rmf_tasks package Signed-off-by: Aaron Chong --- rmf_tasks/CMakeLists.txt | 126 ++++++++++++++++++ rmf_tasks/cmake/rmf_tasks-config.cmake.in | 14 ++ .../include/rmf_tasks/tasks/Delivery.hpp | 0 rmf_tasks/include/rmf_tasks/tasks/Loop.hpp | 0 rmf_tasks/package.xml | 17 +++ rmf_tasks/src/main.cpp | 0 6 files changed, 157 insertions(+) create mode 100644 rmf_tasks/CMakeLists.txt create mode 100644 rmf_tasks/cmake/rmf_tasks-config.cmake.in create mode 100644 rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp create mode 100644 rmf_tasks/include/rmf_tasks/tasks/Loop.hpp create mode 100644 rmf_tasks/package.xml create mode 100644 rmf_tasks/src/main.cpp diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt new file mode 100644 index 000000000..a7cce73d4 --- /dev/null +++ b/rmf_tasks/CMakeLists.txt @@ -0,0 +1,126 @@ +cmake_minimum_required(VERSION 3.5.0) + +project(rmf_tasks VERSION 1.0.0) + +set(CMAKE_EXPORT_COMPILE_COMMANDS on) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +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_utils REQUIRED) +find_package(rmf_traffic REQUIRED) + +# ===== Traffic control library +file(GLOB lib_srcs + "src/*.cpp" +) + +add_library(rmf_tasks SHARED + ${lib_srcs} +) + +target_link_libraries(rmf_tasks + PRIVATE + rmf_utils::rmf_utils + rmf_traffic::rmf_traffic +) + +target_include_directories(rmf_tasks + PUBLIC + $ + $ +) + +# find_package(ament_cmake_catch2 QUIET) +# find_package(rmf_cmake_uncrustify QUIET) +# if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) +# file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + +# ament_add_catch2( +# test_rmf_traffic test/main.cpp ${unit_test_srcs} +# TIMEOUT 300) +# target_link_libraries(test_rmf_traffic +# rmf_traffic +# ${PC_FCL_LIBRARIES} +# Threads::Threads +# ) + +# target_include_directories(test_rmf_traffic +# PRIVATE +# $ +# ) + +# find_file(uncrustify_config_file NAMES "share/format/rmf_code_style.cfg") + +# rmf_uncrustify( +# ARGN include src test +# CONFIG_FILE ${uncrustify_config_file} +# MAX_LINE_LENGTH 80 +# ) +# endif() + +# Create cmake config files +include(CMakePackageConfigHelpers) + +set(INSTALL_CONFIG_DIR "${CMAKE_INSTALL_LIBDIR}/rmf_tasks/cmake") +set(PACKAGE_CONFIG_VERSION_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_tasks-config-version.cmake") +set(PACKAGE_CONFIG_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_tasks-config.cmake") + +configure_package_config_file( + "${CMAKE_CURRENT_LIST_DIR}/cmake/rmf_tasks-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_tasks + EXPORT rmf_tasks-targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +install( + DIRECTORY include/rmf_tasks + DESTINATION include +) + +install( + FILES + ${PACKAGE_CONFIG_VERSION_FILE} + ${PACKAGE_CONFIG_FILE} + DESTINATION ${INSTALL_CONFIG_DIR} +) + +install( + EXPORT rmf_tasks-targets + FILE rmf_tasks-targets.cmake + NAMESPACE rmf_tasks:: + DESTINATION ${INSTALL_CONFIG_DIR} +) + +export( + EXPORT rmf_tasks-targets + FILE ${CMAKE_CURRENT_BINARY_DIR}/rmf_tasks-targets.cmake + NAMESPACE rmf_tasks:: +) + +export(PACKAGE rmf_tasks) diff --git a/rmf_tasks/cmake/rmf_tasks-config.cmake.in b/rmf_tasks/cmake/rmf_tasks-config.cmake.in new file mode 100644 index 000000000..e485d6de9 --- /dev/null +++ b/rmf_tasks/cmake/rmf_tasks-config.cmake.in @@ -0,0 +1,14 @@ +@PACKAGE_INIT@ + +get_filename_component(rmf_tasks_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) + +include(CMakeFindDependencyMacro) + +find_dependency(rmf_utils REQUIRED) +find_dependency(rmf_traffic REQUIRED) + +if(NOT TARGET rmf_tasks::rmf_tasks) + include("${rmf_tasks_CMAKE_DIR}/rmf_tasks-targets.cmake") +endif() + +check_required_components(rmf_tasks) diff --git a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp b/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp b/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_tasks/package.xml b/rmf_tasks/package.xml new file mode 100644 index 000000000..7e12f3abb --- /dev/null +++ b/rmf_tasks/package.xml @@ -0,0 +1,17 @@ + + + + rmf_tasks + 1.0.0 + Package for managing tasks in the Robotics Middleware Framework + Aaron + Apache License 2.0 + Aaron + + rmf_utils + rmf_traffic + + + cmake + + diff --git a/rmf_tasks/src/main.cpp b/rmf_tasks/src/main.cpp new file mode 100644 index 000000000..e69de29bb From 81c5c8ecc8b09f14694a2f5ad16c2dbe43112d41 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 17 Sep 2020 16:42:42 +0800 Subject: [PATCH 027/128] Basic setup --- rmf_tasks/include/rmf_tasks/Task.hpp | 0 rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp | 16 ++++++++++++++++ rmf_tasks/include/rmf_tasks/tasks/Loop.hpp | 16 ++++++++++++++++ rmf_tasks/src/main.cpp | 7 +++++++ rmf_tasks/src/tasks/Delivery.cpp | 0 rmf_tasks/src/tasks/Loop.cpp | 0 6 files changed, 39 insertions(+) create mode 100644 rmf_tasks/include/rmf_tasks/Task.hpp create mode 100644 rmf_tasks/src/tasks/Delivery.cpp create mode 100644 rmf_tasks/src/tasks/Loop.cpp diff --git a/rmf_tasks/include/rmf_tasks/Task.hpp b/rmf_tasks/include/rmf_tasks/Task.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp b/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp index e69de29bb..7f704809e 100644 --- a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp @@ -0,0 +1,16 @@ +/* + * 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. + * +*/ \ No newline at end of file diff --git a/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp b/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp index e69de29bb..7f704809e 100644 --- a/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp +++ b/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp @@ -0,0 +1,16 @@ +/* + * 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. + * +*/ \ No newline at end of file diff --git a/rmf_tasks/src/main.cpp b/rmf_tasks/src/main.cpp index e69de29bb..32b997012 100644 --- a/rmf_tasks/src/main.cpp +++ b/rmf_tasks/src/main.cpp @@ -0,0 +1,7 @@ +#include + +int main() +{ + std::cout << "all done" << std::endl; + return 0; +} diff --git a/rmf_tasks/src/tasks/Delivery.cpp b/rmf_tasks/src/tasks/Delivery.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_tasks/src/tasks/Loop.cpp b/rmf_tasks/src/tasks/Loop.cpp new file mode 100644 index 000000000..e69de29bb From 934d0bf7f7fcadbdbc73163de01d1228978657d4 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 17 Sep 2020 19:21:35 +0800 Subject: [PATCH 028/128] AGV state defined and implemented --- rmf_tasks/include/rmf_tasks/agv/State.hpp | 69 ++++++++++ .../include/rmf_tasks/tasks/Delivery.hpp | 31 ++++- .../rmf_tasks/Task.hpp => src/Task.cpp} | 0 rmf_tasks/src/Task.hpp | 101 ++++++++++++++ rmf_tasks/src/agv/State.cpp | 127 ++++++++++++++++++ 5 files changed, 327 insertions(+), 1 deletion(-) create mode 100644 rmf_tasks/include/rmf_tasks/agv/State.hpp rename rmf_tasks/{include/rmf_tasks/Task.hpp => src/Task.cpp} (100%) create mode 100644 rmf_tasks/src/Task.hpp create mode 100644 rmf_tasks/src/agv/State.cpp diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp new file mode 100644 index 000000000..15edb474b --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -0,0 +1,69 @@ +/* + * 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 INCLUDE__RMF_TASKS__AGV__STATE_HPP +#define INCLUDE__RMF_TASKS__AGV__STATE_HPP + +#include +#include + +#include + +namespace rmf_tasks { +namespace agv { + +class State +{ +public: + + /// Constructor + /// + /// \param[in] waypoint + /// \param[in] charging_waypoint + State( + std::size_t waypoint, + std::size_t charging_waypoint); + + std::size_t waypoint() const; + + State& waypoint(std::size_t new_waypoint); + + std::size_t charging_waypoint() const; + + State& charging_waypoint(std::size_t new_charging_waypoint); + + rmf_utils::optional finish_time() const; + + State& finish_time(rmf_traffic::Time new_finish_time); + + rmf_utils::optional battery_soc() const; + + State& battery_soc(double new_battery_soc); + + rmf_utils::optional threshold_soc() const; + + State& threshold_soc(double new_threshold_soc); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__AGV__STATE_HPP diff --git a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp b/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp index 7f704809e..5f94a46ec 100644 --- a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp @@ -13,4 +13,33 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ \ No newline at end of file +*/ + +#ifndef INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#define INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP + +#include +#include + +#include + +#include + +namespace rmf_tasks { +namespace tasks { + +class DeliveryEstimate +{ +public: + + +}; + + + + + +} // namespace tasks +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP diff --git a/rmf_tasks/include/rmf_tasks/Task.hpp b/rmf_tasks/src/Task.cpp similarity index 100% rename from rmf_tasks/include/rmf_tasks/Task.hpp rename to rmf_tasks/src/Task.cpp diff --git a/rmf_tasks/src/Task.hpp b/rmf_tasks/src/Task.hpp new file mode 100644 index 000000000..b32e28a05 --- /dev/null +++ b/rmf_tasks/src/Task.hpp @@ -0,0 +1,101 @@ +/* + * 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 SRC__TASK_HPP +#define SRC__TASK_HPP + +#include +#include +#include + +#include + +namespace rmf_tasks { + +class Task : public std::enable_shared_from_this +{ +public: + + using StatusMsg = rmf_task_msgs::msg::TaskSummary; + + /// This class represents the active phase of a Task. It provides an + /// observable that the Task can track to stay up-to-date on the status and to + /// know when to begin the next phase. + /// + /// The ActivePhase class must be a schedule Negotiator so that it can + /// negotiate its way out of conflicts with other schedule participants to + /// complete its work. + class ActivePhase + { + public: + + using StatusMsg = Task::StatusMsg; + + /// Get a reference to an observable for the status of this ActivePhase. + /// When this phase is complete, it will trigger on_completed() + virtual const rxcpp::observable& observe() const = 0; + + /// Estimate how much time remains in this phase. + virtual rmf_traffic::Duration estimate_remaining_time() const = 0; + + /// Activate or deactivate the emergency alarm behavior. + virtual void emergency_alarm(bool on) = 0; + + /// Tell this phase to cancel + virtual void cancel() = 0; + + /// Human-readable description of the phase + virtual const std::string& description() const = 0; + + // Virtual destructor + virtual ~ActivePhase() = default; + }; + + class PendingPhase + { + public: + + /// Begin this phase. + virtual std::shared_ptr begin() = 0; + + /// Estimate how much time this phase will require. + virtual rmf_traffic::Duration estimate_phase_duration() const = 0; + + /// Human-readable description of the phase + virtual const std::string& description() const = 0; + + // Virtual destructor + virtual ~PendingPhase() = default; + }; + + using StatusCallback = + std::function; + + using PendingPhases = std::vector>; + + static std::shared_ptr make( + std::string id, + PendingPhases phases); + +private: + + +}; + +} // namespace rmf_tasks + +#endif // SRC__TASK_HPP diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp new file mode 100644 index 000000000..2ac1d4512 --- /dev/null +++ b/rmf_tasks/src/agv/State.cpp @@ -0,0 +1,127 @@ +/* + * 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 + +namespace rmf_tasks { +namespace agv { + +//============================================================================== + +class State::Implementation +{ +public: + + Implementation(std::size_t waypoint, std::size_t charging_waypoint) + : _waypoint(waypoint), + _charging_waypoint(charging_waypoint) + {} + + std::size_t _waypoint; + std::size_t _charging_waypoint; + rmf_utils::optional _finish_time; + rmf_utils::optional _battery_soc; + rmf_utils::optional _threshold_soc; +}; + +//============================================================================== + +State::State(std::size_t waypoint, std::size_t charging_waypoint) +: _pimpl(rmf_utils::make_impl( + Implementation{waypoint, charging_waypoint})) +{ + // do nothing +} + +//============================================================================== + +std::size_t State::waypoint() const +{ + return _pimpl->_waypoint; +} + +//============================================================================== + +State& State::waypoint(std::size_t new_waypoint) +{ + _pimpl->_waypoint = new_waypoint; + 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; +} + +//============================================================================== + +rmf_utils::optional State::finish_time() const +{ + return _pimpl->_finish_time; +} + +//============================================================================== + +State& State::finish_time(rmf_traffic::Time new_finish_time) +{ + _pimpl->_finish_time = new_finish_time; + return *this; +} + +//============================================================================== + +rmf_utils::optional State::battery_soc() const +{ + return _pimpl->_battery_soc; +} +//============================================================================== + +State& State::battery_soc(double new_battery_soc) +{ + _pimpl->_battery_soc = new_battery_soc; + return *this; +} + +//============================================================================== + +rmf_utils::optional State::threshold_soc() const +{ + return _pimpl->_threshold_soc; +} + +//============================================================================== + +State& State::threshold_soc(double new_threshold_soc) +{ + _pimpl->_threshold_soc = new_threshold_soc; + return *this; +} + +//============================================================================== + +} // namespace agv +} // namespace rmf_tasks From d71b5089f7d14b09bc824da30a4d74bf01a3c11a Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 18 Sep 2020 14:30:51 +0800 Subject: [PATCH 029/128] Request abstract API and derived API for delivery Signed-off-by: Aaron Chong --- rmf_tasks/CMakeLists.txt | 1 + .../{tasks/Delivery.hpp => Estimate.hpp} | 28 ++++---- rmf_tasks/include/rmf_tasks/Request.hpp | 55 +++++++++++++++ rmf_tasks/include/rmf_tasks/Task.hpp | 0 .../include/rmf_tasks/requests/Delivery.hpp | 70 +++++++++++++++++++ rmf_tasks/src/Estimate.cpp | 69 ++++++++++++++++++ rmf_tasks/src/agv/State.cpp | 14 +--- rmf_tasks/src/requests/Delivery.cpp | 64 +++++++++++++++++ 8 files changed, 276 insertions(+), 25 deletions(-) rename rmf_tasks/include/rmf_tasks/{tasks/Delivery.hpp => Estimate.hpp} (55%) create mode 100644 rmf_tasks/include/rmf_tasks/Request.hpp create mode 100644 rmf_tasks/include/rmf_tasks/Task.hpp create mode 100644 rmf_tasks/include/rmf_tasks/requests/Delivery.hpp create mode 100644 rmf_tasks/src/Estimate.cpp create mode 100644 rmf_tasks/src/requests/Delivery.cpp diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt index a7cce73d4..cc1d53d58 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_tasks/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(rmf_traffic REQUIRED) # ===== Traffic control library file(GLOB lib_srcs + "src/agv/*.cpp" "src/*.cpp" ) diff --git a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp b/rmf_tasks/include/rmf_tasks/Estimate.hpp similarity index 55% rename from rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp rename to rmf_tasks/include/rmf_tasks/Estimate.hpp index 5f94a46ec..b57c8e79e 100644 --- a/rmf_tasks/include/rmf_tasks/tasks/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/Estimate.hpp @@ -15,31 +15,35 @@ * */ -#ifndef INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP -#define INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#ifndef INCLUDE__RMF_TASKS__ESTIMATE_HPP +#define INCLUDE__RMF_TASKS__ESTIMATE_HPP +#include #include -#include - -#include - -#include +#include namespace rmf_tasks { -namespace tasks { -class DeliveryEstimate +/// Estimates for requests +class Estimate { public: + Estimate(agv::State finish_state, rmf_traffic::Time wait_until); -}; + agv::State finish_state() const; + Estimate& finish_state(agv::State new_finish_state); + rmf_traffic::Time wait_until() const; + Estimate& wait_until(rmf_traffic::Time new_wait_until); + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; -} // namespace tasks } // namespace rmf_tasks -#endif // INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#endif // INCLUDE__RMF_TASKS__ESTIMATE_HPP diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp new file mode 100644 index 000000000..0773b69cd --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -0,0 +1,55 @@ +/* + * 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 INCLUDE__RMF_TASKS__TASK_HPP +#define INCLUDE__RMF_TASKS__TASK_HPP + +#include + +#include +#include + +#include +#include + +namespace rmf_tasks { + +/// Implement this for new tasks +class Request +{ +public: + + using SharedPtr = std::shared_ptr; + + // Get the id of the task + virtual std::size_t id() const = 0; + + // 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 rmf_utils::optional estimate_finish( + const agv::State& initial_state) const = 0; + + // Estimate the invariant component of the task's duration + virtual rmf_traffic::Duration invariant_duration() const = 0; + + // Get the earliest start time that this task may begin + virtual rmf_traffic::Time earliest_start_time() const = 0; +}; + +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__TASK_HPP diff --git a/rmf_tasks/include/rmf_tasks/Task.hpp b/rmf_tasks/include/rmf_tasks/Task.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp new file mode 100644 index 000000000..a5790416d --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -0,0 +1,70 @@ +/* + * 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 INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#define INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include + +namespace rmf_tasks { +namespace requests { + +class Delivery : public rmf_tasks::Request +{ +public: + + static rmf_tasks::Request::SharedPtr make( + std::size_t id, + std::size_t pickup_waypoint, + std::size_t dropoff_waypoint, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + bool drain_battery = true, + double start_time = 0.0); + + std::size_t id() const final; + + rmf_utils::optional estimate_finish( + const RobotState& initial_state) const final; + + rmf_traffic::Duration invariant_duration() const final; + + rmf_traffic::Time earliest_start_time() const final; + + class Implementation; +private: + Delivery(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace tasks +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP diff --git a/rmf_tasks/src/Estimate.cpp b/rmf_tasks/src/Estimate.cpp new file mode 100644 index 000000000..dee397fe1 --- /dev/null +++ b/rmf_tasks/src/Estimate.cpp @@ -0,0 +1,69 @@ +/* + * 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 + +namespace rmf_tasks { + +//============================================================================== +class Estimate::Implementation +{ +public: + + Implementation(agv::State finish_state, rmf_traffic::Time wait_until) + : _finish_state(std::move(finish_state)), + _wait_until(std::move(wait_until)) + {} + + agv::State _finish_state; + rmf_traffic::Time _wait_until; +}; + +//============================================================================== +Estimate::Estimate(agv::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 +{ + return _pimpl->_finish_state; +} + +//============================================================================== +Estimate& Estimate::finish_state(agv::State new_finish_state) +{ + _pimpl->_finish_state = std::move(new_finish_state); + return *this; +} + +//============================================================================== +rmf_traffic::Time Estimate::wait_until() const +{ + return _pimpl->_wait_until; +} + +//============================================================================== +Estimate& Estimate::wait_until(rmf_traffic::Time new_wait_until) +{ + _pimpl->_wait_until = std::move(new_wait_until); + return *this; +} + +//============================================================================== +} // namespace rmf_tasks diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index 2ac1d4512..d023eb95e 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -21,7 +21,6 @@ namespace rmf_tasks { namespace agv { //============================================================================== - class State::Implementation { public: @@ -39,7 +38,6 @@ class State::Implementation }; //============================================================================== - State::State(std::size_t waypoint, std::size_t charging_waypoint) : _pimpl(rmf_utils::make_impl( Implementation{waypoint, charging_waypoint})) @@ -48,14 +46,12 @@ State::State(std::size_t waypoint, std::size_t charging_waypoint) } //============================================================================== - std::size_t State::waypoint() const { return _pimpl->_waypoint; } //============================================================================== - State& State::waypoint(std::size_t new_waypoint) { _pimpl->_waypoint = new_waypoint; @@ -63,14 +59,12 @@ State& State::waypoint(std::size_t new_waypoint) } //============================================================================== - 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; @@ -78,14 +72,12 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== - rmf_utils::optional State::finish_time() const { return _pimpl->_finish_time; } //============================================================================== - State& State::finish_time(rmf_traffic::Time new_finish_time) { _pimpl->_finish_time = new_finish_time; @@ -93,13 +85,12 @@ State& State::finish_time(rmf_traffic::Time new_finish_time) } //============================================================================== - rmf_utils::optional State::battery_soc() const { return _pimpl->_battery_soc; } -//============================================================================== +//============================================================================== State& State::battery_soc(double new_battery_soc) { _pimpl->_battery_soc = new_battery_soc; @@ -107,14 +98,12 @@ State& State::battery_soc(double new_battery_soc) } //============================================================================== - rmf_utils::optional State::threshold_soc() const { return _pimpl->_threshold_soc; } //============================================================================== - State& State::threshold_soc(double new_threshold_soc) { _pimpl->_threshold_soc = new_threshold_soc; @@ -122,6 +111,5 @@ State& State::threshold_soc(double new_threshold_soc) } //============================================================================== - } // namespace agv } // namespace rmf_tasks diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp new file mode 100644 index 000000000..e69bb3b5e --- /dev/null +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -0,0 +1,64 @@ +/* + * 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 + +namespace rmf_tasks { +namespace requests { + +class Delivery::Implementation +{ +public: + + Implementation( + std::size_t id, + std::size_t pickup_waypoint, + std::size_t dropoff_waypoint, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + bool drain_battery = true, + rmf_traffic::Time start_time) + : _id(id), + _pickup_waypoint(pickup_waypoint) + {} + + std::size_t _id; + std::size_t _pickup_waypoint; + std::size_t _dropoff_waypoint; + std::shared_ptr _motion_sink; + std::shared_ptr _device_sink; + std::shared_ptr _planner; + bool _drain_battery; + double _invariant_battery_drain; + rmf_traffic::Duration _invariant_duration; + rmf_traffic::Time _start_time; +}; + +std::size_t Delivery::id() const +{} + +rmf_utils::optional estimate_finish( + const RobotState& initial_state) const final; + +rmf_traffic::Duration invariant_duration() const final; + +rmf_traffic::Time earliest_start_time() const final; + + +} // namespace requests +} // namespace rmf_tasks From 14469745db7c165d30a9af8b738a9063eb8291fa Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 18 Sep 2020 17:39:47 +0800 Subject: [PATCH 030/128] Delivery request estimation implemented --- rmf_tasks/CMakeLists.txt | 3 + rmf_tasks/include/rmf_tasks/agv/State.hpp | 16 +- .../include/rmf_tasks/requests/Delivery.hpp | 10 +- rmf_tasks/package.xml | 1 + rmf_tasks/src/agv/State.cpp | 38 ++-- rmf_tasks/src/requests/Delivery.cpp | 166 ++++++++++++++++-- 6 files changed, 194 insertions(+), 40 deletions(-) diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt index cc1d53d58..2611f3e17 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_tasks/CMakeLists.txt @@ -22,10 +22,12 @@ include(GNUInstallDirs) find_package(rmf_utils REQUIRED) find_package(rmf_traffic REQUIRED) +find_package(rmf_battery REQUIRED) # ===== Traffic control library file(GLOB lib_srcs "src/agv/*.cpp" + "src/requests/*.cpp" "src/*.cpp" ) @@ -37,6 +39,7 @@ target_link_libraries(rmf_tasks PRIVATE rmf_utils::rmf_utils rmf_traffic::rmf_traffic + rmf_battery::rmf_battery ) target_include_directories(rmf_tasks diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index 15edb474b..484cb80fb 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -18,6 +18,8 @@ #ifndef INCLUDE__RMF_TASKS__AGV__STATE_HPP #define INCLUDE__RMF_TASKS__AGV__STATE_HPP +#include + #include #include @@ -34,9 +36,15 @@ class State /// /// \param[in] waypoint /// \param[in] charging_waypoint + /// \param[in] finish_time + /// \param[in] battery_soc + /// \param[in] threshold_soc State( std::size_t waypoint, - std::size_t charging_waypoint); + std::size_t charging_waypoint, + rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), + double battery_soc = 1.0, + double threshold_soc = 0.2); std::size_t waypoint() const; @@ -46,15 +54,15 @@ class State State& charging_waypoint(std::size_t new_charging_waypoint); - rmf_utils::optional finish_time() const; + rmf_traffic::Time finish_time() const; State& finish_time(rmf_traffic::Time new_finish_time); - rmf_utils::optional battery_soc() const; + double battery_soc() const; State& battery_soc(double new_battery_soc); - rmf_utils::optional threshold_soc() const; + double threshold_soc() const; State& threshold_soc(double new_threshold_soc); diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index a5790416d..17e1c5705 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -18,6 +18,8 @@ #ifndef INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP #define INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#include + #include #include @@ -30,8 +32,6 @@ #include #include -#include - namespace rmf_tasks { namespace requests { @@ -47,13 +47,13 @@ class Delivery : public rmf_tasks::Request std::shared_ptr device_sink, std::shared_ptr planner, bool drain_battery = true, - double start_time = 0.0); + rmf_traffic::Time start_time = std::chrono::steady_clock::now()); std::size_t id() const final; rmf_utils::optional estimate_finish( - const RobotState& initial_state) const final; - + const agv::State& initial_state) const final; + rmf_traffic::Duration invariant_duration() const final; rmf_traffic::Time earliest_start_time() const final; diff --git a/rmf_tasks/package.xml b/rmf_tasks/package.xml index 7e12f3abb..110410a98 100644 --- a/rmf_tasks/package.xml +++ b/rmf_tasks/package.xml @@ -10,6 +10,7 @@ rmf_utils rmf_traffic + rmf_battery cmake diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index d023eb95e..b7baf4512 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -25,25 +25,37 @@ class State::Implementation { public: - Implementation(std::size_t waypoint, std::size_t charging_waypoint) + Implementation( + std::size_t waypoint, + std::size_t charging_waypoint, + rmf_traffic::Time finish_time, + double battery_soc, + double threshold_soc) : _waypoint(waypoint), - _charging_waypoint(charging_waypoint) + _charging_waypoint(charging_waypoint), + _finish_time(finish_time), + _battery_soc(battery_soc), + _threshold_soc(threshold_soc) {} std::size_t _waypoint; std::size_t _charging_waypoint; - rmf_utils::optional _finish_time; - rmf_utils::optional _battery_soc; - rmf_utils::optional _threshold_soc; + rmf_traffic::Time _finish_time; + double _battery_soc; + double _threshold_soc; }; //============================================================================== -State::State(std::size_t waypoint, std::size_t charging_waypoint) +State::State( + std::size_t waypoint, + std::size_t charging_waypoint, + rmf_traffic::Time finish_time, + double battery_soc, + double threshold_soc) : _pimpl(rmf_utils::make_impl( - Implementation{waypoint, charging_waypoint})) -{ - // do nothing -} + Implementation( + waypoint, charging_waypoint, finish_time, battery_soc, threshold_soc))) +{} //============================================================================== std::size_t State::waypoint() const @@ -72,7 +84,7 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== -rmf_utils::optional State::finish_time() const +rmf_traffic::Time State::finish_time() const { return _pimpl->_finish_time; } @@ -85,7 +97,7 @@ State& State::finish_time(rmf_traffic::Time new_finish_time) } //============================================================================== -rmf_utils::optional State::battery_soc() const +double State::battery_soc() const { return _pimpl->_battery_soc; } @@ -98,7 +110,7 @@ State& State::battery_soc(double new_battery_soc) } //============================================================================== -rmf_utils::optional State::threshold_soc() const +double State::threshold_soc() const { return _pimpl->_threshold_soc; } diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index e69bb3b5e..8395de1a2 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -15,6 +15,8 @@ * */ +#include + #include namespace rmf_tasks { @@ -24,17 +26,7 @@ class Delivery::Implementation { public: - Implementation( - std::size_t id, - std::size_t pickup_waypoint, - std::size_t dropoff_waypoint, - std::shared_ptr motion_sink, - std::shared_ptr device_sink, - std::shared_ptr planner, - bool drain_battery = true, - rmf_traffic::Time start_time) - : _id(id), - _pickup_waypoint(pickup_waypoint) + Implementation() {} std::size_t _id; @@ -44,20 +36,158 @@ class Delivery::Implementation std::shared_ptr _device_sink; std::shared_ptr _planner; bool _drain_battery; - double _invariant_battery_drain; - rmf_traffic::Duration _invariant_duration; rmf_traffic::Time _start_time; + + rmf_traffic::Duration _invariant_duration; + double _invariant_battery_drain; }; -std::size_t Delivery::id() const +rmf_tasks::Request::SharedPtr Delivery::make( + std::size_t id, + std::size_t pickup_waypoint, + std::size_t dropoff_waypoint, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + bool drain_battery, + rmf_traffic::Time start_time) +{ + std::shared_ptr delivery(new Delivery()); + delivery->_pimpl->_id = id; + delivery->_pimpl->_pickup_waypoint = pickup_waypoint; + delivery->_pimpl->_dropoff_waypoint = dropoff_waypoint; + delivery->_pimpl->_motion_sink = std::move(motion_sink); + delivery->_pimpl->_device_sink = std::move(device_sink); + delivery->_pimpl->_planner = std::move(planner); + delivery->_pimpl->_drain_battery = drain_battery; + delivery->_pimpl->_start_time = start_time; + + // Calculate duration of invariant component of task + const auto plan_start_time = std::chrono::steady_clock::now(); + rmf_traffic::agv::Planner::Start start{ + plan_start_time, + delivery->_pimpl->_pickup_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; + const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); + + const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + + delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + delivery->_pimpl->_invariant_battery_drain = 0.0; + + if (delivery->_pimpl->_drain_battery) + { + // Compute battery drain + const double dSOC_motion = + delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + delivery->_pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); + delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + } + + return delivery; +} + +Delivery::Delivery() +: _pimpl(rmf_utils::make_impl(Implementation())) {} -rmf_utils::optional estimate_finish( - const RobotState& initial_state) const final; +std::size_t Delivery::id() const +{ + return _pimpl->_id; +} + +rmf_utils::optional Delivery::estimate_finish( + const agv::State& initial_state) const +{ + agv::State state( + _pimpl->_dropoff_waypoint, + initial_state.charging_waypoint(), + initial_state.finish_time(), + initial_state.battery_soc(), + initial_state.threshold_soc()); + + rmf_traffic::Duration variant_duration(0); + + auto start_time = initial_state.finish_time(); + double battery_soc = initial_state.battery_soc(); + double dSOC_motion = 0.0; + double dSOC_device = 0.0; + + if (initial_state.waypoint() != _pimpl->_pickup_waypoint) + { + // Compute plan to pickup waypoint along with battery drain + rmf_traffic::agv::Planner::Start start{ + start_time, + initial_state.waypoint(), + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->_pickup_waypoint}; + + const auto result_to_pickup = _pimpl->_planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_pickup->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if(_pimpl->_drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); + dSOC_device = + _pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + + if (battery_soc <= state.threshold_soc()) + { + std::cout << " -- Delivery: Unable to reach pickup" << std::endl; + return rmf_utils::nullopt; + } + + start_time = finish_time; + } -rmf_traffic::Duration invariant_duration() const final; + const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; + const rmf_traffic::Time wait_until = + initial_state.finish_time() > ideal_start ? + initial_state.finish_time() : ideal_start; -rmf_traffic::Time earliest_start_time() const final; + // Factor in invariants + const rmf_traffic::Time with_invariants = + wait_until + variant_duration + _pimpl->_invariant_duration; + state.finish_time(with_invariants); + + battery_soc -= _pimpl->_invariant_battery_drain; + + if (battery_soc <= state.threshold_soc()) + { + std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; + return rmf_utils::nullopt; + } + + state.battery_soc(battery_soc); + + // TODO: Check if we have enough charge to head back to nearest charger + + return Estimate(state, wait_until); +} + +rmf_traffic::Duration Delivery::invariant_duration() const +{ + return _pimpl->_invariant_duration; +} + +rmf_traffic::Time Delivery::earliest_start_time() const +{ + return _pimpl->_start_time; +} } // namespace requests From ccb1721604bbb412e74b204d3623bc51365fd89d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 18 Sep 2020 18:32:47 +0800 Subject: [PATCH 031/128] ChargeBattery request implemented --- .../rmf_tasks/requests/ChargeBattery.hpp | 66 ++++++++ .../include/rmf_tasks/requests/Delivery.hpp | 6 +- rmf_tasks/src/requests/ChargeBattery.cpp | 158 ++++++++++++++++++ rmf_tasks/src/requests/Delivery.cpp | 13 +- 4 files changed, 238 insertions(+), 5 deletions(-) create mode 100644 rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp create mode 100644 rmf_tasks/src/requests/ChargeBattery.cpp diff --git a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp new file mode 100644 index 000000000..478003a02 --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp @@ -0,0 +1,66 @@ +/* + * 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 INCLUDE__RMF_TASKS__REQUESTS__CHARGEBATTERY_HPP +#define INCLUDE__RMF_TASKS__REQUESTS__CHARGEBATTERY_HPP + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace rmf_tasks { +namespace requests { + +class ChargeBattery : public rmf_tasks::Request +{ +public: + + static rmf_tasks::Request::SharedPtr make( + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + bool drain_battery = true); + + std::size_t id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state) const final; + + rmf_traffic::Duration invariant_duration() const final; + + rmf_traffic::Time earliest_start_time() const final; + + class Implementation; +private: + ChargeBattery(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace requests +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__REQUESTS__CHARGEBATTERY_HPP diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index 17e1c5705..07bd5b813 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP -#define INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#ifndef INCLUDE__RMF_TASKS__REQUESTS__DELIVERY_HPP +#define INCLUDE__RMF_TASKS__REQUESTS__DELIVERY_HPP #include @@ -67,4 +67,4 @@ class Delivery : public rmf_tasks::Request } // namespace tasks } // namespace rmf_tasks -#endif // INCLUDE__RMF_TASKS__TASKS__DELIVERY_HPP +#endif // INCLUDE__RMF_TASKS__REQUESTS__DELIVERY_HPP diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp new file mode 100644 index 000000000..f0222b74d --- /dev/null +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -0,0 +1,158 @@ +/* + * 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_tasks { +namespace requests { + +class ChargeBattery::Implementation +{ +public: + + Implementation() + {} + + // fixed id for now + std::size_t _id = 101; + rmf_battery::agv::BatterySystem _battery_system; + std::shared_ptr _motion_sink; + std::shared_ptr _device_sink; + std::shared_ptr _planner; + bool _drain_battery; + + // soc to always charge the battery up to + double _charge_soc = 1.0; + rmf_traffic::Duration _invariant_duration; + rmf_traffic::Time _start_time; +}; + +rmf_tasks::Request::SharedPtr ChargeBattery::make( + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + bool drain_battery) +{ + std::shared_ptr charge_battery(new ChargeBattery()); + charge_battery->_pimpl->_battery_system = std::move(battery_system); + charge_battery->_pimpl->_motion_sink = std::move(motion_sink); + charge_battery->_pimpl->_device_sink = std::move(device_sink); + charge_battery->_pimpl->_planner = std::move(planner); + charge_battery->_pimpl->_drain_battery = drain_battery; + charge_battery->_pimpl->_invariant_duration = 0.0; + charge_battery->_pimpl->_start_time = std::chrono::steady_clock::now(); + return charge_battery; +} + +ChargeBattery::ChargeBattery() +: _pimpl(rmf_utils::make_impl(Implementation())) +{} + +std::size_t ChargeBattery::id() const +{ + return _pimpl->_id; +} + +rmf_utils::optional ChargeBattery::estimate_finish( + const agv::State& initial_state) const +{ + if (abs(initial_state.battery_soc() - _charge_soc) < 1e-3) + { + std::cout << " -- Charge battery: Battery full" << std::endl; + return rmf_utils::nullopt; + } + + // Compute time taken to reach charging waypoint from current location + agv::State state( + initial_state.charging_waypoint(), + initial_state.charging_waypoint(), + initial_state.finish_time(), + initial_state.battery_soc(), + initial_state.threshold_soc()); + + const auto time_now = std::chrono::steady_clock::now(); + const auto start_time = time_now > initial_state.finish_time() ? + time_now : initial_state.finish_time(); + + double battery_soc = initial_state.battery_soc(); + rmf_traffic::Duration variant_duration(0); + + if (initial_state.waypoint != initial_state.charging_waypoint) + { + // Compute plan to charging waypoint along with battery drain + rmf_traffic::agv::Planner::Start start{ + start_time, + initial_state.waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{initial_state.charging_waypoint}; + + const auto result = _planner->plan(start, goal); + const auto& trajectory = result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const double variant_duration = finish_time - start_time; + + if (_pimpl->_drain_battery) + { + const double dSOC_motion = _motion_sink->compute_change_in_charge( + trajectory); + const double dSOC_device = _device_sink->compute_change_in_charge( + variant_duration); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + + if (battery_soc <= state.threshold_soc()) + { + // If a robot cannot reach its charging dock given its initial battery soc + std::cout << " -- Charge battery: Unable to reach charger" << std::endl; + return rmf_utils::nullopt; + } + } + + // Default _charge_soc = 1.0 + double delta_soc = _charge_soc - battery_soc; + assert(delta_soc >= 0.0); + double time_to_charge = + (3600 * delta_soc * _battery_system.nominal_capacity()) / + _battery_system.charging_current(); + + const auto wait_until = initial_state.finish_time(); + const rmf_traffic::Time after_charging = + wait_until + variant_duration + + rmf_traffic::time::from_seconds(time_to_charge); + + state.finish_time(after_charging); + state.battery_soc(_pimpl->_charge_soc); + + return Estimate(state, wait_until); +} + +rmf_traffic::Duration ChargeBattery::invariant_duration() const +{ + return _pimpl->_invariant_duration; +} + +rmf_traffic::Time ChargeBattery::earliest_start_time() const +{ + return _pimpl->_start_time; +} + +} // namespace requests +} // namespace rmf_tasks diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 8395de1a2..12dbd6ad7 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -22,6 +22,7 @@ namespace rmf_tasks { namespace requests { +//============================================================================== class Delivery::Implementation { public: @@ -42,6 +43,7 @@ class Delivery::Implementation double _invariant_battery_drain; }; +//============================================================================== rmf_tasks::Request::SharedPtr Delivery::make( std::size_t id, std::size_t pickup_waypoint, @@ -92,15 +94,18 @@ rmf_tasks::Request::SharedPtr Delivery::make( return delivery; } +//============================================================================== Delivery::Delivery() : _pimpl(rmf_utils::make_impl(Implementation())) {} +//============================================================================== std::size_t Delivery::id() const { return _pimpl->_id; } +//============================================================================== rmf_utils::optional Delivery::estimate_finish( const agv::State& initial_state) const { @@ -113,7 +118,9 @@ rmf_utils::optional Delivery::estimate_finish( rmf_traffic::Duration variant_duration(0); - auto start_time = initial_state.finish_time(); + const auto time_now = std::chrono::steady_clock::now(); + auto start_time = time_now > initial_state.finish_time() ? + time_now : initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; double dSOC_device = 0.0; @@ -179,16 +186,18 @@ rmf_utils::optional Delivery::estimate_finish( return Estimate(state, wait_until); } +//============================================================================== rmf_traffic::Duration Delivery::invariant_duration() const { return _pimpl->_invariant_duration; } +//============================================================================== rmf_traffic::Time Delivery::earliest_start_time() const { return _pimpl->_start_time; } - +//============================================================================== } // namespace requests } // namespace rmf_tasks From c2a4ee25c0168eb41177dbb1905c79061d508ff9 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 21 Sep 2020 15:31:01 +0800 Subject: [PATCH 032/128] Using finish_duration instead of finish_time --- rmf_tasks/include/rmf_tasks/agv/State.hpp | 9 ++++--- rmf_tasks/src/agv/State.cpp | 18 ++++++------- rmf_tasks/src/requests/ChargeBattery.cpp | 32 +++++++++++++---------- rmf_tasks/src/requests/Delivery.cpp | 12 ++++----- 4 files changed, 38 insertions(+), 33 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index 484cb80fb..62bbda908 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -36,13 +36,14 @@ class State /// /// \param[in] waypoint /// \param[in] charging_waypoint - /// \param[in] finish_time + /// \param[in] finish_duration + /// The duration of time until current tasks are finished. /// \param[in] battery_soc /// \param[in] threshold_soc State( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), + rmf_traffic::Duration finish_duration = rmf_traffic::Duration(0), double battery_soc = 1.0, double threshold_soc = 0.2); @@ -54,9 +55,9 @@ class State State& charging_waypoint(std::size_t new_charging_waypoint); - rmf_traffic::Time finish_time() const; + rmf_traffic::Duration finish_duration() const; - State& finish_time(rmf_traffic::Time new_finish_time); + State& finish_duration(rmf_traffic::Duration new_finish_duration); double battery_soc() const; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index b7baf4512..14e070c45 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -28,19 +28,19 @@ class State::Implementation Implementation( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Time finish_time, + rmf_traffic::Duration finish_duration, double battery_soc, double threshold_soc) : _waypoint(waypoint), _charging_waypoint(charging_waypoint), - _finish_time(finish_time), + _finish_duration(finish_duration), _battery_soc(battery_soc), _threshold_soc(threshold_soc) {} std::size_t _waypoint; std::size_t _charging_waypoint; - rmf_traffic::Time _finish_time; + rmf_traffic::Duration _finish_duration; double _battery_soc; double _threshold_soc; }; @@ -49,12 +49,12 @@ class State::Implementation State::State( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Time finish_time, + rmf_traffic::Duration finish_duration, double battery_soc, double threshold_soc) : _pimpl(rmf_utils::make_impl( Implementation( - waypoint, charging_waypoint, finish_time, battery_soc, threshold_soc))) + waypoint, charging_waypoint, finish_duration, battery_soc, threshold_soc))) {} //============================================================================== @@ -84,15 +84,15 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== -rmf_traffic::Time State::finish_time() const +rmf_traffic::Duration State::finish_duration() const { - return _pimpl->_finish_time; + return _pimpl->_finish_duration; } //============================================================================== -State& State::finish_time(rmf_traffic::Time new_finish_time) +State& State::finish_duration(rmf_traffic::Duration new_finish_duration) { - _pimpl->_finish_time = new_finish_time; + _pimpl->_finish_duration = new_finish_duration; return *this; } diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index f0222b74d..d268034d4 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -22,6 +22,7 @@ namespace rmf_tasks { namespace requests { +//============================================================================== class ChargeBattery::Implementation { public: @@ -39,10 +40,10 @@ class ChargeBattery::Implementation // soc to always charge the battery up to double _charge_soc = 1.0; - rmf_traffic::Duration _invariant_duration; - rmf_traffic::Time _start_time; + rmf_traffic::Duration _invariant_duration; }; +//============================================================================== rmf_tasks::Request::SharedPtr ChargeBattery::make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, @@ -57,19 +58,21 @@ rmf_tasks::Request::SharedPtr ChargeBattery::make( charge_battery->_pimpl->_planner = std::move(planner); charge_battery->_pimpl->_drain_battery = drain_battery; charge_battery->_pimpl->_invariant_duration = 0.0; - charge_battery->_pimpl->_start_time = std::chrono::steady_clock::now(); return charge_battery; } +//============================================================================== ChargeBattery::ChargeBattery() : _pimpl(rmf_utils::make_impl(Implementation())) {} +//============================================================================== std::size_t ChargeBattery::id() const { return _pimpl->_id; } +//============================================================================== rmf_utils::optional ChargeBattery::estimate_finish( const agv::State& initial_state) const { @@ -83,13 +86,13 @@ rmf_utils::optional ChargeBattery::estimate_finish( agv::State state( initial_state.charging_waypoint(), initial_state.charging_waypoint(), - initial_state.finish_time(), + initial_state.finish_duration(), initial_state.battery_soc(), initial_state.threshold_soc()); const auto time_now = std::chrono::steady_clock::now(); - const auto start_time = time_now > initial_state.finish_time() ? - time_now : initial_state.finish_time(); + const auto start_time = + rmf_traffic::Time::now() + initial_state.finish_duration(); double battery_soc = initial_state.battery_soc(); rmf_traffic::Duration variant_duration(0); @@ -107,14 +110,14 @@ rmf_utils::optional ChargeBattery::estimate_finish( const auto result = _planner->plan(start, goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - const double variant_duration = finish_time - start_time; + const rmf_traffic::Time variant_duration = finish_time - start_time; if (_pimpl->_drain_battery) { const double dSOC_motion = _motion_sink->compute_change_in_charge( trajectory); const double dSOC_device = _device_sink->compute_change_in_charge( - variant_duration); + rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_device; } @@ -133,26 +136,27 @@ rmf_utils::optional ChargeBattery::estimate_finish( (3600 * delta_soc * _battery_system.nominal_capacity()) / _battery_system.charging_current(); - const auto wait_until = initial_state.finish_time(); - const rmf_traffic::Time after_charging = + const auto wait_until = initial_state.finish_duration(); + state.finish_duration( wait_until + variant_duration + - rmf_traffic::time::from_seconds(time_to_charge); - - state.finish_time(after_charging); + rmf_traffic::time::from_seconds(time_to_charge)); state.battery_soc(_pimpl->_charge_soc); return Estimate(state, wait_until); } +//============================================================================== rmf_traffic::Duration ChargeBattery::invariant_duration() const { return _pimpl->_invariant_duration; } +//============================================================================== rmf_traffic::Time ChargeBattery::earliest_start_time() const { - return _pimpl->_start_time; + return rmf_traffic::Time::min(); } +//============================================================================== } // namespace requests } // namespace rmf_tasks diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 12dbd6ad7..c6773e8e3 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -112,15 +112,14 @@ rmf_utils::optional Delivery::estimate_finish( agv::State state( _pimpl->_dropoff_waypoint, initial_state.charging_waypoint(), - initial_state.finish_time(), + initial_state.finish_duration(), initial_state.battery_soc(), initial_state.threshold_soc()); rmf_traffic::Duration variant_duration(0); const auto time_now = std::chrono::steady_clock::now(); - auto start_time = time_now > initial_state.finish_time() ? - time_now : initial_state.finish_time(); + auto start_time = time_now + initial_state.finish_duration(); double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; double dSOC_device = 0.0; @@ -162,14 +161,15 @@ rmf_utils::optional Delivery::estimate_finish( } const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; + const rmf_traffic::Time without_variant = + _pimpl->_start_time + initial_state.finish_duration(); const rmf_traffic::Time wait_until = - initial_state.finish_time() > ideal_start ? - initial_state.finish_time() : ideal_start; + without_variant > ideal_start ? without_variant : ideal_start; // Factor in invariants const rmf_traffic::Time with_invariants = wait_until + variant_duration + _pimpl->_invariant_duration; - state.finish_time(with_invariants); + state.finish_duration(with_invariants - _pimpl->_start_time); battery_soc -= _pimpl->_invariant_battery_drain; From 12ee02702d543b168264f75882a0f61a0199a805 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 21 Sep 2020 22:56:03 +0800 Subject: [PATCH 033/128] Switching back to using Time instead of Duration for finish_time Signed-off-by: Aaron Chong --- rmf_tasks/cmake/rmf_tasks-config.cmake.in | 1 + rmf_tasks/include/rmf_tasks/agv/State.hpp | 2 +- rmf_tasks/src/requests/ChargeBattery.cpp | 27 ++++++++++++----------- rmf_tasks/src/requests/Delivery.cpp | 9 ++++++++ 4 files changed, 25 insertions(+), 14 deletions(-) diff --git a/rmf_tasks/cmake/rmf_tasks-config.cmake.in b/rmf_tasks/cmake/rmf_tasks-config.cmake.in index e485d6de9..71021c48b 100644 --- a/rmf_tasks/cmake/rmf_tasks-config.cmake.in +++ b/rmf_tasks/cmake/rmf_tasks-config.cmake.in @@ -6,6 +6,7 @@ include(CMakeFindDependencyMacro) find_dependency(rmf_utils REQUIRED) find_dependency(rmf_traffic REQUIRED) +find_dependency(rmf_battery REQUIRED) if(NOT TARGET rmf_tasks::rmf_tasks) include("${rmf_tasks_CMAKE_DIR}/rmf_tasks-targets.cmake") diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index 62bbda908..a6e603365 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -43,7 +43,7 @@ class State State( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Duration finish_duration = rmf_traffic::Duration(0), + rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), double battery_soc = 1.0, double threshold_soc = 0.2); diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index d268034d4..ecf336463 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -57,7 +57,8 @@ rmf_tasks::Request::SharedPtr ChargeBattery::make( charge_battery->_pimpl->_device_sink = std::move(device_sink); charge_battery->_pimpl->_planner = std::move(planner); charge_battery->_pimpl->_drain_battery = drain_battery; - charge_battery->_pimpl->_invariant_duration = 0.0; + charge_battery->_pimpl->_invariant_duration = + rmf_traffic::time::from_seconds(0.0); return charge_battery; } @@ -76,7 +77,7 @@ std::size_t ChargeBattery::id() const rmf_utils::optional ChargeBattery::estimate_finish( const agv::State& initial_state) const { - if (abs(initial_state.battery_soc() - _charge_soc) < 1e-3) + if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) { std::cout << " -- Charge battery: Battery full" << std::endl; return rmf_utils::nullopt; @@ -92,31 +93,31 @@ rmf_utils::optional ChargeBattery::estimate_finish( const auto time_now = std::chrono::steady_clock::now(); const auto start_time = - rmf_traffic::Time::now() + initial_state.finish_duration(); + std::chrono::steady_clock::now() + initial_state.finish_duration(); double battery_soc = initial_state.battery_soc(); rmf_traffic::Duration variant_duration(0); - if (initial_state.waypoint != initial_state.charging_waypoint) + if (initial_state.waypoint() != initial_state.charging_waypoint()) { // Compute plan to charging waypoint along with battery drain rmf_traffic::agv::Planner::Start start{ start_time, - initial_state.waypoint, + initial_state.waypoint(), 0.0}; - rmf_traffic::agv::Planner::Goal goal{initial_state.charging_waypoint}; + rmf_traffic::agv::Planner::Goal goal{initial_state.charging_waypoint()}; - const auto result = _planner->plan(start, goal); + const auto result = _pimpl->_planner->plan(start, goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Time variant_duration = finish_time - start_time; + const rmf_traffic::Duration variant_duration = finish_time - start_time; if (_pimpl->_drain_battery) { - const double dSOC_motion = _motion_sink->compute_change_in_charge( + const double dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge( trajectory); - const double dSOC_device = _device_sink->compute_change_in_charge( + const double dSOC_device = _pimpl->_device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_device; } @@ -130,11 +131,11 @@ rmf_utils::optional ChargeBattery::estimate_finish( } // Default _charge_soc = 1.0 - double delta_soc = _charge_soc - battery_soc; + double delta_soc = _pimpl->_charge_soc - battery_soc; assert(delta_soc >= 0.0); double time_to_charge = - (3600 * delta_soc * _battery_system.nominal_capacity()) / - _battery_system.charging_current(); + (3600 * delta_soc * _pimpl->_battery_system.nominal_capacity()) / + _pimpl->_battery_system.charging_current(); const auto wait_until = initial_state.finish_duration(); state.finish_duration( diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index c6773e8e3..e7a043828 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -16,6 +16,7 @@ */ #include +#include #include @@ -39,6 +40,14 @@ class Delivery::Implementation bool _drain_battery; rmf_traffic::Time _start_time; + struct Entry + { + std::size_t candidate; + rmf_tasks::agv::State state; + rmf_traffic::Time wait_until; + }; + std::multimap _map; + rmf_traffic::Duration _invariant_duration; double _invariant_battery_drain; }; From 3fd6724c991c2ebeaa84170b528377e8f8eb5756 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 22 Sep 2020 17:02:44 +0800 Subject: [PATCH 034/128] Migration to using this library is done --- rmf_tasks/include/rmf_tasks/agv/State.hpp | 9 ++++---- rmf_tasks/src/agv/State.cpp | 24 +++++++++++++-------- rmf_tasks/src/requests/ChargeBattery.cpp | 26 +++++++++++++---------- rmf_tasks/src/requests/Delivery.cpp | 23 ++++++-------------- 4 files changed, 41 insertions(+), 41 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index a6e603365..dc93be0ab 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -36,8 +36,7 @@ class State /// /// \param[in] waypoint /// \param[in] charging_waypoint - /// \param[in] finish_duration - /// The duration of time until current tasks are finished. + /// \param[in] finish_time /// \param[in] battery_soc /// \param[in] threshold_soc State( @@ -46,6 +45,8 @@ class State rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), double battery_soc = 1.0, double threshold_soc = 0.2); + + State(); std::size_t waypoint() const; @@ -55,9 +56,9 @@ class State State& charging_waypoint(std::size_t new_charging_waypoint); - rmf_traffic::Duration finish_duration() const; + rmf_traffic::Time finish_time() const; - State& finish_duration(rmf_traffic::Duration new_finish_duration); + State& finish_time(rmf_traffic::Time new_finish_time); double battery_soc() const; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index 14e070c45..7b4b6da39 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -28,19 +28,19 @@ class State::Implementation Implementation( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Duration finish_duration, + rmf_traffic::Time finish_time, double battery_soc, double threshold_soc) : _waypoint(waypoint), _charging_waypoint(charging_waypoint), - _finish_duration(finish_duration), + _finish_time(finish_time), _battery_soc(battery_soc), _threshold_soc(threshold_soc) {} std::size_t _waypoint; std::size_t _charging_waypoint; - rmf_traffic::Duration _finish_duration; + rmf_traffic::Time _finish_time; double _battery_soc; double _threshold_soc; }; @@ -49,12 +49,18 @@ class State::Implementation State::State( std::size_t waypoint, std::size_t charging_waypoint, - rmf_traffic::Duration finish_duration, + rmf_traffic::Time finish_time, double battery_soc, double threshold_soc) : _pimpl(rmf_utils::make_impl( Implementation( - waypoint, charging_waypoint, finish_duration, battery_soc, threshold_soc))) + waypoint, charging_waypoint, finish_time, battery_soc, threshold_soc))) +{} + +//============================================================================== +State::State() +: _pimpl(rmf_utils::make_impl( + Implementation(0, 0, std::chrono::steady_clock::now(), 0.0, 0.0))) {} //============================================================================== @@ -84,15 +90,15 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== -rmf_traffic::Duration State::finish_duration() const +rmf_traffic::Time State::finish_time() const { - return _pimpl->_finish_duration; + return _pimpl->_finish_time; } //============================================================================== -State& State::finish_duration(rmf_traffic::Duration new_finish_duration) +State& State::finish_time(rmf_traffic::Time new_finish_time) { - _pimpl->_finish_duration = new_finish_duration; + _pimpl->_finish_time = new_finish_time; return *this; } diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index ecf336463..eeb43e3f2 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -32,7 +32,7 @@ class ChargeBattery::Implementation // fixed id for now std::size_t _id = 101; - rmf_battery::agv::BatterySystem _battery_system; + rmf_battery::agv::BatterySystemPtr _battery_system; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; @@ -52,7 +52,13 @@ rmf_tasks::Request::SharedPtr ChargeBattery::make( bool drain_battery) { std::shared_ptr charge_battery(new ChargeBattery()); - charge_battery->_pimpl->_battery_system = std::move(battery_system); + charge_battery->_pimpl->_battery_system = + rmf_battery::agv::BatterySystemPtr(new rmf_battery::agv::BatterySystem( + battery_system.nominal_capacity(), + battery_system.nominal_capacity(), + battery_system.charging_current(), + battery_system.type(), + battery_system.profile())); charge_battery->_pimpl->_motion_sink = std::move(motion_sink); charge_battery->_pimpl->_device_sink = std::move(device_sink); charge_battery->_pimpl->_planner = std::move(planner); @@ -87,13 +93,11 @@ rmf_utils::optional ChargeBattery::estimate_finish( agv::State state( initial_state.charging_waypoint(), initial_state.charging_waypoint(), - initial_state.finish_duration(), + initial_state.finish_time(), initial_state.battery_soc(), initial_state.threshold_soc()); - const auto time_now = std::chrono::steady_clock::now(); - const auto start_time = - std::chrono::steady_clock::now() + initial_state.finish_duration(); + const auto start_time = initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); rmf_traffic::Duration variant_duration(0); @@ -134,12 +138,12 @@ rmf_utils::optional ChargeBattery::estimate_finish( double delta_soc = _pimpl->_charge_soc - battery_soc; assert(delta_soc >= 0.0); double time_to_charge = - (3600 * delta_soc * _pimpl->_battery_system.nominal_capacity()) / - _pimpl->_battery_system.charging_current(); + (3600 * delta_soc * _pimpl->_battery_system->nominal_capacity()) / + _pimpl->_battery_system->charging_current(); - const auto wait_until = initial_state.finish_duration(); - state.finish_duration( - wait_until + variant_duration + + const rmf_traffic::Time wait_until = initial_state.finish_time(); + state.finish_time( + wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge)); state.battery_soc(_pimpl->_charge_soc); diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index e7a043828..487392fb1 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -40,14 +40,6 @@ class Delivery::Implementation bool _drain_battery; rmf_traffic::Time _start_time; - struct Entry - { - std::size_t candidate; - rmf_tasks::agv::State state; - rmf_traffic::Time wait_until; - }; - std::multimap _map; - rmf_traffic::Duration _invariant_duration; double _invariant_battery_drain; }; @@ -121,14 +113,13 @@ rmf_utils::optional Delivery::estimate_finish( agv::State state( _pimpl->_dropoff_waypoint, initial_state.charging_waypoint(), - initial_state.finish_duration(), + initial_state.finish_time(), initial_state.battery_soc(), initial_state.threshold_soc()); rmf_traffic::Duration variant_duration(0); - const auto time_now = std::chrono::steady_clock::now(); - auto start_time = time_now + initial_state.finish_duration(); + 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; @@ -170,15 +161,13 @@ rmf_utils::optional Delivery::estimate_finish( } const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; - const rmf_traffic::Time without_variant = - _pimpl->_start_time + initial_state.finish_duration(); const rmf_traffic::Time wait_until = - without_variant > ideal_start ? without_variant : ideal_start; + initial_state.finish_time() > ideal_start ? + initial_state.finish_time() : ideal_start; // Factor in invariants - const rmf_traffic::Time with_invariants = - wait_until + variant_duration + _pimpl->_invariant_duration; - state.finish_duration(with_invariants - _pimpl->_start_time); + state.finish_time( + wait_until + variant_duration + _pimpl->_invariant_duration); battery_soc -= _pimpl->_invariant_battery_drain; From e80a7c5c9fd63f0f589c0e6538551c67c27d16d6 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 23 Sep 2020 18:13:33 +0800 Subject: [PATCH 035/128] Increased sim_step to 0.5 --- rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index d49401d3c..35c10d9c2 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -93,7 +93,7 @@ double SimpleMotionPowerSink::compute_change_in_charge( const auto motion = rmf_traffic::Motion::compute_cubic_splines( begin_it, trajectory.end()); - const double sim_step = 0.1; // seconds + const double sim_step = 0.5; // seconds // Change in energy double dE = 0.0; From 1aa471f3def06343374e27a4631c153b892fd322 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 23 Sep 2020 18:31:33 +0800 Subject: [PATCH 036/128] Cleaned up unused source files and cmake commands --- rmf_tasks/CMakeLists.txt | 28 ------ rmf_tasks/include/rmf_tasks/Task.hpp | 0 rmf_tasks/include/rmf_tasks/tasks/Loop.hpp | 16 ---- rmf_tasks/src/Task.cpp | 0 rmf_tasks/src/Task.hpp | 101 --------------------- rmf_tasks/src/main.cpp | 7 -- rmf_tasks/src/tasks/Delivery.cpp | 0 rmf_tasks/src/tasks/Loop.cpp | 0 8 files changed, 152 deletions(-) delete mode 100644 rmf_tasks/include/rmf_tasks/Task.hpp delete mode 100644 rmf_tasks/include/rmf_tasks/tasks/Loop.hpp delete mode 100644 rmf_tasks/src/Task.cpp delete mode 100644 rmf_tasks/src/Task.hpp delete mode 100644 rmf_tasks/src/main.cpp delete mode 100644 rmf_tasks/src/tasks/Delivery.cpp delete mode 100644 rmf_tasks/src/tasks/Loop.cpp diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt index 2611f3e17..73b706b4a 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_tasks/CMakeLists.txt @@ -48,34 +48,6 @@ target_include_directories(rmf_tasks $ ) -# find_package(ament_cmake_catch2 QUIET) -# find_package(rmf_cmake_uncrustify QUIET) -# if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) -# file(GLOB_RECURSE unit_test_srcs "test/*.cpp") - -# ament_add_catch2( -# test_rmf_traffic test/main.cpp ${unit_test_srcs} -# TIMEOUT 300) -# target_link_libraries(test_rmf_traffic -# rmf_traffic -# ${PC_FCL_LIBRARIES} -# Threads::Threads -# ) - -# target_include_directories(test_rmf_traffic -# PRIVATE -# $ -# ) - -# find_file(uncrustify_config_file NAMES "share/format/rmf_code_style.cfg") - -# rmf_uncrustify( -# ARGN include src test -# CONFIG_FILE ${uncrustify_config_file} -# MAX_LINE_LENGTH 80 -# ) -# endif() - # Create cmake config files include(CMakePackageConfigHelpers) diff --git a/rmf_tasks/include/rmf_tasks/Task.hpp b/rmf_tasks/include/rmf_tasks/Task.hpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp b/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp deleted file mode 100644 index 7f704809e..000000000 --- a/rmf_tasks/include/rmf_tasks/tasks/Loop.hpp +++ /dev/null @@ -1,16 +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. - * -*/ \ No newline at end of file diff --git a/rmf_tasks/src/Task.cpp b/rmf_tasks/src/Task.cpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/rmf_tasks/src/Task.hpp b/rmf_tasks/src/Task.hpp deleted file mode 100644 index b32e28a05..000000000 --- a/rmf_tasks/src/Task.hpp +++ /dev/null @@ -1,101 +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 SRC__TASK_HPP -#define SRC__TASK_HPP - -#include -#include -#include - -#include - -namespace rmf_tasks { - -class Task : public std::enable_shared_from_this -{ -public: - - using StatusMsg = rmf_task_msgs::msg::TaskSummary; - - /// This class represents the active phase of a Task. It provides an - /// observable that the Task can track to stay up-to-date on the status and to - /// know when to begin the next phase. - /// - /// The ActivePhase class must be a schedule Negotiator so that it can - /// negotiate its way out of conflicts with other schedule participants to - /// complete its work. - class ActivePhase - { - public: - - using StatusMsg = Task::StatusMsg; - - /// Get a reference to an observable for the status of this ActivePhase. - /// When this phase is complete, it will trigger on_completed() - virtual const rxcpp::observable& observe() const = 0; - - /// Estimate how much time remains in this phase. - virtual rmf_traffic::Duration estimate_remaining_time() const = 0; - - /// Activate or deactivate the emergency alarm behavior. - virtual void emergency_alarm(bool on) = 0; - - /// Tell this phase to cancel - virtual void cancel() = 0; - - /// Human-readable description of the phase - virtual const std::string& description() const = 0; - - // Virtual destructor - virtual ~ActivePhase() = default; - }; - - class PendingPhase - { - public: - - /// Begin this phase. - virtual std::shared_ptr begin() = 0; - - /// Estimate how much time this phase will require. - virtual rmf_traffic::Duration estimate_phase_duration() const = 0; - - /// Human-readable description of the phase - virtual const std::string& description() const = 0; - - // Virtual destructor - virtual ~PendingPhase() = default; - }; - - using StatusCallback = - std::function; - - using PendingPhases = std::vector>; - - static std::shared_ptr make( - std::string id, - PendingPhases phases); - -private: - - -}; - -} // namespace rmf_tasks - -#endif // SRC__TASK_HPP diff --git a/rmf_tasks/src/main.cpp b/rmf_tasks/src/main.cpp deleted file mode 100644 index 32b997012..000000000 --- a/rmf_tasks/src/main.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include - -int main() -{ - std::cout << "all done" << std::endl; - return 0; -} diff --git a/rmf_tasks/src/tasks/Delivery.cpp b/rmf_tasks/src/tasks/Delivery.cpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/rmf_tasks/src/tasks/Loop.cpp b/rmf_tasks/src/tasks/Loop.cpp deleted file mode 100644 index e69de29bb..000000000 From f17171344abfa1a7f2cfba1e69d568f84f65076d Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 25 Sep 2020 17:57:53 +0800 Subject: [PATCH 037/128] Initial commit Signed-off-by: Yadunund Vijay --- rmf_tasks/CMakeLists.txt | 2 - .../include/rmf_tasks/agv/TaskPlanner.hpp | 137 ++++++++++++++++++ rmf_tasks/package.xml | 2 - 3 files changed, 137 insertions(+), 4 deletions(-) create mode 100644 rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt index 73b706b4a..e9d6559a5 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_tasks/CMakeLists.txt @@ -37,8 +37,6 @@ add_library(rmf_tasks SHARED target_link_libraries(rmf_tasks PRIVATE - rmf_utils::rmf_utils - rmf_traffic::rmf_traffic rmf_battery::rmf_battery ) diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp new file mode 100644 index 000000000..f9578a8ef --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -0,0 +1,137 @@ +/* + * 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_TASKS__AGV__TASKPLANNER_HPP +#define RMF_TASKS__AGV__TASKPLANNER_HPP + +#include +#include + +#include + +#include + +namespace rmf_tasks { +namespace agv { + + + +//============================================================================== +class TaskPlanner +{ +public: + + // The type of filter used for solving the task assignment problem + enum class FilterType + { + Passthrough, + Trie, + Hash + }; + + /// The Configuration class contains planning parameters that are immutable + /// for each TaskPlanner instance and should not change in between plans. + class Configuration + { + public: + /// Constructor + /// + /// \param[in] charge_battery_request + /// A pointer to the ChargeBattery request for this AGV + /// + /// \param[in] filter_type + /// The type of filter used for planning + Configuration( + Request::SharedPtr charge_battery_request, + const FilterType filter_type= FilterType::Hash); + + /// Get the pointer to the ChargeBattery request + Request::SharedPtr charge_battery_request() const; + + /// Set the pointer to the ChargeBattery request + Configuration& charge_battery_request(Request::SharedPtr charge_battery); + class Implementation; + + private: + rmf_utils::impl_ptr _pimpl; + }; + + class Assignment + { + public: + + /// Constructor + /// + /// \param[in] task_id + /// The task id for this assignment + /// + /// \param[in] state + /// The state of the agent at the end of the assigned task + /// + /// \param[in] earliest_start_time + /// The earliest time the agent will begin exececuting this task + Assignment( + std::size_t task_id, + State state, + rmf_traffic::Time earliest_start_time); + + + // Get a const reference to the task_id + const std::size_t task_id() const; + + // Get a const reference to the state + const State& state() const; + + class Implementation; + + private: + rmf_utils::impl_ptr _pimpl; + }; + + using Assignments = std::vector>; + + // Forward declaration + class Result; + + TaskPlanner(Configuration configuration); + + Result plan( + std::vector initial_states, + std::vector requests); + + + class Implementation; + +private: + rmf_utils::impl_ptr _pimpl; + +}; + + +class TaskPlanner::Result +{ +public: + // Get the results of the greedy algorithm based planner + Assignments& greedy_solution() const; + + +} + +} // namespace agv +} // namespace rmf_tasks + +#endif // RMF_TASKS__AGV__TASKPLANNER_HPP diff --git a/rmf_tasks/package.xml b/rmf_tasks/package.xml index 110410a98..91370e207 100644 --- a/rmf_tasks/package.xml +++ b/rmf_tasks/package.xml @@ -8,8 +8,6 @@ Apache License 2.0 Aaron - rmf_utils - rmf_traffic rmf_battery From 8bd15507b2d045e4a92fe7d64b2477ab9b7d50a8 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 28 Sep 2020 10:24:00 +0800 Subject: [PATCH 038/128] Started implementation --- rmf_tasks/CMakeLists.txt | 2 +- .../include/rmf_tasks/agv/TaskPlanner.hpp | 52 +++++-- rmf_tasks/src/agv/TaskPlanner.cpp | 132 ++++++++++++++++++ 3 files changed, 171 insertions(+), 15 deletions(-) create mode 100644 rmf_tasks/src/agv/TaskPlanner.cpp diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt index e9d6559a5..a7e133738 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_tasks/CMakeLists.txt @@ -24,7 +24,7 @@ find_package(rmf_utils REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_battery REQUIRED) -# ===== Traffic control library +# ===== RMF Tasks library file(GLOB lib_srcs "src/agv/*.cpp" "src/requests/*.cpp" diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp index f9578a8ef..7412ec07c 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -24,6 +24,7 @@ #include #include +#include namespace rmf_tasks { namespace agv { @@ -57,13 +58,20 @@ class TaskPlanner /// The type of filter used for planning Configuration( Request::SharedPtr charge_battery_request, - const FilterType filter_type= FilterType::Hash); + FilterType filter_type= FilterType::Hash); /// Get the pointer to the ChargeBattery request Request::SharedPtr charge_battery_request() const; /// Set the pointer to the ChargeBattery request Configuration& charge_battery_request(Request::SharedPtr charge_battery); + + /// Get the filter type + FilterType filter_type() const; + + /// Set the filter type + Configuration& filter_type(FilterType filter_type); + class Implementation; private: @@ -91,11 +99,14 @@ class TaskPlanner // Get a const reference to the task_id - const std::size_t task_id() const; + std::size_t task_id() const; // Get a const reference to the state const State& state() const; + // Get a const reference to the earliest start time + const rmf_traffic::Time& earliest_start_time() const; + class Implementation; private: @@ -104,15 +115,37 @@ class TaskPlanner using Assignments = std::vector>; - // Forward declaration - class Result; + /// Constructor + /// + /// \param[in] config + /// The configuration for the planner + TaskPlanner(Configuration config); - TaskPlanner(Configuration configuration); + using Result = std::pair>; + /// Return a pair containting the greedy solution and a future to the optimal + /// solution for a set of initial states and requests Result plan( std::vector initial_states, std::vector requests); + /// Get the greedy planner based assignments for a set of initial states and + /// requests + Assignments greedy_plan( + std::vector initial_states, + std::vector requests); + + /// Get the optimal planner based assignments for a set of initial states and + /// requests + /// \note When the number of requests exceed 10 for the same start time + /// segment, this plan may take a while to be generated. Hence, it is + /// recommended to call plan() method and use the greedy solution for bidding. + /// If a bid is awarded, the optimal solution may be used for assignments. + Assignments optimal_plan( + std::vector initial_states, + std::vector requests); + + double compute_cost(const Assignments); class Implementation; @@ -122,15 +155,6 @@ class TaskPlanner }; -class TaskPlanner::Result -{ -public: - // Get the results of the greedy algorithm based planner - Assignments& greedy_solution() const; - - -} - } // namespace agv } // namespace rmf_tasks diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp new file mode 100644 index 000000000..24659b51b --- /dev/null +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -0,0 +1,132 @@ +/* + * 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_tasks { +namespace agv { + + +//============================================================================== +class TaskPlanner::Configuration::Implementation +{ +public: + + Request::SharedPtr charge_battery_request; + FilterType filter_type; +}; + +TaskPlanner::Configuration::Configuration( + Request::SharedPtr charge_battery_request, + const FilterType filter_type) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(charge_battery_request), + filter_type + })) +{ + // Do nothing +} + +//============================================================================== +Request::SharedPtr TaskPlanner::Configuration::charge_battery_request() const +{ + return std::move(_pimpl->charge_battery_request); +} + +//============================================================================== +auto TaskPlanner::Configuration::charge_battery_request( + Request::SharedPtr charge_battery_request) -> Configuration& +{ + _pimpl->charge_battery_request = std::move(charge_battery_request); + return *this; +} + +//============================================================================== +TaskPlanner::FilterType TaskPlanner::Configuration::filter_type() const +{ + return _pimpl->filter_type; +} + +//============================================================================== +auto TaskPlanner::Configuration::filter_type( + TaskPlanner::FilterType filter_type) -> Configuration& +{ + _pimpl->filter_type = filter_type; + return *this; +} + +//============================================================================== +class TaskPlanner::Assignment::Implementation +{ +public: + + std::size_t task_id; + State state; + rmf_traffic::Time earliest_start_time; +}; + +//============================================================================== +TaskPlanner::Assignment::Assignment( + std::size_t task_id, + State state, + rmf_traffic::Time earliest_start_time) +: _pimpl(rmf_utils::make_impl( + Implementation{ + task_id, + std::move(state), + earliest_start_time + })) +{ + // Do nothing +} + +//============================================================================== +std::size_t TaskPlanner::Assignment::task_id() const +{ + return _pimpl->task_id; +} + +//============================================================================== +const State& TaskPlanner::Assignment::state() const +{ + return _pimpl->state; +} + +//============================================================================== +const rmf_traffic::Time& TaskPlanner::Assignment::earliest_start_time() const +{ + return _pimpl->earliest_start_time; +} + +//============================================================================== + +namespace { + +} // anonymous namespace + + +class TaskPlanner::Implementation +{ + +} + + +} // namespace agv +} // namespace rmf_tasks From c392c1ce69f602e60142ac3e8a8653a6eb759615 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 29 Sep 2020 15:19:36 +0800 Subject: [PATCH 039/128] Completed migration and added toy ptoblem as unit test Signed-off-by: Yadunund Vijay --- rmf_battery/CMakeLists.txt | 5 + rmf_tasks/CMakeLists.txt | 38 +- .../include/rmf_tasks/agv/TaskPlanner.hpp | 18 +- rmf_tasks/package.xml | 6 + rmf_tasks/src/agv/TaskPlanner.cpp | 956 ++++++++++++++++++ rmf_tasks/src/requests/ChargeBattery.cpp | 4 +- rmf_tasks/test/main.cpp | 21 + rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 342 +++++++ 8 files changed, 1375 insertions(+), 15 deletions(-) create mode 100644 rmf_tasks/test/main.cpp create mode 100644 rmf_tasks/test/unit/agv/test_TaskPlanner.cpp diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 77cdaa651..99d5e742f 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -10,6 +10,11 @@ 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() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") diff --git a/rmf_tasks/CMakeLists.txt b/rmf_tasks/CMakeLists.txt index a7e133738..a91cdafb3 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_tasks/CMakeLists.txt @@ -18,11 +18,19 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") + + include(GNUInstallDirs) find_package(rmf_utils REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_battery REQUIRED) +find_package(Eigen3 REQUIRED) + +find_package(ament_cmake_catch2 QUIET) +find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Tasks library file(GLOB lib_srcs @@ -36,7 +44,7 @@ add_library(rmf_tasks SHARED ) target_link_libraries(rmf_tasks - PRIVATE + PUBLIC rmf_battery::rmf_battery ) @@ -44,8 +52,36 @@ target_include_directories(rmf_tasks PUBLIC $ $ + ${EIGEN3_INCLUDE_DIRS} ) +if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) + file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + + ament_add_catch2( + test_rmf_tasks test/main.cpp ${unit_test_srcs} + TIMEOUT 300) + target_link_libraries(test_rmf_tasks + rmf_tasks + rmf_utils::rmf_utils + rmf_traffic::rmf_traffic + ) + + target_include_directories(test_rmf_tasks + PRIVATE + $ + ) + + find_file(uncrustify_config_file NAMES "share/format/rmf_code_style.cfg") + + rmf_uncrustify( + ARGN include src test + CONFIG_FILE ${uncrustify_config_file} + MAX_LINE_LENGTH 80 + ) +endif() + + # Create cmake config files include(CMakePackageConfigHelpers) diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp index 7412ec07c..0b038a6cf 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -24,7 +24,8 @@ #include #include -#include +#include +#include namespace rmf_tasks { namespace agv { @@ -119,15 +120,7 @@ class TaskPlanner /// /// \param[in] config /// The configuration for the planner - TaskPlanner(Configuration config); - - using Result = std::pair>; - - /// Return a pair containting the greedy solution and a future to the optimal - /// solution for a set of initial states and requests - Result plan( - std::vector initial_states, - std::vector requests); + TaskPlanner(std::shared_ptr config); /// Get the greedy planner based assignments for a set of initial states and /// requests @@ -143,9 +136,10 @@ class TaskPlanner /// If a bid is awarded, the optimal solution may be used for assignments. Assignments optimal_plan( std::vector initial_states, - std::vector requests); + std::vector requests, + std::function interrupter); - double compute_cost(const Assignments); + double compute_cost(const Assignments& assignments); class Implementation; diff --git a/rmf_tasks/package.xml b/rmf_tasks/package.xml index 91370e207..618939f87 100644 --- a/rmf_tasks/package.xml +++ b/rmf_tasks/package.xml @@ -9,6 +9,12 @@ Aaron rmf_battery + rmf_utils + + eigen + + ament_cmake_catch2 + rmf_cmake_uncrustify cmake diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 24659b51b..206919df6 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -16,8 +16,19 @@ */ #include +#include +#include +#include + +#include #include +#include +#include +#include +#include +#include +#include namespace rmf_tasks { namespace agv { @@ -119,14 +130,959 @@ const rmf_traffic::Time& TaskPlanner::Assignment::earliest_start_time() const namespace { +// ============================================================================ +struct Invariant +{ + std::size_t task_id; + double invariant_cost; +}; + +// ============================================================================ +struct InvariantLess +{ + bool operator()(const Invariant& a, const Invariant& b) const + { + return a.invariant_cost < b.invariant_cost; + } +}; + +// ============================================================================ +class Candidates +{ +public: + + struct Entry + { + std::size_t candidate; + State state; + rmf_traffic::Time wait_until; + }; + + // Map finish time to Entry + using Map = std::multimap; + + static Candidates make( + const std::vector& initial_states, + const rmf_tasks::Request& request); + + Candidates(const Candidates& other) + { + _value_map = other._value_map; + _update_map(); + } + + Candidates& operator=(const Candidates& other) + { + _value_map = other._value_map; + _update_map(); + return *this; + } + + Candidates(Candidates&&) = default; + Candidates& operator=(Candidates&&) = default; + + // We may have more than one best candidate so we store their iterators in + // a Range + struct Range + { + Map::const_iterator begin; + Map::const_iterator end; + }; + + Range best_candidates() const + { + assert(!_value_map.empty()); + + Range range; + range.begin = _value_map.begin(); + auto it = range.begin; + while (it->first == range.begin->first) + ++it; + + range.end = it; + return range; + } + + rmf_traffic::Time best_finish_time() const + { + assert(!_value_map.empty()); + return _value_map.begin()->first; + } + + void update_candidate( + std::size_t candidate, + State state, + rmf_traffic::Time wait_until) + { + const auto it = _candidate_map.at(candidate); + _value_map.erase(it); + _candidate_map[candidate] = _value_map.insert( + { + state.finish_time(), + Entry{candidate, state, wait_until} + }); + } + +private: + Map _value_map; + std::vector _candidate_map; + + Candidates(Map candidate_values) + : _value_map(std::move(candidate_values)) + { + _update_map(); + } + + void _update_map() + { + for (auto it = _value_map.begin(); it != _value_map.end(); ++it) + { + const auto c = it->second.candidate; + if (_candidate_map.size() <= c) + _candidate_map.resize(c+1); + + _candidate_map[c] = it; + } + } +}; + +Candidates Candidates::make( + const std::vector& initial_states, + const rmf_tasks::Request& request) +{ + Map initial_map; + for (std::size_t i = 0; i < initial_states.size(); ++i) + { + const auto& state = initial_states[i]; + const auto finish = request.estimate_finish(state); + if (finish.has_value()) + { + initial_map.insert( + { + finish.value().finish_state().finish_time(), + Entry{i, finish.value().finish_state(), finish.value().wait_until()} + }); + } + } + + return Candidates(std::move(initial_map)); +} + +// ============================================================================ +struct PendingTask +{ + PendingTask( + std::vector initial_states, + rmf_tasks::Request::SharedPtr request_) + : request(std::move(request_)), + candidates(Candidates::make(initial_states, *request)), + earliest_start_time(request->earliest_start_time()) + { + // Do nothing + } + + rmf_tasks::Request::SharedPtr request; + Candidates candidates; + rmf_traffic::Time earliest_start_time; +}; + +// ============================================================================ +struct Node +{ + using AssignedTasks = TaskPlanner::Assignments; + using UnassignedTasks = + std::unordered_map; + using InvariantSet = std::multiset; + + AssignedTasks assigned_tasks; + UnassignedTasks unassigned_tasks; + double cost_estimate; + rmf_traffic::Time latest_time; + InvariantSet unassigned_invariants; + + void sort_invariants() + { + unassigned_invariants.clear(); + for (const auto& u : unassigned_tasks) + { + unassigned_invariants.insert( + Invariant{ + u.first, + rmf_traffic::time::to_seconds(u.second.request->invariant_duration()) + }); + } + } + + void pop_unassigned(std::size_t task_id) + { + unassigned_tasks.erase(task_id); + + bool popped_invariant = false; + for (auto it = unassigned_invariants.begin(); + it != unassigned_invariants.end(); ++it) + { + if (it->task_id == task_id) + { + popped_invariant = true; + unassigned_invariants.erase(it); + } + } + + assert(popped_invariant); + } +}; + + +using NodePtr = std::shared_ptr; +using ConstNodePtr = std::shared_ptr; + +// ============================================================================ +struct LowestCostEstimate +{ + bool operator()(const ConstNodePtr& a, const ConstNodePtr& b) + { + return b->cost_estimate < a->cost_estimate; + } +}; + +//============================================================================== +class InvariantHeuristicQueue +{ +public: + + InvariantHeuristicQueue(std::vector initial_values) + { + assert(!initial_values.empty()); + std::sort(initial_values.begin(), initial_values.end()); + + for (const auto value : initial_values) + _stacks.push_back({value}); + } + + void add(double new_value) + { + // Add the new value to the smallest stack + const double value = _stacks[0].back() + new_value; + _stacks[0].push_back(value); + + // Find the largest stack that is still smaller than the current front + const auto next_it = _stacks.begin() + 1; + auto end_it = next_it; + for (; end_it != _stacks.end(); ++end_it) + { + if (value <= end_it->back()) + break; + } + + if (next_it != end_it) + { + // Rotate the vector elements to move the front stack to its new place + // in the order + std::rotate(_stacks.begin(), next_it, end_it); + } + } + + double compute_cost() const + { + double total_cost = 0.0; + for (const auto& stack : _stacks) + { + // NOTE: We start iterating from i=1 because i=0 represents a component of + // the cost that is already accounted for by g(n) and the variant + // component of h(n) + for (std::size_t i=1; i < stack.size(); ++i) + total_cost += stack[i]; + } + + return total_cost; + } + +private: + std::vector> _stacks; +}; + +// ============================================================================ +class Filter +{ +public: + + Filter(TaskPlanner::FilterType type, const std::size_t N_tasks) + : _type(type), + _set(N_tasks, AssignmentHash(N_tasks)) + { + // Do nothing + } + + bool ignore(const Node& node); + +private: + + struct TaskTable; + + struct AgentTable + { + std::unordered_map> agent; + }; + + struct TaskTable + { + std::unordered_map> task; + }; + + struct AssignmentHash + { + AssignmentHash(std::size_t N) + { + // We add 1 to N because + _shift = std::ceil(std::log2(N+1)); + } + + std::size_t operator()(const Node::AssignedTasks& assignments) const + { + std::size_t output = 0; + std::size_t count = 0; + for (const auto& a : assignments) + { + for (const auto& s : a) + { + // We add 1 to the task_id to differentiate between task_id == 0 and + // a task being unassigned. + const std::size_t id = s.task_id() + 1; + output += id << (_shift * (count++)); + } + } + + return output; + } + + std::size_t _shift; + }; + + struct AssignmentEqual + { + bool operator()( + const Node::AssignedTasks& A, const Node::AssignedTasks& B) const + { + if (A.size() != B.size()) + return false; + + for (std::size_t i=0; i < A.size(); ++i) + { + const auto& a = A[i]; + const auto& b = B[i]; + + if (a.size() != b.size()) + return false; + + for (std::size_t j=0; j < a.size(); ++j) + { + if (a[j].task_id() != b[j].task_id()) + return false; + } + } + + return true; + } + }; + + using Set = std::unordered_set; + + TaskPlanner::FilterType _type; + AgentTable _root; + Set _set; +}; + +bool Filter::ignore(const Node& node) +{ + if (_type == TaskPlanner::FilterType::Passthrough) + return false; + + if (_type == TaskPlanner::FilterType::Hash) + return !_set.insert(node.assigned_tasks).second; + + bool new_node = false; + + // TODO(MXG): Consider replacing this tree structure with a hash set + + AgentTable* agent_table = &_root; + std::size_t a = 0; + std::size_t t = 0; + while(a < node.assigned_tasks.size()) + { + const auto& current_agent = node.assigned_tasks.at(a); + + if (t < current_agent.size()) + { + const auto& task_id = current_agent[t].task_id(); + const auto agent_insertion = agent_table->agent.insert({a, nullptr}); + if (agent_insertion.second) + agent_insertion.first->second = std::make_unique(); + + auto* task_table = agent_insertion.first->second.get(); + + const auto task_insertion = task_table->task.insert({task_id, nullptr}); + if (task_insertion.second) + { + new_node = true; + task_insertion.first->second = std::make_unique(); + } + + agent_table = task_insertion.first->second.get(); + ++t; + } + else + { + t = 0; + ++a; + } + } + + return !new_node; +} + +const rmf_traffic::Duration segmentation_threshold = + rmf_traffic::time::from_seconds(1.0); + } // anonymous namespace class TaskPlanner::Implementation { +public: + + std::shared_ptr config; + + double compute_g(const Assignments& assigned_tasks) + { + double cost = 0.0; + for (const auto& agent : assigned_tasks) + { + for (const auto& assignment : agent) + { + cost += + rmf_traffic::time::to_seconds( + assignment.state().finish_time() - assignment.earliest_start_time()); + } + } + + return cost; + } + + Assignments complete_solve( + std::vector initial_states, + std::vector requests, + std::function interrupter, + bool greedy) + + { + const rmf_traffic::Time start_time = std::chrono::steady_clock::now(); + auto node = make_initial_node(initial_states, requests, start_time); + + Node::AssignedTasks complete_assignments; + complete_assignments.resize(node->assigned_tasks.size()); + + while (node) + { + if (greedy) + node = greedy_solve(node, initial_states, start_time); + else + node = solve(node, initial_states, requests.size(), start_time, interrupter); + + if (!node) + { + // std::cout << "No solution found!" << std::endl; + return {}; + } + + assert(complete_assignments.size() == node->assigned_tasks.size()); + for (std::size_t i = 0; i < complete_assignments.size(); ++i) + { + auto& all_assignments = complete_assignments[i]; + const auto& new_assignments = node->assigned_tasks[i]; + for (const auto& a : new_assignments) + { + all_assignments.push_back(a); + // all_assignments.back().task_id = task_id_map.at(a.task_id); + } + } + + if (node->unassigned_tasks.empty()) + return complete_assignments; + + // std::unordered_map new_task_id_map; + std::vector new_tasks; + // std::size_t task_counter = 0; + for (const auto& u : node->unassigned_tasks) + { + new_tasks.push_back(u.second.request); + // new_task_id_map[task_counter++] = task_id_map[u.first]; + } + + // task_id_map = std::move(new_task_id_map); + + // copy final state estimates + std::vector estimates; + estimates.resize(node->assigned_tasks.size()); + 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().state(); + } + + node = make_initial_node(estimates, new_tasks, start_time); + initial_states = estimates; + } + + return complete_assignments; + } + + double compute_g(const Node& node) + { + return compute_g(node.assigned_tasks); + } + + double compute_h(const Node& node, const rmf_traffic::Time relative_start_time) + { + std::vector initial_queue_values; + initial_queue_values.resize( + node.assigned_tasks.size(), std::numeric_limits::infinity()); + + for (const auto& u : node.unassigned_tasks) + { + // We subtract the invariant duration here because otherwise its + // contribution to the cost estimate will be duplicated in the next section, + // which could result in an overestimate. + const rmf_traffic::Time variant_time = + u.second.candidates.best_finish_time() + - u.second.request->invariant_duration(); + const double variant_value = + rmf_traffic::time::to_seconds(variant_time - relative_start_time); + + const auto& range = u.second.candidates.best_candidates(); + for (auto it = range.begin; it != range.end; ++it) + { + const std::size_t candidate = it->second.candidate; + if (variant_value < initial_queue_values[candidate]) + initial_queue_values[candidate] = variant_value; + } + } + + for (std::size_t i=0; i < initial_queue_values.size(); ++i) + { + auto& value = initial_queue_values[i]; + if (std::isinf(value)) + { + // Clear out any infinity placeholders. Those candidates simply don't have + // any unassigned tasks that want to use it. + const auto& assignments = node.assigned_tasks[i]; + if (assignments.empty()) + value = 0.0; + else + value = + rmf_traffic::time::to_seconds( + assignments.back().state().finish_time() - relative_start_time); + } + } + + InvariantHeuristicQueue queue(std::move(initial_queue_values)); + // NOTE: It is crucial that we use the ordered set of unassigned_invariants + // here. The InvariantHeuristicQueue expects the invariant costs to be passed + // to it in order of smallest to largest. If that assumption is not met, then + // the final cost that's calculated may be invalid. + for (const auto& u : node.unassigned_invariants) + queue.add(u.invariant_cost); + + return queue.compute_cost(); + } + + double compute_f(const Node& n, const rmf_traffic::Time relative_start_time) + { + return compute_g(n) + compute_h(n, relative_start_time); + } + + ConstNodePtr make_initial_node( + std::vector initial_states, + std::vector requests, + rmf_traffic::Time relative_start_time) + { + auto initial_node = std::make_shared(); + + initial_node->assigned_tasks.resize(initial_states.size()); + + for (const auto& request : requests) + initial_node->unassigned_tasks.insert( + {request->id(), PendingTask(initial_states, request)}); + + initial_node->cost_estimate = compute_f(*initial_node, relative_start_time); + + initial_node->sort_invariants(); + + initial_node->latest_time = [&]() -> rmf_traffic::Time + { + rmf_traffic::Time latest = rmf_traffic::Time::min(); + for (const auto& s : initial_states) + { + if (latest < s.finish_time()) + latest = s.finish_time(); + } + + return latest; + }(); + + rmf_traffic::Time wait_until = rmf_traffic::Time::max(); + + for (const auto& u : initial_node->unassigned_tasks) + { + const auto& range = u.second.candidates.best_candidates(); + for (auto it = range.begin; it != range.end; ++it) + { + if (it->second.wait_until < wait_until) + wait_until = it->second.wait_until; + } + } + + if (initial_node->latest_time < wait_until) + initial_node->latest_time = wait_until; + + return initial_node; + } + + rmf_traffic::Time get_latest_time(const Node& node) + { + rmf_traffic::Time latest = rmf_traffic::Time::min(); + for (const auto& a : node.assigned_tasks) + { + if (a.empty()) + continue; + + const auto finish_time = a.back().state().finish_time(); + if (latest < finish_time) + latest = finish_time; + } + + assert (latest > rmf_traffic::Time::min()); + return latest; + } + + ConstNodePtr expand_candidate( + const Candidates::Map::const_iterator& it, + const Node::UnassignedTasks::value_type& u, + const ConstNodePtr& parent, + Filter* filter, + rmf_traffic::Time relative_start_time) + + { + const auto& entry = it->second; + + if (parent->latest_time + segmentation_threshold < entry.wait_until) + { + + // No need to assign task as timeline is not relevant + return nullptr; + } + + auto new_node = std::make_shared(*parent); + + // Assign the unassigned task + new_node->assigned_tasks[entry.candidate].push_back( + Assignment{u.first, entry.state, u.second.earliest_start_time}); + + // Erase the assigned task from unassigned tasks + new_node->pop_unassigned(u.first); + + // Update states of unassigned tasks for the candidate + for (auto& new_u : new_node->unassigned_tasks) + { + const auto finish = + new_u.second.request->estimate_finish(entry.state); + if (finish.has_value()) + { + new_u.second.candidates.update_candidate( + entry.candidate, + finish.value().finish_state(), + finish.value().wait_until()); + } + else + { + return nullptr; + } + } + + // Update the cost estimate for new_node + new_node->cost_estimate = compute_f(*new_node, relative_start_time); + new_node->latest_time = get_latest_time(*new_node); + + // Apply filter + if (filter && filter->ignore(*new_node)) + { + return nullptr; + } + + return new_node; + + } + + ConstNodePtr expand_charger( + ConstNodePtr parent, + const std::size_t agent, + const std::vector initial_states, + rmf_traffic::Time relative_start_time) + { + auto new_node = std::make_shared(*parent); + // Assign charging task to an agent + const auto& assignments = new_node->assigned_tasks[agent]; + State state; + if (!assignments.empty()) + { + state = assignments.back().state(); + } + else + { + // We use the initial state of the robot + state = initial_states[agent]; + } + + auto estimate = config->charge_battery_request()->estimate_finish(state); + if (estimate.has_value()) + { + new_node->assigned_tasks[agent].push_back( + Assignment{ + config->charge_battery_request()->id(), + estimate.value().finish_state(), + estimate.value().wait_until()}); + + for (auto& new_u : new_node->unassigned_tasks) + { + const auto finish = + new_u.second.request->estimate_finish(estimate.value().finish_state()); + if (finish.has_value()) + { + new_u.second.candidates.update_candidate( + agent, finish.value().finish_state(), finish.value().wait_until()); + } + else + { + return nullptr; + } + } + + new_node->cost_estimate = compute_f(*new_node, relative_start_time); + new_node->latest_time = get_latest_time(*new_node); + return new_node; + } + + return nullptr; + } + + ConstNodePtr greedy_solve( + ConstNodePtr node, + const std::vector initial_states, + rmf_traffic::Time relative_start_time) + { + while (!finished(*node)) + { + ConstNodePtr next_node = nullptr; + for (const auto& u : node->unassigned_tasks) + { + const auto& range = u.second.candidates.best_candidates(); + for (auto it = range.begin; it != range.end; ++it) + { + if (auto n = expand_candidate(it, u, node, nullptr, relative_start_time)) + { + if (!next_node || (n->cost_estimate < next_node->cost_estimate)) + { + next_node = std::move(n); + } + } + else + { + // expand_candidate returned nullptr either due to start time + // segmentation or insufficient charge to complete task. For the + // later case, we assign a charging task to the agent + if (node->latest_time + segmentation_threshold > it->second.wait_until) + { + const auto charge_node = expand_charger( + node, it->second.candidate, initial_states, relative_start_time); + if (charge_node) + { + next_node = std::move(charge_node); + } + else + { + // agent has either full battery or insufficient charge to reach + // its charger. If later, we pop assigned task until + // we can make it to the charger + auto parent_node = std::make_shared(*node); + State state; + if (parent_node->assigned_tasks[it->second.candidate].empty()) + { + state = initial_states[it->second.candidate]; + } + else + { + state = parent_node->assigned_tasks[it->second.candidate].back().state(); + } + + if (state.battery_soc() < 0.99) + { + while (!parent_node->assigned_tasks[it->second.candidate].empty()) + { + auto& assignments = parent_node->assigned_tasks[it->second.candidate]; + assignments.pop_back(); + auto new_charge_node = expand_charger( + parent_node, it->second.candidate, initial_states, relative_start_time); + if (new_charge_node) + { + next_node = std::move(new_charge_node); + break; + } + } + } + } + } + } + } + } + + node = next_node; + assert(node); + } + + return node; + } + + std::vector expand( + ConstNodePtr parent, + Filter& filter, + const std::vector initial_states, + rmf_traffic::Time relative_start_time) + { + std::vector new_nodes; + new_nodes.reserve( + parent->unassigned_tasks.size() + parent->assigned_tasks.size()); + for (const auto& u : parent->unassigned_tasks) + { + const auto& range = u.second.candidates.best_candidates(); + for (auto it = range.begin; it!= range.end; it++) + { + if (auto new_node = expand_candidate(it, u, parent, &filter, relative_start_time)) + new_nodes.push_back(std::move(new_node)); + } + } + + // Assign charging task to each robot + for (std::size_t i = 0; i < parent->assigned_tasks.size(); ++i) + { + if (auto new_node = expand_charger(parent, i, initial_states, relative_start_time)) + new_nodes.push_back(new_node); + } + + return new_nodes; + } + + bool finished(const Node& node) + { + for (const auto& u : node.unassigned_tasks) + { + const auto range = u.second.candidates.best_candidates(); + for (auto it = range.begin; it!= range.end; ++it) + { + const auto wait_time = it->second.wait_until; + if (wait_time <= node.latest_time + segmentation_threshold) + return false; + } + } + + return true; + } + + ConstNodePtr solve( + ConstNodePtr initial_node, + const std::vector initial_states, + const std::size_t num_tasks, + rmf_traffic::Time relative_start_time, + std::function interrupter) + { + using PriorityQueue = std::priority_queue< + ConstNodePtr, + std::vector, + LowestCostEstimate>; + + PriorityQueue priority_queue; + priority_queue.push(std::move(initial_node)); + + Filter filter{config->filter_type(), num_tasks}; + ConstNodePtr top = nullptr; + + while (!priority_queue.empty() && !(interrupter && interrupter())) + { + top = priority_queue.top(); + + // Pop the top of the priority queue + priority_queue.pop(); + + // Check if unassigned tasks is empty -> solution found + if (finished(*top)) + { + return top; + } + + // Apply possible actions to expand the node + const auto new_nodes = expand(top, filter, initial_states, relative_start_time); + + // Add copies and with a newly assigned task to queue + for (const auto&n : new_nodes) + priority_queue.push(n); + + } + + return nullptr; + } + +}; + +TaskPlanner::TaskPlanner(std::shared_ptr config) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(config) + })) +{ + // Do nothing +} + +auto TaskPlanner::greedy_plan( + std::vector initial_states, + std::vector requests) -> Assignments +{ + return _pimpl->complete_solve( + initial_states, + requests, + nullptr, + true); +} +auto TaskPlanner::optimal_plan( + std::vector initial_states, + std::vector requests, + std::function interrupter) -> Assignments +{ + return _pimpl->complete_solve( + initial_states, + requests, + interrupter, + false); } +auto TaskPlanner::compute_cost(const Assignments& assignments) -> double +{ + return _pimpl->compute_g(assignments); +} + + + } // namespace agv } // namespace rmf_tasks diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index eeb43e3f2..e61fd1116 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -85,7 +85,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( { if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) { - std::cout << " -- Charge battery: Battery full" << std::endl; + // std::cout << " -- Charge battery: Battery full" << std::endl; return rmf_utils::nullopt; } @@ -129,7 +129,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( if (battery_soc <= state.threshold_soc()) { // If a robot cannot reach its charging dock given its initial battery soc - std::cout << " -- Charge battery: Unable to reach charger" << std::endl; + // std::cout << " -- Charge battery: Unable to reach charger" << std::endl; return rmf_utils::nullopt; } } diff --git a/rmf_tasks/test/main.cpp b/rmf_tasks/test/main.cpp new file mode 100644 index 000000000..23e837acb --- /dev/null +++ b/rmf_tasks/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_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp new file mode 100644 index 000000000..be10152f6 --- /dev/null +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -0,0 +1,342 @@ +/* + * 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +//============================================================================== +inline void display_solution( + std::string parent, + const rmf_tasks::agv::TaskPlanner::Assignments& assignments, + const double cost) +{ + std::cout << parent << " cost: " << cost << std::endl; + std::cout << parent << " assignments:" << std::endl; + for (std::size_t i = 0; i < assignments.size(); ++i) + { + std:: cout << "--Agent: " << i << std::endl; + for (const auto& a : assignments[i]) + { + const auto& s = a.state(); + std::cout << " <" << a.task_id() << ": " << 100* s.battery_soc() + << "%>" << std::endl; + } + } + std::cout << " ----------------------" << std::endl; +} + +//============================================================================== +SCENARIO("Grid World") +{ + const int grid_size = 4; + const double edge_length = 1000; + const bool drain_battery = true; + + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + + rmf_traffic::agv::Graph graph; + auto add_bidir_lane = [&](const std::size_t w0, const std::size_t w1) + { + graph.add_lane(w0, w1); + graph.add_lane(w1, w0); + }; + + const std::string map_name = "test_map"; + + for (int i = 0; i < grid_size; ++i) + { + for (int j = 0; j < grid_size; ++j) + { + // const auto random = (double) rand() / RAND_MAX; + const double random = 1.0; + graph.add_waypoint(map_name, + {j*edge_length*random, -i*edge_length*random}); + } + } + + for (int i = 0; i < grid_size*grid_size; ++i) + { + if ((i+1) % grid_size != 0) + add_bidir_lane(i, i+1); + if (i + grid_size < grid_size*grid_size) + add_bidir_lane(i, i+4); + } + + 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_options = rmf_traffic::agv::Planner::Options{ + nullptr}; + + auto planner = std::make_shared( + rmf_traffic::agv::Planner::Configuration{graph, traits}, + default_options); + + rmf_battery::agv::BatterySystem battery_system{24.0, 40.0, 8.8}; + REQUIRE(battery_system.valid()); + rmf_battery::agv::MechanicalSystem mechanical_system{70.0, 40.0, 0.22}; + REQUIRE(mechanical_system.valid()); + rmf_battery::agv::PowerSystem power_system{"processor", 20.0}; + REQUIRE(power_system.valid()); + + std::shared_ptr motion_sink = + std::make_shared(battery_system, mechanical_system); + std::shared_ptr device_sink = + std::make_shared(battery_system, power_system); + + auto charge_battery_task = rmf_tasks::requests::ChargeBattery::make( + battery_system, + motion_sink, device_sink, + planner, + drain_battery); + + WHEN("Planning for 3 requests and 2 agents") + { + const auto now = std::chrono::steady_clock::now(); + + std::vector initial_states = + { + rmf_tasks::agv::State{13, 13}, + rmf_tasks::agv::State{2, 2} + }; + + std::vector requests = + { + rmf_tasks::requests::Delivery::make( + 1, + 0, + 3, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)), + + rmf_tasks::requests::Delivery::make( + 2, + 15, + 2, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)), + + rmf_tasks::requests::Delivery::make( + 3, + 7, + 9, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)) + }; + + std::shared_ptr task_config = + std::make_shared(charge_battery_task); + rmf_tasks::agv::TaskPlanner task_planner(task_config); + + const auto greedy_assignments = task_planner.greedy_plan( + initial_states, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); + + const auto optimal_assignments = task_planner.optimal_plan( + initial_states, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); + + display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Optimal", optimal_assignments, optimal_cost); + + REQUIRE(optimal_cost <= greedy_cost); + } + + WHEN("Planning for 11 requests and 2 agents") + { + const auto now = std::chrono::steady_clock::now(); + + std::vector initial_states = + { + rmf_tasks::agv::State{13, 13}, + rmf_tasks::agv::State{2, 2} + }; + + std::vector requests = + { + rmf_tasks::requests::Delivery::make( + 1, + 0, + 3, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)), + + rmf_tasks::requests::Delivery::make( + 2, + 15, + 2, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)), + + rmf_tasks::requests::Delivery::make( + 3, + 7, + 9, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)), + + rmf_tasks::requests::Delivery::make( + 3, + 7, + 9, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(0)), + + rmf_tasks::requests::Delivery::make( + 4, + 8, + 11, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(50000)), + + rmf_tasks::requests::Delivery::make( + 5, + 10, + 0, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(50000)), + + rmf_tasks::requests::Delivery::make( + 6, + 4, + 8, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(60000)), + + rmf_tasks::requests::Delivery::make( + 7, + 8, + 14, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(60000)), + + rmf_tasks::requests::Delivery::make( + 8, + 5, + 11, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(60000)), + + rmf_tasks::requests::Delivery::make( + 9, + 9, + 0, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(60000)), + + rmf_tasks::requests::Delivery::make( + 10, + 1, + 3, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(60000)), + + rmf_tasks::requests::Delivery::make( + 11, + 0, + 12, + motion_sink, + device_sink, + planner, + drain_battery, + now + rmf_traffic::time::from_seconds(60000)) + }; + + std::shared_ptr task_config = + std::make_shared(charge_battery_task); + rmf_tasks::agv::TaskPlanner task_planner(task_config); + + const auto greedy_assignments = task_planner.greedy_plan( + initial_states, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); + + const auto optimal_assignments = task_planner.optimal_plan( + initial_states, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); + + display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Optimal", optimal_assignments, optimal_cost); + + REQUIRE(optimal_cost <= greedy_cost); + } +} \ No newline at end of file From 5f4aab83505881860d1ad3741312c871482eef89 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 29 Sep 2020 17:44:10 +0800 Subject: [PATCH 040/128] Moved threshold_soc into StateConfig class --- rmf_tasks/include/rmf_tasks/Request.hpp | 4 +- rmf_tasks/include/rmf_tasks/agv/State.hpp | 7 +- .../include/rmf_tasks/agv/StateConfig.hpp | 47 +++++++++++ .../include/rmf_tasks/agv/TaskPlanner.hpp | 4 + .../rmf_tasks/requests/ChargeBattery.hpp | 3 +- .../include/rmf_tasks/requests/Delivery.hpp | 3 +- rmf_tasks/src/agv/State.cpp | 28 ++----- rmf_tasks/src/agv/StateConfig.cpp | 53 ++++++++++++ rmf_tasks/src/agv/TaskPlanner.cpp | 83 +++++++++++++------ rmf_tasks/src/requests/ChargeBattery.cpp | 8 +- rmf_tasks/src/requests/Delivery.cpp | 14 ++-- rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 22 ++++- 12 files changed, 203 insertions(+), 73 deletions(-) create mode 100644 rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp create mode 100644 rmf_tasks/src/agv/StateConfig.cpp diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index 0773b69cd..83fbc915e 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -41,7 +42,8 @@ class Request // Estimate the state of the robot when the task is finished along with the // time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( - const agv::State& initial_state) const = 0; + const agv::State& initial_state, + const agv::StateConfig& state_config) const = 0; // Estimate the invariant component of the task's duration virtual rmf_traffic::Duration invariant_duration() const = 0; diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index dc93be0ab..bd569874b 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -43,8 +43,7 @@ class State std::size_t waypoint, std::size_t charging_waypoint, rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), - double battery_soc = 1.0, - double threshold_soc = 0.2); + double battery_soc = 1.0); State(); @@ -64,10 +63,6 @@ class State State& battery_soc(double new_battery_soc); - double threshold_soc() const; - - State& threshold_soc(double new_threshold_soc); - class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp new file mode 100644 index 000000000..87274040f --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp @@ -0,0 +1,47 @@ +/* + * 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 INCLUDE__RMF_TASKS__AGV__STATECONFIG_HPP +#define INCLUDE__RMF_TASKS__AGV__STATECONFIG_HPP + +#include + +namespace rmf_tasks { +namespace agv { + +class StateConfig +{ +public: + + /// Constructor + /// + /// \param[in] threshold_soc + StateConfig(double threshold_soc); + + double threshold_soc() const; + + StateConfig& threshold_soc(double threshold_soc); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__AGV__STATE_HPP diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp index 0b038a6cf..731258b8c 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -20,6 +20,7 @@ #include #include +#include #include @@ -114,6 +115,7 @@ class TaskPlanner rmf_utils::impl_ptr _pimpl; }; + /// Container for assignments for each agent using Assignments = std::vector>; /// Constructor @@ -126,6 +128,7 @@ class TaskPlanner /// requests Assignments greedy_plan( std::vector initial_states, + std::vector state_configs, std::vector requests); /// Get the optimal planner based assignments for a set of initial states and @@ -136,6 +139,7 @@ class TaskPlanner /// If a bid is awarded, the optimal solution may be used for assignments. Assignments optimal_plan( std::vector initial_states, + std::vector state_configs, std::vector requests, std::function interrupter); diff --git a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp index 478003a02..4bfcef0d6 100644 --- a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp @@ -48,7 +48,8 @@ class ChargeBattery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( - const agv::State& initial_state) const final; + const agv::State& initial_state, + const agv::StateConfig& state_config) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index 07bd5b813..16a2e8937 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -52,7 +52,8 @@ class Delivery : public rmf_tasks::Request std::size_t id() const final; rmf_utils::optional estimate_finish( - const agv::State& initial_state) const final; + const agv::State& initial_state, + const agv::StateConfig& state_config) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index 7b4b6da39..accbd3112 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -29,20 +29,17 @@ class State::Implementation std::size_t waypoint, std::size_t charging_waypoint, rmf_traffic::Time finish_time, - double battery_soc, - double threshold_soc) + double battery_soc) : _waypoint(waypoint), _charging_waypoint(charging_waypoint), _finish_time(finish_time), - _battery_soc(battery_soc), - _threshold_soc(threshold_soc) + _battery_soc(battery_soc) {} std::size_t _waypoint; std::size_t _charging_waypoint; rmf_traffic::Time _finish_time; double _battery_soc; - double _threshold_soc; }; //============================================================================== @@ -50,17 +47,16 @@ State::State( std::size_t waypoint, std::size_t charging_waypoint, rmf_traffic::Time finish_time, - double battery_soc, - double threshold_soc) + double battery_soc) : _pimpl(rmf_utils::make_impl( Implementation( - waypoint, charging_waypoint, finish_time, battery_soc, threshold_soc))) + waypoint, charging_waypoint, finish_time, battery_soc))) {} //============================================================================== State::State() : _pimpl(rmf_utils::make_impl( - Implementation(0, 0, std::chrono::steady_clock::now(), 0.0, 0.0))) + Implementation(0, 0, std::chrono::steady_clock::now(), 0.0))) {} //============================================================================== @@ -115,19 +111,5 @@ State& State::battery_soc(double new_battery_soc) return *this; } -//============================================================================== -double State::threshold_soc() const -{ - return _pimpl->_threshold_soc; -} - -//============================================================================== -State& State::threshold_soc(double new_threshold_soc) -{ - _pimpl->_threshold_soc = new_threshold_soc; - return *this; -} - -//============================================================================== } // namespace agv } // namespace rmf_tasks diff --git a/rmf_tasks/src/agv/StateConfig.cpp b/rmf_tasks/src/agv/StateConfig.cpp new file mode 100644 index 000000000..d5593cb72 --- /dev/null +++ b/rmf_tasks/src/agv/StateConfig.cpp @@ -0,0 +1,53 @@ +/* + * 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 + +namespace rmf_tasks { +namespace agv { + +class StateConfig::Implementation +{ +public: + double threshold_soc; + +}; + +StateConfig::StateConfig(double threshold_soc) +: _pimpl(rmf_utils::make_impl( + Implementation + { + threshold_soc + })) +{ + // Do nothing +} + +double StateConfig::threshold_soc() const +{ + return _pimpl->threshold_soc; +} + +auto StateConfig::threshold_soc(double threshold_soc) -> StateConfig& +{ + _pimpl->threshold_soc = threshold_soc; + return *this; +} + + +} // namespace agv +} // namespace rmf_tasks \ No newline at end of file diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 206919df6..4bf287ea3 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -163,6 +163,7 @@ class Candidates static Candidates make( const std::vector& initial_states, + const std::vector& state_configs, const rmf_tasks::Request& request); Candidates(const Candidates& other) @@ -248,13 +249,15 @@ class Candidates Candidates Candidates::make( const std::vector& initial_states, + const std::vector& state_configs, const rmf_tasks::Request& request) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) { const auto& state = initial_states[i]; - const auto finish = request.estimate_finish(state); + const auto& state_config = state_configs[i]; + const auto finish = request.estimate_finish(state, state_config); if (finish.has_value()) { initial_map.insert( @@ -272,10 +275,11 @@ Candidates Candidates::make( struct PendingTask { PendingTask( - std::vector initial_states, + std::vector& initial_states, + std::vector& state_configs, rmf_tasks::Request::SharedPtr request_) : request(std::move(request_)), - candidates(Candidates::make(initial_states, *request)), + candidates(Candidates::make(initial_states, state_configs, *request)), earliest_start_time(request->earliest_start_time()) { // Do nothing @@ -569,14 +573,16 @@ class TaskPlanner::Implementation } Assignments complete_solve( - std::vector initial_states, - std::vector requests, - std::function interrupter, + std::vector& initial_states, + const std::vector& state_configs, + const std::vector& requests, + const std::function interrupter, bool greedy) - { + assert(initial_states.size() == state_configs.size()); + const rmf_traffic::Time start_time = std::chrono::steady_clock::now(); - auto node = make_initial_node(initial_states, requests, start_time); + auto node = make_initial_node(initial_states, state_configs, requests, start_time); Node::AssignedTasks complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); @@ -584,9 +590,9 @@ class TaskPlanner::Implementation while (node) { if (greedy) - node = greedy_solve(node, initial_states, start_time); + node = greedy_solve(node, initial_states, state_configs, start_time); else - node = solve(node, initial_states, requests.size(), start_time, interrupter); + node = solve(node, initial_states, state_configs, requests.size(), start_time, interrupter); if (!node) { @@ -632,7 +638,7 @@ class TaskPlanner::Implementation estimates[i] = assignments.back().state(); } - node = make_initial_node(estimates, new_tasks, start_time); + node = make_initial_node(estimates, state_configs, new_tasks, start_time); initial_states = estimates; } @@ -705,6 +711,7 @@ class TaskPlanner::Implementation ConstNodePtr make_initial_node( std::vector initial_states, + std::vector state_configs, std::vector requests, rmf_traffic::Time relative_start_time) { @@ -714,7 +721,7 @@ class TaskPlanner::Implementation for (const auto& request : requests) initial_node->unassigned_tasks.insert( - {request->id(), PendingTask(initial_states, request)}); + {request->id(), PendingTask(initial_states, state_configs, request)}); initial_node->cost_estimate = compute_f(*initial_node, relative_start_time); @@ -772,7 +779,8 @@ class TaskPlanner::Implementation const Node::UnassignedTasks::value_type& u, const ConstNodePtr& parent, Filter* filter, - rmf_traffic::Time relative_start_time) + rmf_traffic::Time relative_start_time, + const std::vector& state_configs) { const auto& entry = it->second; @@ -797,7 +805,8 @@ class TaskPlanner::Implementation for (auto& new_u : new_node->unassigned_tasks) { const auto finish = - new_u.second.request->estimate_finish(entry.state); + new_u.second.request->estimate_finish( + entry.state, state_configs[entry.candidate]); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -828,7 +837,8 @@ class TaskPlanner::Implementation ConstNodePtr expand_charger( ConstNodePtr parent, const std::size_t agent, - const std::vector initial_states, + const std::vector& initial_states, + const std::vector& state_configs, rmf_traffic::Time relative_start_time) { auto new_node = std::make_shared(*parent); @@ -845,7 +855,8 @@ class TaskPlanner::Implementation state = initial_states[agent]; } - auto estimate = config->charge_battery_request()->estimate_finish(state); + auto estimate = config->charge_battery_request()->estimate_finish( + state, state_configs[agent]); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( @@ -857,7 +868,8 @@ class TaskPlanner::Implementation for (auto& new_u : new_node->unassigned_tasks) { const auto finish = - new_u.second.request->estimate_finish(estimate.value().finish_state()); + new_u.second.request->estimate_finish( + estimate.value().finish_state(), state_configs[agent]); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -879,7 +891,8 @@ class TaskPlanner::Implementation ConstNodePtr greedy_solve( ConstNodePtr node, - const std::vector initial_states, + const std::vector& initial_states, + const std::vector& state_configs, rmf_traffic::Time relative_start_time) { while (!finished(*node)) @@ -890,7 +903,8 @@ class TaskPlanner::Implementation const auto& range = u.second.candidates.best_candidates(); for (auto it = range.begin; it != range.end; ++it) { - if (auto n = expand_candidate(it, u, node, nullptr, relative_start_time)) + if (auto n = expand_candidate( + it, u, node, nullptr, relative_start_time, state_configs)) { if (!next_node || (n->cost_estimate < next_node->cost_estimate)) { @@ -905,7 +919,11 @@ class TaskPlanner::Implementation if (node->latest_time + segmentation_threshold > it->second.wait_until) { const auto charge_node = expand_charger( - node, it->second.candidate, initial_states, relative_start_time); + node, + it->second.candidate, + initial_states, + state_configs, + relative_start_time); if (charge_node) { next_node = std::move(charge_node); @@ -933,7 +951,11 @@ class TaskPlanner::Implementation auto& assignments = parent_node->assigned_tasks[it->second.candidate]; assignments.pop_back(); auto new_charge_node = expand_charger( - parent_node, it->second.candidate, initial_states, relative_start_time); + parent_node, + it->second.candidate, + initial_states, + state_configs, + relative_start_time); if (new_charge_node) { next_node = std::move(new_charge_node); @@ -957,7 +979,8 @@ class TaskPlanner::Implementation std::vector expand( ConstNodePtr parent, Filter& filter, - const std::vector initial_states, + const std::vector& initial_states, + const std::vector& state_configs, rmf_traffic::Time relative_start_time) { std::vector new_nodes; @@ -968,7 +991,8 @@ class TaskPlanner::Implementation const auto& range = u.second.candidates.best_candidates(); for (auto it = range.begin; it!= range.end; it++) { - if (auto new_node = expand_candidate(it, u, parent, &filter, relative_start_time)) + if (auto new_node = expand_candidate( + it, u, parent, &filter, relative_start_time, state_configs)) new_nodes.push_back(std::move(new_node)); } } @@ -976,7 +1000,8 @@ class TaskPlanner::Implementation // Assign charging task to each robot for (std::size_t i = 0; i < parent->assigned_tasks.size(); ++i) { - if (auto new_node = expand_charger(parent, i, initial_states, relative_start_time)) + if (auto new_node = expand_charger( + parent, i, initial_states, state_configs, relative_start_time)) new_nodes.push_back(new_node); } @@ -1001,7 +1026,8 @@ class TaskPlanner::Implementation ConstNodePtr solve( ConstNodePtr initial_node, - const std::vector initial_states, + const std::vector& initial_states, + const std::vector& state_configs, const std::size_t num_tasks, rmf_traffic::Time relative_start_time, std::function interrupter) @@ -1031,7 +1057,8 @@ class TaskPlanner::Implementation } // Apply possible actions to expand the node - const auto new_nodes = expand(top, filter, initial_states, relative_start_time); + const auto new_nodes = expand( + top, filter, initial_states, state_configs, relative_start_time); // Add copies and with a newly assigned task to queue for (const auto&n : new_nodes) @@ -1055,10 +1082,12 @@ TaskPlanner::TaskPlanner(std::shared_ptr config) auto TaskPlanner::greedy_plan( std::vector initial_states, + std::vector state_configs, std::vector requests) -> Assignments { return _pimpl->complete_solve( initial_states, + state_configs, requests, nullptr, true); @@ -1066,11 +1095,13 @@ auto TaskPlanner::greedy_plan( auto TaskPlanner::optimal_plan( std::vector initial_states, + std::vector state_configs, std::vector requests, std::function interrupter) -> Assignments { return _pimpl->complete_solve( initial_states, + state_configs, requests, interrupter, false); diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index e61fd1116..a794dc8af 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -81,7 +81,8 @@ std::size_t ChargeBattery::id() const //============================================================================== rmf_utils::optional ChargeBattery::estimate_finish( - const agv::State& initial_state) const + const agv::State& initial_state, + const agv::StateConfig& state_config) const { if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) { @@ -94,8 +95,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( initial_state.charging_waypoint(), initial_state.charging_waypoint(), initial_state.finish_time(), - initial_state.battery_soc(), - initial_state.threshold_soc()); + initial_state.battery_soc()); const auto start_time = initial_state.finish_time(); @@ -126,7 +126,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( battery_soc = battery_soc - dSOC_motion - dSOC_device; } - if (battery_soc <= state.threshold_soc()) + if (battery_soc <= state_config.threshold_soc()) { // If a robot cannot reach its charging dock given its initial battery soc // std::cout << " -- Charge battery: Unable to reach charger" << std::endl; diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 487392fb1..2bc9aac43 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -108,14 +108,14 @@ std::size_t Delivery::id() const //============================================================================== rmf_utils::optional Delivery::estimate_finish( - const agv::State& initial_state) const + const agv::State& initial_state, + const agv::StateConfig& state_config) const { agv::State state( _pimpl->_dropoff_waypoint, initial_state.charging_waypoint(), initial_state.finish_time(), - initial_state.battery_soc(), - initial_state.threshold_soc()); + initial_state.battery_soc()); rmf_traffic::Duration variant_duration(0); @@ -151,9 +151,9 @@ rmf_utils::optional Delivery::estimate_finish( battery_soc = battery_soc - dSOC_motion - dSOC_device; } - if (battery_soc <= state.threshold_soc()) + if (battery_soc <= state_config.threshold_soc()) { - std::cout << " -- Delivery: Unable to reach pickup" << std::endl; + // std::cout << " -- Delivery: Unable to reach pickup" << std::endl; return rmf_utils::nullopt; } @@ -171,9 +171,9 @@ rmf_utils::optional Delivery::estimate_finish( battery_soc -= _pimpl->_invariant_battery_drain; - if (battery_soc <= state.threshold_soc()) + if (battery_soc <= state_config.threshold_soc()) { - std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; + // std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; return rmf_utils::nullopt; } diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index be10152f6..5a01757ff 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -17,6 +17,8 @@ #include +#include +#include #include #include @@ -138,6 +140,12 @@ SCENARIO("Grid World") rmf_tasks::agv::State{2, 2} }; + std::vector state_configs = + { + rmf_tasks::agv::StateConfig{0.2}, + rmf_tasks::agv::StateConfig{0.2} + }; + std::vector requests = { rmf_tasks::requests::Delivery::make( @@ -176,11 +184,11 @@ SCENARIO("Grid World") rmf_tasks::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( - initial_states, requests); + initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); const auto optimal_assignments = task_planner.optimal_plan( - initial_states, requests, nullptr); + initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); display_solution("Greedy", greedy_assignments, greedy_cost); @@ -199,6 +207,12 @@ SCENARIO("Grid World") rmf_tasks::agv::State{2, 2} }; + std::vector state_configs = + { + rmf_tasks::agv::StateConfig{0.2}, + rmf_tasks::agv::StateConfig{0.2} + }; + std::vector requests = { rmf_tasks::requests::Delivery::make( @@ -327,11 +341,11 @@ SCENARIO("Grid World") rmf_tasks::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( - initial_states, requests); + initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); const auto optimal_assignments = task_planner.optimal_plan( - initial_states, requests, nullptr); + initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); display_solution("Greedy", greedy_assignments, greedy_cost); From 510525b682e8737f31bfefe926e6a99cc89301e4 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 29 Sep 2020 17:48:47 +0800 Subject: [PATCH 041/128] Fixed UB in pop_unassigned --- rmf_tasks/src/agv/TaskPlanner.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 4bf287ea3..de0cc555d 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -322,16 +322,18 @@ struct Node unassigned_tasks.erase(task_id); bool popped_invariant = false; + InvariantSet::iterator erase_it; for (auto it = unassigned_invariants.begin(); it != unassigned_invariants.end(); ++it) { if (it->task_id == task_id) { popped_invariant = true; - unassigned_invariants.erase(it); + erase_it = it; + break; } } - + unassigned_invariants.erase(erase_it); assert(popped_invariant); } }; From 8ff32e8086e8e76ccc10ad8b0ad5a2047f379194 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 3 Oct 2020 13:14:38 +0800 Subject: [PATCH 042/128] Fixed UB in Candidates::make() --- rmf_tasks/src/agv/TaskPlanner.cpp | 50 ++++++++++++++++++++++++------- 1 file changed, 39 insertions(+), 11 deletions(-) diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index de0cc555d..1ac3257ae 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -29,6 +29,7 @@ #include #include #include +#include namespace rmf_tasks { namespace agv { @@ -164,7 +165,8 @@ class Candidates static Candidates make( const std::vector& initial_states, const std::vector& state_configs, - const rmf_tasks::Request& request); + const rmf_tasks::Request& request, + const rmf_tasks::Request& charge_battery_request); Candidates(const Candidates& other) { @@ -250,7 +252,8 @@ class Candidates Candidates Candidates::make( const std::vector& initial_states, const std::vector& state_configs, - const rmf_tasks::Request& request) + const rmf_tasks::Request& request, + const rmf_tasks::Request& charge_battery_request) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) @@ -260,12 +263,31 @@ Candidates Candidates::make( const auto finish = request.estimate_finish(state, state_config); if (finish.has_value()) { - initial_map.insert( - { - finish.value().finish_state().finish_time(), - Entry{i, finish.value().finish_state(), finish.value().wait_until()} - }); + initial_map.insert({ + finish.value().finish_state().finish_time(), + Entry{i, finish.value().finish_state(), finish.value().wait_until()}}); } + else + { + auto battery_estimate = + charge_battery_request.estimate_finish(state, state_config); + if (battery_estimate.has_value()) + { + auto new_finish = request.estimate_finish( + battery_estimate.value().finish_state(), state_config); + assert(new_finish.has_value()); + initial_map.insert( + {new_finish.value().finish_state().finish_time(), + Entry{i, new_finish.value().finish_state(), new_finish.value().wait_until()}}); + } + else + { + std::cerr << "Unable to create entry for candidate [" << i + << "] and request [" << request.id() << " ]" << std::endl; + assert(false); + } + } + } return Candidates(std::move(initial_map)); @@ -277,9 +299,11 @@ struct PendingTask PendingTask( std::vector& initial_states, std::vector& state_configs, - rmf_tasks::Request::SharedPtr request_) + rmf_tasks::Request::SharedPtr request_, + rmf_tasks::Request::SharedPtr charge_battery_request) : request(std::move(request_)), - candidates(Candidates::make(initial_states, state_configs, *request)), + candidates(Candidates::make( + initial_states, state_configs, *request, *charge_battery_request)), earliest_start_time(request->earliest_start_time()) { // Do nothing @@ -723,7 +747,10 @@ class TaskPlanner::Implementation for (const auto& request : requests) initial_node->unassigned_tasks.insert( - {request->id(), PendingTask(initial_states, state_configs, request)}); + { + request->id(), + PendingTask(initial_states, state_configs, request, config->charge_battery_request()) + }); initial_node->cost_estimate = compute_f(*initial_node, relative_start_time); @@ -804,11 +831,12 @@ class TaskPlanner::Implementation new_node->pop_unassigned(u.first); // Update states of unassigned tasks for the candidate + const auto& state_config = state_configs[entry.candidate]; for (auto& new_u : new_node->unassigned_tasks) { const auto finish = new_u.second.request->estimate_finish( - entry.state, state_configs[entry.candidate]); + entry.state, state_config); if (finish.has_value()) { new_u.second.candidates.update_candidate( From 24a03a89c458266151fff3a8f9a51a220b0d51f2 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sun, 4 Oct 2020 18:10:32 +0800 Subject: [PATCH 043/128] Updated expand_candidate and implemented correct_assingments() to add missing charging tasks --- rmf_tasks/src/agv/TaskPlanner.cpp | 80 +++++++++++++++++++++++++++---- 1 file changed, 70 insertions(+), 10 deletions(-) diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 1ac3257ae..8d05df11d 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -597,6 +597,48 @@ class TaskPlanner::Implementation return cost; } + + Assignments correct_assignments( + Assignments& assignments, + const std::vector& state_configs) + { + for (std::size_t a = 0; a < assignments.size(); ++a) + { + if (assignments[a].empty()) + continue; + + // Remove charging task at end of assignments if any + if (assignments[a].back().task_id() == config->charge_battery_request()->id()) + assignments[a].pop_back(); + + // Insert missing charging tasks if any + if (assignments[a].size() > 1) + { + auto it = ++assignments[a].begin(); + for (; it != assignments[a].end(); ++it) + { + auto prev_it = it; --prev_it; + if (it->state().battery_soc() > prev_it->state().battery_soc() && + it->task_id() != config->charge_battery_request()->id()) + { + auto estimate = config->charge_battery_request()->estimate_finish( + prev_it->state(), state_configs[a]); + assert(estimate.has_value()); + assignments[a].insert( + it, + Assignment + { + config->charge_battery_request()->id(), + estimate.value().finish_state(), + estimate.value().wait_until() + }); + } + } + } + } + + return assignments; + } Assignments complete_solve( std::vector& initial_states, @@ -639,7 +681,7 @@ class TaskPlanner::Implementation } if (node->unassigned_tasks.empty()) - return complete_assignments; + return correct_assignments(complete_assignments, state_configs); // std::unordered_map new_task_id_map; std::vector new_tasks; @@ -846,7 +888,27 @@ class TaskPlanner::Implementation } else { - return nullptr; + // return nullptr; + + auto battery_estimate = + config->charge_battery_request()->estimate_finish(entry.state, state_config); + if (battery_estimate.has_value()) + { + auto new_finish = + new_u.second.request->estimate_finish( + battery_estimate.value().finish_state(), + state_config); + assert(new_finish.has_value()); + new_u.second.candidates.update_candidate( + entry.candidate, + new_finish.value().finish_state(), + new_finish.value().wait_until()); + } + else + { + // unable to reach charger + return nullptr; + } } } @@ -872,18 +934,16 @@ class TaskPlanner::Implementation rmf_traffic::Time relative_start_time) { auto new_node = std::make_shared(*parent); - // Assign charging task to an agent - const auto& assignments = new_node->assigned_tasks[agent]; - State state; + // Assign charging task to an agent + State state = initial_states[agent]; + auto& assignments = new_node->assigned_tasks[agent]; + if (!assignments.empty()) { + if (assignments.back().task_id() == config->charge_battery_request()->id()) + return nullptr; state = assignments.back().state(); } - else - { - // We use the initial state of the robot - state = initial_states[agent]; - } auto estimate = config->charge_battery_request()->estimate_finish( state, state_configs[agent]); From 5a81c8f86604d97ea7007ebf5a3c964f77bd7b3d Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 6 Oct 2020 14:12:41 +0800 Subject: [PATCH 044/128] Fixed UB in greedy_solve() --- rmf_tasks/src/agv/TaskPlanner.cpp | 188 ++++++++++--------- rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 2 +- 2 files changed, 101 insertions(+), 89 deletions(-) diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 8d05df11d..37624a943 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -598,7 +598,7 @@ class TaskPlanner::Implementation return cost; } - Assignments correct_assignments( + Assignments prune_assignments( Assignments& assignments, const std::vector& state_configs) { @@ -612,29 +612,29 @@ class TaskPlanner::Implementation assignments[a].pop_back(); // Insert missing charging tasks if any - if (assignments[a].size() > 1) - { - auto it = ++assignments[a].begin(); - for (; it != assignments[a].end(); ++it) - { - auto prev_it = it; --prev_it; - if (it->state().battery_soc() > prev_it->state().battery_soc() && - it->task_id() != config->charge_battery_request()->id()) - { - auto estimate = config->charge_battery_request()->estimate_finish( - prev_it->state(), state_configs[a]); - assert(estimate.has_value()); - assignments[a].insert( - it, - Assignment - { - config->charge_battery_request()->id(), - estimate.value().finish_state(), - estimate.value().wait_until() - }); - } - } - } + // if (assignments[a].size() > 1) + // { + // auto it = ++assignments[a].begin(); + // for (; it != assignments[a].end(); ++it) + // { + // auto prev_it = it; --prev_it; + // if (it->state().battery_soc() > prev_it->state().battery_soc() && + // it->task_id() != config->charge_battery_request()->id()) + // { + // auto estimate = config->charge_battery_request()->estimate_finish( + // prev_it->state(), state_configs[a]); + // assert(estimate.has_value()); + // assignments[a].insert( + // it, + // Assignment + // { + // config->charge_battery_request()->id(), + // estimate.value().finish_state(), + // estimate.value().wait_until() + // }); + // } + // } + // } } return assignments; @@ -681,7 +681,8 @@ class TaskPlanner::Implementation } if (node->unassigned_tasks.empty()) - return correct_assignments(complete_assignments, state_configs); + return prune_assignments(complete_assignments, state_configs); + // return complete_assignments; // std::unordered_map new_task_id_map; std::vector new_tasks; @@ -874,11 +875,13 @@ class TaskPlanner::Implementation // Update states of unassigned tasks for the candidate const auto& state_config = state_configs[entry.candidate]; + bool add_charger = false; for (auto& new_u : new_node->unassigned_tasks) { const auto finish = new_u.second.request->estimate_finish( entry.state, state_config); + if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -888,27 +891,65 @@ class TaskPlanner::Implementation } else { - // return nullptr; + // TODO(YV): Revisit this strategy + // auto battery_estimate = + // config->charge_battery_request()->estimate_finish(entry.state, state_config); + // if (battery_estimate.has_value()) + // { + // auto new_finish = + // new_u.second.request->estimate_finish( + // battery_estimate.value().finish_state(), + // state_config); + // assert(new_finish.has_value()); + // new_u.second.candidates.update_candidate( + // entry.candidate, + // new_finish.value().finish_state(), + // new_finish.value().wait_until()); + // } + // else + // { + // // Unable to reach charger + // return nullptr; + // } + + add_charger = true; + break; + } + } - auto battery_estimate = - config->charge_battery_request()->estimate_finish(entry.state, state_config); - if (battery_estimate.has_value()) - { - auto new_finish = - new_u.second.request->estimate_finish( - battery_estimate.value().finish_state(), - state_config); - assert(new_finish.has_value()); - new_u.second.candidates.update_candidate( - entry.candidate, - new_finish.value().finish_state(), - new_finish.value().wait_until()); - } - else + if (add_charger) + { + auto battery_estimate = config->charge_battery_request()->estimate_finish(entry.state, state_config); + if (battery_estimate.has_value()) + { + new_node->assigned_tasks[entry.candidate].push_back( + Assignment + { + config->charge_battery_request()->id(), + battery_estimate.value().finish_state(), + battery_estimate.value().wait_until() + }); + for (auto& new_u : new_node->unassigned_tasks) { - // unable to reach charger - return nullptr; + const auto finish = + new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), state_config); + if (finish.has_value()) + { + new_u.second.candidates.update_candidate( + entry.candidate, finish.value().finish_state(), finish.value().wait_until()); + } + else + { + // We should stop expanding this node + return nullptr; + } } + + } + else + { + // Agent cannot make it back to the charger + return nullptr; } } @@ -1004,56 +1045,27 @@ class TaskPlanner::Implementation else { // expand_candidate returned nullptr either due to start time - // segmentation or insufficient charge to complete task. For the - // later case, we assign a charging task to the agent + // segmentation or insufficient charge to return to its charger. + // For the later case, we aim to backtrack and assign a charging + // task to the agent. if (node->latest_time + segmentation_threshold > it->second.wait_until) { - const auto charge_node = expand_charger( - node, - it->second.candidate, - initial_states, - state_configs, - relative_start_time); - if (charge_node) + auto parent_node = std::make_shared(*node); + while (!parent_node->assigned_tasks[it->second.candidate].empty()) { - next_node = std::move(charge_node); - } - else - { - // agent has either full battery or insufficient charge to reach - // its charger. If later, we pop assigned task until - // we can make it to the charger - auto parent_node = std::make_shared(*node); - State state; - if (parent_node->assigned_tasks[it->second.candidate].empty()) + parent_node->assigned_tasks[it->second.candidate].pop_back(); + auto new_charge_node = expand_charger( + parent_node, + it->second.candidate, + initial_states, + state_configs, + relative_start_time); + if (new_charge_node) { - state = initial_states[it->second.candidate]; + next_node = std::move(new_charge_node); + break; } - else - { - state = parent_node->assigned_tasks[it->second.candidate].back().state(); - } - - if (state.battery_soc() < 0.99) - { - while (!parent_node->assigned_tasks[it->second.candidate].empty()) - { - auto& assignments = parent_node->assigned_tasks[it->second.candidate]; - assignments.pop_back(); - auto new_charge_node = expand_charger( - parent_node, - it->second.candidate, - initial_states, - state_configs, - relative_start_time); - if (new_charge_node) - { - next_node = std::move(new_charge_node); - break; - } - } - } - } + } } } } diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index 5a01757ff..4dcf30e99 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -347,7 +347,7 @@ SCENARIO("Grid World") const auto optimal_assignments = task_planner.optimal_plan( initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - + display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost); From cf7562e4438469360320e5d7e2a21b361cc0de12 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 6 Oct 2020 16:59:32 +0800 Subject: [PATCH 045/128] estimate_finish() of Delivery checks if robot can return to its charger after reaching dropoff waypoint --- rmf_tasks/src/requests/Delivery.cpp | 51 +++++++++++++++----- rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 5 +- 2 files changed, 42 insertions(+), 14 deletions(-) diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 2bc9aac43..dffff1447 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -89,7 +89,7 @@ rmf_tasks::Request::SharedPtr Delivery::make( const double dSOC_device = delivery->_pimpl->_device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); - delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; } return delivery; @@ -119,7 +119,7 @@ rmf_utils::optional Delivery::estimate_finish( rmf_traffic::Duration variant_duration(0); - rmf_traffic::Time start_time = initial_state.finish_time(); + 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; @@ -156,8 +156,6 @@ rmf_utils::optional Delivery::estimate_finish( // std::cout << " -- Delivery: Unable to reach pickup" << std::endl; return rmf_utils::nullopt; } - - start_time = finish_time; } const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; @@ -169,17 +167,44 @@ rmf_utils::optional Delivery::estimate_finish( state.finish_time( wait_until + variant_duration + _pimpl->_invariant_duration); - battery_soc -= _pimpl->_invariant_battery_drain; - - if (battery_soc <= state_config.threshold_soc()) + if (_pimpl->_drain_battery) { - // std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; - return rmf_utils::nullopt; - } - - state.battery_soc(battery_soc); + battery_soc -= _pimpl->_invariant_battery_drain; + if (battery_soc <= state_config.threshold_soc()) + { + // std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; + return rmf_utils::nullopt; + } - // TODO: Check if we have enough charge to head back to nearest charger + // Check if we have enough charge to head back to nearest charger + double retreat_battery_drain = 0.0; + if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) + { + rmf_traffic::agv::Planner::Start start{ + state.finish_time(), + _pimpl->_dropoff_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{state.charging_waypoint()}; + + const auto result_to_charger = _pimpl->_planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = finish_time - state.finish_time(); + + dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_device; + } + + if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + return rmf_utils::nullopt; + + state.battery_soc(battery_soc); + } return Estimate(state, wait_until); } diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index 4dcf30e99..01f377307 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -54,7 +54,10 @@ inline void display_solution( for (const auto& a : assignments[i]) { const auto& s = a.state(); - std::cout << " <" << a.task_id() << ": " << 100* s.battery_soc() + const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9; + const double finish_seconds = s.finish_time().time_since_epoch().count()/1e9; + std::cout << " <" << a.task_id() << ": " << start_seconds + << ", "<< finish_seconds << ", " << 100* s.battery_soc() << "%>" << std::endl; } } From 1d01e35b119a80ce29d8886b84afba374b7d7dc6 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 6 Oct 2020 18:38:31 +0800 Subject: [PATCH 046/128] Added Clean request --- .../include/rmf_tasks/requests/Clean.hpp | 74 ++++++ rmf_tasks/src/requests/Clean.cpp | 229 ++++++++++++++++++ rmf_tasks/src/requests/Delivery.cpp | 5 +- 3 files changed, 306 insertions(+), 2 deletions(-) create mode 100644 rmf_tasks/include/rmf_tasks/requests/Clean.hpp create mode 100644 rmf_tasks/src/requests/Clean.cpp diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp new file mode 100644 index 000000000..759fbe335 --- /dev/null +++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp @@ -0,0 +1,74 @@ +/* + * 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 INCLUDE__RMF_TASKS__REQUESTS__CLEAN_HPP +#define INCLUDE__RMF_TASKS__REQUESTS__CLEAN_HPP + +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace rmf_tasks { +namespace requests { + +class Clean : public rmf_tasks::Request +{ +public: + + static rmf_tasks::Request::SharedPtr make( + std::size_t id, + std::size_t start_waypoint, + std::size_t end_waypoint, + rmf_traffic::Trajectory& cleaning_path, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr cleaning_sink, + std::shared_ptr planner, + bool drain_battery = true, + rmf_traffic::Time start_time = std::chrono::steady_clock::now()); + + std::size_t id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::StateConfig& state_config) const final; + + rmf_traffic::Duration invariant_duration() const final; + + rmf_traffic::Time earliest_start_time() const final; + + class Implementation; +private: + Clean(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace tasks +} // namespace rmf_tasks + +#endif // INCLUDE__RMF_TASKS__REQUESTS__CLEAN_HPP diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp new file mode 100644 index 000000000..ae97b11f4 --- /dev/null +++ b/rmf_tasks/src/requests/Clean.cpp @@ -0,0 +1,229 @@ +/* + * 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 + +#include + +namespace rmf_tasks { +namespace requests { + +//============================================================================== +class Clean::Implementation +{ +public: + + Implementation() + {} + + std::size_t id; + std::size_t start_waypoint; + std::size_t end_waypoint; + rmf_traffic::Trajectory cleaning_path; + std::shared_ptr motion_sink; + std::shared_ptr ambient_sink; + std::shared_ptr cleaning_sink; + std::shared_ptr planner; + bool drain_battery; + rmf_traffic::Time start_time; + + rmf_traffic::Duration invariant_duration; + double invariant_battery_drain; +}; + +//============================================================================== +rmf_tasks::Request::SharedPtr Clean::make( + std::size_t id, + std::size_t start_waypoint, + std::size_t end_waypoint, + rmf_traffic::Trajectory& cleaning_path, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr cleaning_sink, + std::shared_ptr planner, + bool drain_battery, + rmf_traffic::Time start_time) +{ + std::shared_ptr clean(new Clean()); + clean->_pimpl->id = id; + clean->_pimpl->start_waypoint = start_waypoint; + clean->_pimpl->end_waypoint = end_waypoint; + clean->_pimpl->cleaning_path = cleaning_path; + clean->_pimpl->motion_sink = std::move(motion_sink); + clean->_pimpl->ambient_sink = std::move(ambient_sink); + clean->_pimpl->cleaning_sink = std::move(cleaning_sink); + clean->_pimpl->planner = std::move(planner); + clean->_pimpl->drain_battery = drain_battery; + clean->_pimpl->start_time = start_time; + + // Calculate duration of invariant component of task + const auto& cleaning_start_time = cleaning_path.begin()->time(); + const auto& cleaning_finish_time = *cleaning_path.finish_time(); + + clean->_pimpl->invariant_duration = + cleaning_finish_time - cleaning_start_time; + clean->_pimpl->invariant_battery_drain = 0.0; + + if (clean->_pimpl->drain_battery) + { + // Compute battery drain over invariant path + const double dSOC_motion = + clean->_pimpl->motion_sink->compute_change_in_charge(cleaning_path); + const double dSOC_ambient = + clean->_pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(clean->_pimpl->invariant_duration)); + const double dSOC_cleaning = + clean->_pimpl->cleaning_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(clean->_pimpl->invariant_duration)); + clean->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_ambient + + dSOC_cleaning; + } + + return clean; +} + +//============================================================================== +Clean::Clean() +: _pimpl(rmf_utils::make_impl(Implementation())) +{} + +//============================================================================== +std::size_t Clean::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional Clean::estimate_finish( + const agv::State& initial_state, + const agv::StateConfig& state_config) const +{ + agv::State state( + _pimpl->end_waypoint, + initial_state.charging_waypoint(), + initial_state.finish_time(), + initial_state.battery_soc()); + + 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_ambient = 0.0; + + if (initial_state.waypoint() != _pimpl->start_waypoint) + { + // Compute plan to cleaning start waypoint along with battery drain + rmf_traffic::agv::Planner::Start start{ + start_time, + initial_state.waypoint(), + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; + + const auto result_to_start = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_start->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if(_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_ambient = + _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_ambient; + + if (battery_soc <= state_config.threshold_soc()) + { + // std::cout << " -- Clean: Unable to reach start" << std::endl; + return rmf_utils::nullopt; + } + } + } + + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; + const rmf_traffic::Time wait_until = + initial_state.finish_time() > ideal_start ? + initial_state.finish_time() : ideal_start; + + // Factor in invariants + state.finish_time( + wait_until + variant_duration + _pimpl->invariant_duration); + + if (_pimpl->drain_battery) + { + battery_soc -= _pimpl->invariant_battery_drain; + if (battery_soc <= state_config.threshold_soc()) + { + // std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; + return rmf_utils::nullopt; + } + + // Check if the robot has enough charge to head back to nearest charger + double retreat_battery_drain = 0.0; + if ( _pimpl->end_waypoint != state.charging_waypoint()) + { + rmf_traffic::agv::Planner::Start start{ + state.finish_time(), + _pimpl->end_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{state.charging_waypoint()}; + + const auto result_to_charger = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state.finish_time(); + + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_ambient; + } + + if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + return rmf_utils::nullopt; + + state.battery_soc(battery_soc); + } + + return Estimate(state, wait_until); +} + +//============================================================================== +rmf_traffic::Duration Clean::invariant_duration() const +{ + return _pimpl->invariant_duration; +} + +//============================================================================== +rmf_traffic::Time Clean::earliest_start_time() const +{ + return _pimpl->start_time; +} + +//============================================================================== +} // namespace requests +} // namespace rmf_tasks diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index dffff1447..71d04ddc7 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -176,7 +176,7 @@ rmf_utils::optional Delivery::estimate_finish( return rmf_utils::nullopt; } - // Check if we have enough charge to head back to nearest charger + // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) { @@ -192,7 +192,8 @@ rmf_utils::optional Delivery::estimate_finish( const auto& trajectory = result_to_charger->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = finish_time - state.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state.finish_time(); dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); dSOC_device = _pimpl->_device_sink->compute_change_in_charge( From 9f17b57a3b590e2a0db3165f5ad5a7b9ecdebf93 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 7 Oct 2020 21:39:25 +0800 Subject: [PATCH 047/128] Cleanup --- rmf_tasks/include/rmf_tasks/Request.hpp | 2 ++ rmf_tasks/src/agv/TaskPlanner.cpp | 44 ++---------------------- rmf_tasks/src/requests/ChargeBattery.cpp | 12 ++----- rmf_tasks/src/requests/Clean.cpp | 7 ---- rmf_tasks/src/requests/Delivery.cpp | 7 ---- 5 files changed, 7 insertions(+), 65 deletions(-) diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index 83fbc915e..846fe89fa 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -50,6 +50,8 @@ class Request // Get the earliest start time that this task may begin virtual rmf_traffic::Time earliest_start_time() const = 0; + + virtual ~Request() = default; }; } // namespace rmf_tasks diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index 37624a943..e1a25a692 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -598,43 +598,16 @@ class TaskPlanner::Implementation return cost; } - Assignments prune_assignments( - Assignments& assignments, - const std::vector& state_configs) + Assignments prune_assignments(Assignments& assignments) { for (std::size_t a = 0; a < assignments.size(); ++a) { if (assignments[a].empty()) continue; - + // Remove charging task at end of assignments if any if (assignments[a].back().task_id() == config->charge_battery_request()->id()) assignments[a].pop_back(); - - // Insert missing charging tasks if any - // if (assignments[a].size() > 1) - // { - // auto it = ++assignments[a].begin(); - // for (; it != assignments[a].end(); ++it) - // { - // auto prev_it = it; --prev_it; - // if (it->state().battery_soc() > prev_it->state().battery_soc() && - // it->task_id() != config->charge_battery_request()->id()) - // { - // auto estimate = config->charge_battery_request()->estimate_finish( - // prev_it->state(), state_configs[a]); - // assert(estimate.has_value()); - // assignments[a].insert( - // it, - // Assignment - // { - // config->charge_battery_request()->id(), - // estimate.value().finish_state(), - // estimate.value().wait_until() - // }); - // } - // } - // } } return assignments; @@ -663,10 +636,7 @@ class TaskPlanner::Implementation node = solve(node, initial_states, state_configs, requests.size(), start_time, interrupter); if (!node) - { - // std::cout << "No solution found!" << std::endl; return {}; - } assert(complete_assignments.size() == node->assigned_tasks.size()); for (std::size_t i = 0; i < complete_assignments.size(); ++i) @@ -681,19 +651,11 @@ class TaskPlanner::Implementation } if (node->unassigned_tasks.empty()) - return prune_assignments(complete_assignments, state_configs); - // return complete_assignments; + return prune_assignments(complete_assignments); - // std::unordered_map new_task_id_map; std::vector new_tasks; - // std::size_t task_counter = 0; for (const auto& u : node->unassigned_tasks) - { new_tasks.push_back(u.second.request); - // new_task_id_map[task_counter++] = task_id_map[u.first]; - } - - // task_id_map = std::move(new_task_id_map); // copy final state estimates std::vector estimates; diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index a794dc8af..1c466b9f7 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -15,8 +15,6 @@ * */ -#include - #include namespace rmf_tasks { @@ -31,7 +29,7 @@ class ChargeBattery::Implementation {} // fixed id for now - std::size_t _id = 101; + std::size_t _id = 1001; rmf_battery::agv::BatterySystemPtr _battery_system; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; @@ -85,10 +83,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( const agv::StateConfig& state_config) const { if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) - { - // std::cout << " -- Charge battery: Battery full" << std::endl; return rmf_utils::nullopt; - } // Compute time taken to reach charging waypoint from current location agv::State state( @@ -126,12 +121,9 @@ rmf_utils::optional ChargeBattery::estimate_finish( battery_soc = battery_soc - dSOC_motion - dSOC_device; } + // If a robot cannot reach its charging dock given its initial battery soc if (battery_soc <= state_config.threshold_soc()) - { - // If a robot cannot reach its charging dock given its initial battery soc - // std::cout << " -- Charge battery: Unable to reach charger" << std::endl; return rmf_utils::nullopt; - } } // Default _charge_soc = 1.0 diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp index ae97b11f4..2d3e6bb59 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_tasks/src/requests/Clean.cpp @@ -15,7 +15,6 @@ * */ -#include #include #include @@ -153,10 +152,7 @@ rmf_utils::optional Clean::estimate_finish( battery_soc = battery_soc - dSOC_motion - dSOC_ambient; if (battery_soc <= state_config.threshold_soc()) - { - // std::cout << " -- Clean: Unable to reach start" << std::endl; return rmf_utils::nullopt; - } } } @@ -173,10 +169,7 @@ rmf_utils::optional Clean::estimate_finish( { battery_soc -= _pimpl->invariant_battery_drain; if (battery_soc <= state_config.threshold_soc()) - { - // std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; return rmf_utils::nullopt; - } // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 71d04ddc7..1fbe10d86 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -15,7 +15,6 @@ * */ -#include #include #include @@ -152,10 +151,7 @@ rmf_utils::optional Delivery::estimate_finish( } if (battery_soc <= state_config.threshold_soc()) - { - // std::cout << " -- Delivery: Unable to reach pickup" << std::endl; return rmf_utils::nullopt; - } } const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; @@ -171,10 +167,7 @@ rmf_utils::optional Delivery::estimate_finish( { battery_soc -= _pimpl->_invariant_battery_drain; if (battery_soc <= state_config.threshold_soc()) - { - // std::cout << " -- Delivery: Unable to reach dropoff" << std::endl; return rmf_utils::nullopt; - } // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; From 33e8a84415bc86f7d9e3f60cd2cbd02dbc272fea Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 12 Oct 2020 17:13:37 +0800 Subject: [PATCH 048/128] Using rmf_traffic::agv::Plan::Start for holding waypoint and time information (#188) * Using rmf_traffic::agv::Plan::Start for holding waypoint and time information Signed-off-by: Aaron Chong * Added tests for State and StateConfig exceptions Signed-off-by: Aaron Chong * Using passed relative_start_time instead of getting current time using chrono Signed-off-by: Aaron Chong * Changing name from plan_start to location Signed-off-by: Aaron Chong --- rmf_tasks/include/rmf_tasks/Estimate.hpp | 11 +++ rmf_tasks/include/rmf_tasks/Request.hpp | 12 +-- rmf_tasks/include/rmf_tasks/agv/State.hpp | 50 ++++++++--- .../include/rmf_tasks/agv/StateConfig.hpp | 5 ++ .../include/rmf_tasks/agv/TaskPlanner.hpp | 2 + .../include/rmf_tasks/requests/Clean.hpp | 4 +- .../include/rmf_tasks/requests/Delivery.hpp | 4 +- rmf_tasks/src/agv/State.cpp | 71 ++++++++------- rmf_tasks/src/agv/StateConfig.cpp | 10 ++- rmf_tasks/src/agv/TaskPlanner.cpp | 60 +++++++------ rmf_tasks/src/requests/ChargeBattery.cpp | 9 +- rmf_tasks/src/requests/Clean.cpp | 15 ++-- rmf_tasks/src/requests/Delivery.cpp | 15 ++-- rmf_tasks/test/unit/agv/test_State.cpp | 76 ++++++++++++++++ rmf_tasks/test/unit/agv/test_StateConfig.cpp | 58 +++++++++++++ rmf_tasks/test/unit/agv/test_TaskPlanner.cpp | 87 ++++++++++--------- 16 files changed, 355 insertions(+), 134 deletions(-) create mode 100644 rmf_tasks/test/unit/agv/test_State.cpp create mode 100644 rmf_tasks/test/unit/agv/test_StateConfig.cpp diff --git a/rmf_tasks/include/rmf_tasks/Estimate.hpp b/rmf_tasks/include/rmf_tasks/Estimate.hpp index b57c8e79e..d0d41f0c9 100644 --- a/rmf_tasks/include/rmf_tasks/Estimate.hpp +++ b/rmf_tasks/include/rmf_tasks/Estimate.hpp @@ -29,14 +29,25 @@ class Estimate { public: + /// Constructor of an estimate of a task request. + /// + /// \param[in] finish_state + /// Finish state of the robot once it completes the task request. + /// + /// \param[in] wait_until + /// The ideal time the robot starts executing this task. Estimate(agv::State finish_state, rmf_traffic::Time wait_until); + /// Finish state of the robot once it completes the task request. agv::State finish_state() const; + /// Sets a new finish state for the robot. Estimate& finish_state(agv::State new_finish_state); + /// The ideal time the robot starts executing this task. rmf_traffic::Time wait_until() const; + /// Sets a new starting time for the robot to execute the task request. Estimate& wait_until(rmf_traffic::Time new_wait_until); class Implementation; diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp index 846fe89fa..c8fbbcf0a 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_tasks/include/rmf_tasks/Request.hpp @@ -29,26 +29,26 @@ namespace rmf_tasks { -/// Implement this for new tasks +/// Implement this for new type of requests. class Request { public: using SharedPtr = std::shared_ptr; - // Get the id of the task + /// Get the id of the task virtual std::size_t id() const = 0; - // Estimate the state of the robot when the task is finished along with the - // time the robot has to wait before commencing the task + /// 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 rmf_utils::optional estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const = 0; - // Estimate the invariant component of the task's duration + /// Estimate the invariant component of the task's duration virtual rmf_traffic::Duration invariant_duration() const = 0; - // Get the earliest start time that this task may begin + /// Get the earliest start time that this task may begin virtual rmf_traffic::Time earliest_start_time() const = 0; virtual ~Request() = default; diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp index bd569874b..08eb3d747 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp @@ -24,45 +24,67 @@ #include #include +#include namespace rmf_tasks { namespace agv { +/// This state representation is used for task planning. class State { public: /// Constructor /// - /// \param[in] waypoint + /// \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 - /// \param[in] finish_time + /// The charging waypoint index of this robot. + /// /// \param[in] battery_soc - /// \param[in] threshold_soc + /// Current battery state of charge of the robot. This value needs to be + /// between 0.0 to 1.0. State( - std::size_t waypoint, + rmf_traffic::agv::Plan::Start location, std::size_t charging_waypoint, - rmf_traffic::Time finish_time = std::chrono::steady_clock::now(), - double battery_soc = 1.0); - - State(); + double battery_soc); - std::size_t waypoint() const; + /// The current state's location. + rmf_traffic::agv::Plan::Start location() const; - State& waypoint(std::size_t new_waypoint); + /// 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); - rmf_traffic::Time finish_time() const; - - State& finish_time(rmf_traffic::Time new_finish_time); - + /// 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; diff --git a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp index 87274040f..0c21cda49 100644 --- a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp @@ -30,10 +30,15 @@ class StateConfig /// Constructor /// /// \param[in] threshold_soc + /// Minimum charge level the battery is allowed to deplete to. This + /// value needs to be between 0.0 and 1.0. StateConfig(double threshold_soc); + /// Gets the battery state of charge threshold value. double threshold_soc() const; + /// Sets a new battery state of charge threshold value. This value needs to be + /// between 0.0 and 1.0. StateConfig& threshold_soc(double threshold_soc); class Implementation; diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp index 731258b8c..594bd844e 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp @@ -127,6 +127,7 @@ class TaskPlanner /// Get the greedy planner based assignments for a set of initial states and /// requests Assignments greedy_plan( + rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, std::vector requests); @@ -138,6 +139,7 @@ class TaskPlanner /// recommended to call plan() method and use the greedy solution for bidding. /// If a bid is awarded, the optimal solution may be used for assignments. Assignments optimal_plan( + rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, std::vector requests, diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp index 759fbe335..53102f1df 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp @@ -49,8 +49,8 @@ class Clean : public rmf_tasks::Request std::shared_ptr ambient_sink, std::shared_ptr cleaning_sink, std::shared_ptr planner, - bool drain_battery = true, - rmf_traffic::Time start_time = std::chrono::steady_clock::now()); + rmf_traffic::Time start_time, + bool drain_battery = true); std::size_t id() const final; diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp index 16a2e8937..0bd9a3dc1 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp @@ -46,8 +46,8 @@ class Delivery : public rmf_tasks::Request std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, - bool drain_battery = true, - rmf_traffic::Time start_time = std::chrono::steady_clock::now()); + rmf_traffic::Time start_time, + bool drain_battery = true); std::size_t id() const final; diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp index accbd3112..9316ae563 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_tasks/src/agv/State.cpp @@ -15,6 +15,8 @@ * */ +#include + #include namespace rmf_tasks { @@ -26,49 +28,42 @@ class State::Implementation public: Implementation( - std::size_t waypoint, + rmf_traffic::agv::Plan::Start location, std::size_t charging_waypoint, - rmf_traffic::Time finish_time, double battery_soc) - : _waypoint(waypoint), + : _location(std::move(location)), _charging_waypoint(charging_waypoint), - _finish_time(finish_time), _battery_soc(battery_soc) - {} + { + if (_battery_soc < 0.0 || _battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs be between 0.0 and 1.0."); + } - std::size_t _waypoint; + rmf_traffic::agv::Plan::Start _location; std::size_t _charging_waypoint; - rmf_traffic::Time _finish_time; double _battery_soc; }; //============================================================================== State::State( - std::size_t waypoint, + rmf_traffic::agv::Plan::Start location, std::size_t charging_waypoint, - rmf_traffic::Time finish_time, double battery_soc) : _pimpl(rmf_utils::make_impl( - Implementation( - waypoint, charging_waypoint, finish_time, battery_soc))) -{} - -//============================================================================== -State::State() -: _pimpl(rmf_utils::make_impl( - Implementation(0, 0, std::chrono::steady_clock::now(), 0.0))) + Implementation(std::move(location), charging_waypoint, battery_soc))) {} //============================================================================== -std::size_t State::waypoint() const +rmf_traffic::agv::Plan::Start State::location() const { - return _pimpl->_waypoint; + return _pimpl->_location; } //============================================================================== -State& State::waypoint(std::size_t new_waypoint) +State& State::location(rmf_traffic::agv::Plan::Start new_location) { - _pimpl->_waypoint = new_waypoint; + _pimpl->_location = std::move(new_location); return *this; } @@ -86,30 +81,48 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint) } //============================================================================== -rmf_traffic::Time State::finish_time() const +double State::battery_soc() const { - return _pimpl->_finish_time; + return _pimpl->_battery_soc; } //============================================================================== -State& State::finish_time(rmf_traffic::Time new_finish_time) +State& State::battery_soc(double new_battery_soc) { - _pimpl->_finish_time = new_finish_time; + if (new_battery_soc < 0.0 || new_battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs be between 0.0 and 1.0."); + + _pimpl->_battery_soc = new_battery_soc; return *this; } //============================================================================== -double State::battery_soc() const +std::size_t State::waypoint() const { - return _pimpl->_battery_soc; + return _pimpl->_location.waypoint(); } //============================================================================== -State& State::battery_soc(double new_battery_soc) +State& State::waypoint(std::size_t new_waypoint) { - _pimpl->_battery_soc = new_battery_soc; + _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_tasks diff --git a/rmf_tasks/src/agv/StateConfig.cpp b/rmf_tasks/src/agv/StateConfig.cpp index d5593cb72..b187fe0d8 100644 --- a/rmf_tasks/src/agv/StateConfig.cpp +++ b/rmf_tasks/src/agv/StateConfig.cpp @@ -15,6 +15,8 @@ * */ +#include + #include namespace rmf_tasks { @@ -34,7 +36,9 @@ StateConfig::StateConfig(double threshold_soc) threshold_soc })) { - // Do nothing + if (threshold_soc < 0.0 || threshold_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge threshold needs be between 0.0 and 1.0."); } double StateConfig::threshold_soc() const @@ -44,6 +48,10 @@ double StateConfig::threshold_soc() const auto StateConfig::threshold_soc(double threshold_soc) -> StateConfig& { + if (threshold_soc < 0.0 || threshold_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge threshold needs be between 0.0 and 1.0."); + _pimpl->threshold_soc = threshold_soc; return *this; } diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp index e1a25a692..e3bc44e8e 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_tasks/src/agv/TaskPlanner.cpp @@ -614,6 +614,7 @@ class TaskPlanner::Implementation } Assignments complete_solve( + rmf_traffic::Time time_now, std::vector& initial_states, const std::vector& state_configs, const std::vector& requests, @@ -622,8 +623,7 @@ class TaskPlanner::Implementation { assert(initial_states.size() == state_configs.size()); - const rmf_traffic::Time start_time = std::chrono::steady_clock::now(); - auto node = make_initial_node(initial_states, state_configs, requests, start_time); + auto node = make_initial_node(initial_states, state_configs, requests, time_now); Node::AssignedTasks complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); @@ -631,9 +631,9 @@ class TaskPlanner::Implementation while (node) { if (greedy) - node = greedy_solve(node, initial_states, state_configs, start_time); + node = greedy_solve(node, initial_states, state_configs, time_now); else - node = solve(node, initial_states, state_configs, requests.size(), start_time, interrupter); + node = solve(node, initial_states, state_configs, requests.size(), time_now, interrupter); if (!node) return {}; @@ -659,17 +659,21 @@ class TaskPlanner::Implementation // copy final state estimates std::vector estimates; - estimates.resize(node->assigned_tasks.size()); + rmf_traffic::agv::Plan::Start empty_new_location{ + time_now, 0, 0.0}; + estimates.resize( + node->assigned_tasks.size(), + State{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().state(); + estimates[i] = assignments.back().state(); } - node = make_initial_node(estimates, state_configs, new_tasks, start_time); + node = make_initial_node(estimates, state_configs, new_tasks, time_now); initial_states = estimates; } @@ -681,7 +685,7 @@ class TaskPlanner::Implementation return compute_g(node.assigned_tasks); } - double compute_h(const Node& node, const rmf_traffic::Time relative_start_time) + double compute_h(const Node& node, const rmf_traffic::Time time_now) { std::vector initial_queue_values; initial_queue_values.resize( @@ -696,7 +700,7 @@ class TaskPlanner::Implementation u.second.candidates.best_finish_time() - u.second.request->invariant_duration(); const double variant_value = - rmf_traffic::time::to_seconds(variant_time - relative_start_time); + rmf_traffic::time::to_seconds(variant_time - time_now); const auto& range = u.second.candidates.best_candidates(); for (auto it = range.begin; it != range.end; ++it) @@ -720,7 +724,7 @@ class TaskPlanner::Implementation else value = rmf_traffic::time::to_seconds( - assignments.back().state().finish_time() - relative_start_time); + assignments.back().state().finish_time() - time_now); } } @@ -735,16 +739,16 @@ class TaskPlanner::Implementation return queue.compute_cost(); } - double compute_f(const Node& n, const rmf_traffic::Time relative_start_time) + double compute_f(const Node& n, const rmf_traffic::Time time_now) { - return compute_g(n) + compute_h(n, relative_start_time); + return compute_g(n) + compute_h(n, time_now); } ConstNodePtr make_initial_node( std::vector initial_states, std::vector state_configs, std::vector requests, - rmf_traffic::Time relative_start_time) + rmf_traffic::Time time_now) { auto initial_node = std::make_shared(); @@ -757,7 +761,7 @@ class TaskPlanner::Implementation PendingTask(initial_states, state_configs, request, config->charge_battery_request()) }); - initial_node->cost_estimate = compute_f(*initial_node, relative_start_time); + initial_node->cost_estimate = compute_f(*initial_node, time_now); initial_node->sort_invariants(); @@ -813,7 +817,7 @@ class TaskPlanner::Implementation const Node::UnassignedTasks::value_type& u, const ConstNodePtr& parent, Filter* filter, - rmf_traffic::Time relative_start_time, + rmf_traffic::Time time_now, const std::vector& state_configs) { @@ -916,7 +920,7 @@ class TaskPlanner::Implementation } // Update the cost estimate for new_node - new_node->cost_estimate = compute_f(*new_node, relative_start_time); + new_node->cost_estimate = compute_f(*new_node, time_now); new_node->latest_time = get_latest_time(*new_node); // Apply filter @@ -934,7 +938,7 @@ class TaskPlanner::Implementation const std::size_t agent, const std::vector& initial_states, const std::vector& state_configs, - rmf_traffic::Time relative_start_time) + rmf_traffic::Time time_now) { auto new_node = std::make_shared(*parent); // Assign charging task to an agent @@ -974,7 +978,7 @@ class TaskPlanner::Implementation } } - new_node->cost_estimate = compute_f(*new_node, relative_start_time); + new_node->cost_estimate = compute_f(*new_node, time_now); new_node->latest_time = get_latest_time(*new_node); return new_node; } @@ -986,7 +990,7 @@ class TaskPlanner::Implementation ConstNodePtr node, const std::vector& initial_states, const std::vector& state_configs, - rmf_traffic::Time relative_start_time) + rmf_traffic::Time time_now) { while (!finished(*node)) { @@ -997,7 +1001,7 @@ class TaskPlanner::Implementation for (auto it = range.begin; it != range.end; ++it) { if (auto n = expand_candidate( - it, u, node, nullptr, relative_start_time, state_configs)) + it, u, node, nullptr, time_now, state_configs)) { if (!next_node || (n->cost_estimate < next_node->cost_estimate)) { @@ -1021,7 +1025,7 @@ class TaskPlanner::Implementation it->second.candidate, initial_states, state_configs, - relative_start_time); + time_now); if (new_charge_node) { next_node = std::move(new_charge_node); @@ -1045,7 +1049,7 @@ class TaskPlanner::Implementation Filter& filter, const std::vector& initial_states, const std::vector& state_configs, - rmf_traffic::Time relative_start_time) + rmf_traffic::Time time_now) { std::vector new_nodes; new_nodes.reserve( @@ -1056,7 +1060,7 @@ class TaskPlanner::Implementation for (auto it = range.begin; it!= range.end; it++) { if (auto new_node = expand_candidate( - it, u, parent, &filter, relative_start_time, state_configs)) + it, u, parent, &filter, time_now, state_configs)) new_nodes.push_back(std::move(new_node)); } } @@ -1065,7 +1069,7 @@ class TaskPlanner::Implementation for (std::size_t i = 0; i < parent->assigned_tasks.size(); ++i) { if (auto new_node = expand_charger( - parent, i, initial_states, state_configs, relative_start_time)) + parent, i, initial_states, state_configs, time_now)) new_nodes.push_back(new_node); } @@ -1093,7 +1097,7 @@ class TaskPlanner::Implementation const std::vector& initial_states, const std::vector& state_configs, const std::size_t num_tasks, - rmf_traffic::Time relative_start_time, + rmf_traffic::Time time_now, std::function interrupter) { using PriorityQueue = std::priority_queue< @@ -1122,7 +1126,7 @@ class TaskPlanner::Implementation // Apply possible actions to expand the node const auto new_nodes = expand( - top, filter, initial_states, state_configs, relative_start_time); + top, filter, initial_states, state_configs, time_now); // Add copies and with a newly assigned task to queue for (const auto&n : new_nodes) @@ -1145,11 +1149,13 @@ TaskPlanner::TaskPlanner(std::shared_ptr config) } auto TaskPlanner::greedy_plan( + rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, std::vector requests) -> Assignments { return _pimpl->complete_solve( + time_now, initial_states, state_configs, requests, @@ -1158,12 +1164,14 @@ auto TaskPlanner::greedy_plan( } auto TaskPlanner::optimal_plan( + rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, std::vector requests, std::function interrupter) -> Assignments { return _pimpl->complete_solve( + time_now, initial_states, state_configs, requests, diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index 1c466b9f7..3a9b170f3 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -86,11 +86,14 @@ rmf_utils::optional ChargeBattery::estimate_finish( return rmf_utils::nullopt; // Compute time taken to reach charging waypoint from current location - agv::State state( + 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.finish_time(), - initial_state.battery_soc()); + initial_state.battery_soc()}; const auto start_time = initial_state.finish_time(); diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp index 2d3e6bb59..ff267b7eb 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_tasks/src/requests/Clean.cpp @@ -55,8 +55,8 @@ rmf_tasks::Request::SharedPtr Clean::make( std::shared_ptr ambient_sink, std::shared_ptr cleaning_sink, std::shared_ptr planner, - bool drain_battery, - rmf_traffic::Time start_time) + rmf_traffic::Time start_time, + bool drain_battery) { std::shared_ptr clean(new Clean()); clean->_pimpl->id = id; @@ -112,11 +112,14 @@ rmf_utils::optional Clean::estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const { - agv::State state( - _pimpl->end_waypoint, - initial_state.charging_waypoint(), + rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), - initial_state.battery_soc()); + _pimpl->end_waypoint, + initial_state.location().orientation()}; + agv::State state{ + std::move(final_plan_start), + initial_state.charging_waypoint(), + initial_state.battery_soc()}; rmf_traffic::Duration variant_duration(0); diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp index 1fbe10d86..798725abe 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_tasks/src/requests/Delivery.cpp @@ -51,8 +51,8 @@ rmf_tasks::Request::SharedPtr Delivery::make( std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, - bool drain_battery, - rmf_traffic::Time start_time) + rmf_traffic::Time start_time, + bool drain_battery) { std::shared_ptr delivery(new Delivery()); delivery->_pimpl->_id = id; @@ -110,11 +110,14 @@ rmf_utils::optional Delivery::estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const { - agv::State state( - _pimpl->_dropoff_waypoint, - initial_state.charging_waypoint(), + rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), - initial_state.battery_soc()); + _pimpl->_dropoff_waypoint, + initial_state.location().orientation()}; + agv::State state{ + std::move(final_plan_start), + initial_state.charging_waypoint(), + initial_state.battery_soc()}; rmf_traffic::Duration variant_duration(0); diff --git a/rmf_tasks/test/unit/agv/test_State.cpp b/rmf_tasks/test/unit/agv/test_State.cpp new file mode 100644 index 000000000..5b21cf471 --- /dev/null +++ b/rmf_tasks/test/unit/agv/test_State.cpp @@ -0,0 +1,76 @@ +/* + * 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 + +#include + +#include +#include + +#include + +SCENARIO("Robot States") +{ + const rmf_traffic::agv::Plan::Start basic_start{ + std::chrono::steady_clock::now(), + 0, + 0.0}; + + std::unique_ptr basic_state; + + WHEN("Empty battery") + { + CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{ + basic_start, + 0, + 0.0})); + } + + WHEN("Full battery") + { + CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{ + basic_start, + 0, + 1.0})); + } + + WHEN("Half battery") + { + CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{ + basic_start, + 0, + 0.5})); + } + + WHEN("Battery soc more than 1.0") + { + CHECK_THROWS(basic_state.reset(new rmf_tasks::agv::State{ + basic_start, + 0, + 1.0 + 1e-4})); + } + + WHEN("Battery soc less than 0.0") + { + CHECK_THROWS(basic_state.reset(new rmf_tasks::agv::State{ + basic_start, + 0, + 0.0 - 1e-4})); + } +} diff --git a/rmf_tasks/test/unit/agv/test_StateConfig.cpp b/rmf_tasks/test/unit/agv/test_StateConfig.cpp new file mode 100644 index 000000000..e205e1c98 --- /dev/null +++ b/rmf_tasks/test/unit/agv/test_StateConfig.cpp @@ -0,0 +1,58 @@ +/* + * 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 + +#include + +#include + +SCENARIO("Robot State Configs") +{ + std::unique_ptr basic_state_config; + + WHEN("Minimum battery threshold") + { + CHECK_NOTHROW( + basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.0})); + } + + WHEN("Maximum battery threshold") + { + CHECK_NOTHROW( + basic_state_config.reset(new rmf_tasks::agv::StateConfig{1.0})); + } + + WHEN("Half battery threshold") + { + CHECK_NOTHROW( + basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.5})); + } + + WHEN("Below minimum battery threshold") + { + CHECK_THROWS( + basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.0 - 1e-4})); + } + + WHEN("Above maximum battery threshold") + { + CHECK_THROWS( + basic_state_config.reset(new rmf_tasks::agv::StateConfig{1.0 + 1e-4})); + } +} diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp index 01f377307..0647ce473 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp @@ -55,7 +55,8 @@ inline void display_solution( { const auto& s = a.state(); const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9; - const double finish_seconds = s.finish_time().time_since_epoch().count()/1e9; + const rmf_traffic::Time finish_time = s.finish_time(); + const double finish_seconds = finish_time.time_since_epoch().count()/1e9; std::cout << " <" << a.task_id() << ": " << start_seconds << ", "<< finish_seconds << ", " << 100* s.battery_soc() << "%>" << std::endl; @@ -136,11 +137,15 @@ SCENARIO("Grid World") WHEN("Planning for 3 requests and 2 agents") { const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + 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 = { - rmf_tasks::agv::State{13, 13}, - rmf_tasks::agv::State{2, 2} + rmf_tasks::agv::State{first_location, 13, 1.0}, + rmf_tasks::agv::State{second_location, 2, 1.0} }; std::vector state_configs = @@ -158,8 +163,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 2, @@ -168,8 +173,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 3, @@ -178,8 +183,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)) + now + rmf_traffic::time::from_seconds(0), + drain_battery) }; std::shared_ptr task_config = @@ -187,11 +192,11 @@ SCENARIO("Grid World") rmf_tasks::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( - initial_states, state_configs, requests); + now, initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); const auto optimal_assignments = task_planner.optimal_plan( - initial_states, state_configs, requests, nullptr); + now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); display_solution("Greedy", greedy_assignments, greedy_cost); @@ -203,11 +208,15 @@ SCENARIO("Grid World") WHEN("Planning for 11 requests and 2 agents") { const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + 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 = { - rmf_tasks::agv::State{13, 13}, - rmf_tasks::agv::State{2, 2} + rmf_tasks::agv::State{first_location, 13, 1.0}, + rmf_tasks::agv::State{second_location, 2, 1.0} }; std::vector state_configs = @@ -225,8 +234,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 2, @@ -235,8 +244,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 3, @@ -245,8 +254,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 3, @@ -255,8 +264,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(0)), + now + rmf_traffic::time::from_seconds(0), + drain_battery), rmf_tasks::requests::Delivery::make( 4, @@ -265,8 +274,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(50000)), + now + rmf_traffic::time::from_seconds(50000), + drain_battery), rmf_tasks::requests::Delivery::make( 5, @@ -275,8 +284,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(50000)), + now + rmf_traffic::time::from_seconds(50000), + drain_battery), rmf_tasks::requests::Delivery::make( 6, @@ -285,8 +294,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 7, @@ -295,8 +304,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 8, @@ -305,8 +314,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 9, @@ -315,8 +324,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 10, @@ -325,8 +334,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)), + now + rmf_traffic::time::from_seconds(60000), + drain_battery), rmf_tasks::requests::Delivery::make( 11, @@ -335,8 +344,8 @@ SCENARIO("Grid World") motion_sink, device_sink, planner, - drain_battery, - now + rmf_traffic::time::from_seconds(60000)) + now + rmf_traffic::time::from_seconds(60000), + drain_battery) }; std::shared_ptr task_config = @@ -344,11 +353,11 @@ SCENARIO("Grid World") rmf_tasks::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( - initial_states, state_configs, requests); + now, initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); const auto optimal_assignments = task_planner.optimal_plan( - initial_states, state_configs, requests, nullptr); + now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); display_solution("Greedy", greedy_assignments, greedy_cost); From 1145a0820f8bb7962b429021d0d4f6a10fa81171 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 13 Oct 2020 16:15:20 +0800 Subject: [PATCH 049/128] Removed unused efficiency from PowerSystem and added estimate_voltage() method to BatterySystem --- rmf_battery/CMakeLists.txt | 2 +- .../include/rmf_battery/agv/BatterySystem.hpp | 31 ++++-- .../include/rmf_battery/agv/PowerSystem.hpp | 8 +- .../src/rmf_battery/agv/BatterySystem.cpp | 104 ++++++++++++++++-- .../src/rmf_battery/agv/PowerSystem.cpp | 23 +--- .../rmf_battery/agv/SimpleDevicePowerSink.cpp | 4 +- .../rmf_battery/agv/SimpleMotionPowerSink.cpp | 5 +- .../test/unit/agv/test_PowerSystem.cpp | 10 -- .../test/unit/agv/test_battery_drain.cpp | 5 +- 9 files changed, 127 insertions(+), 65 deletions(-) diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 77cdaa651..aa5e63626 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -40,7 +40,7 @@ target_include_directories(rmf_battery ${EIGEN3_INCLUDE_DIRS} ) -ament_export_interfaces(rmf_battery HAS_LIBRARY_TARGET) +ament_export_targets(rmf_battery HAS_LIBRARY_TARGET) ament_export_dependencies(rmf_utils) if(BUILD_TESTING) diff --git a/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp b/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp index 6c300456f..c0d16adf3 100644 --- a/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp @@ -57,11 +57,17 @@ class BatterySystem /// \param[in] exp_capacity /// The capacity of the battery in Ah at the end of the exponential zone /// in its discharge profile + /// + /// \param[in] nominal_capacity + /// The capacity of the battery in Ah at the end of the exponential zone + /// in its discharge profile BatteryProfile( double resistance, double max_voltage, double exp_voltage, - double exp_capacity); + double exp_capacity, + double nominal_capacity, + double discharge_current); BatteryProfile& resistance(double resistance); double resistance() const; @@ -75,6 +81,12 @@ class BatterySystem BatteryProfile& exp_capacity(double exp_capacity); double exp_capacity() const; + BatteryProfile& nominal_capacity(double nominal_capacity); + double nominal_capacity() const; + + BatteryProfile& discharge_current(double discharge_current); + double discharge_current() const; + /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; @@ -88,29 +100,34 @@ class BatterySystem /// \param[in] nominal_voltage /// The nominal voltage of the battery in volts /// - /// \param[in] nominal_capacity + /// \param[in] capacity /// The nominal capacity of the battery in Ah /// /// \param[in] charging_current /// The rated current in A for charging the battery /// /// \param[in] type - /// The chemisty type of the battery + /// The chemisty type of the battery /// /// \param[in] profile - /// The battery profile for this battery + /// The battery profile for this battery. This is only required for calling + /// the get_voltage() method BatterySystem( double nominal_voltage, - double nominal_capacity, + double capacity, double charging_current, BatteryType type = BatteryType::LeadAcid, rmf_utils::optional profile = rmf_utils::nullopt); + /// Estimate the voltage of the battery given its state of charge. This function + /// will return a nullopt if the battery profile is not defined + rmf_utils::optional estimate_voltage(const double soc) const; + BatterySystem& nominal_voltage(double nominal_voltage); double nominal_voltage() const; - BatterySystem& nominal_capacity(double nominal_capacity); - double nominal_capacity() const; + BatterySystem& capacity(double nominal_capacity); + double capacity() const; BatterySystem& charging_current(double charging_current); double charging_current() const; diff --git a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp index 7e53c33e9..20ad8f6aa 100644 --- a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp @@ -36,12 +36,9 @@ class PowerSystem /// \param[in] nominal_power /// The rated nominal power consumption in Watts for this power system /// - /// \param[in] efficiency - /// The efficiency of this power system PowerSystem( std::string name, - double nominal_power, - double efficiency = 1.0); + double nominal_power); PowerSystem& name(std::string name); const std::string& name() const; @@ -49,9 +46,6 @@ class PowerSystem PowerSystem& nominal_power(double nom_power); double nominal_power() const; - PowerSystem& efficiency(double efficiency); - double efficiency() const; - /// Returns true if the values are valid, i.e. greater than zero. bool valid() const; diff --git a/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp index 6a92efd3f..ba1523abf 100644 --- a/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp @@ -17,6 +17,8 @@ #include +#include + namespace rmf_battery { namespace agv { @@ -30,6 +32,8 @@ class BatterySystem::BatteryProfile::Implementation double max_voltage; double exp_voltage; double exp_capacity; + double nominal_capacity; + double discharge_current; }; //============================================================================== @@ -37,9 +41,19 @@ BatterySystem::BatteryProfile::BatteryProfile( const double resistance, const double max_voltage, const double exp_voltage, - const double exp_capacity) + const double exp_capacity, + const double nominal_capacity, + const double discharge_current) : _pimpl(rmf_utils::make_impl( - Implementation{resistance, max_voltage, exp_voltage, exp_capacity})) + Implementation + { + resistance, + max_voltage, + exp_voltage, + exp_capacity, + nominal_capacity, + discharge_current + })) { // Do nothing } @@ -90,17 +104,44 @@ auto BatteryProfile::exp_capacity(double exp_capacity) -> BatteryProfile& return *this; } +//============================================================================== +double BatteryProfile::nominal_capacity() const +{ + return _pimpl->nominal_capacity; +} + +//============================================================================== +auto BatteryProfile::nominal_capacity(double nominal_capacity) -> BatteryProfile& +{ + _pimpl->nominal_capacity = nominal_capacity; + return *this; +} + //============================================================================== double BatteryProfile::exp_capacity() const { return _pimpl->exp_capacity; } +//============================================================================== +auto BatteryProfile::discharge_current(double discharge_current) -> BatteryProfile& +{ + _pimpl->discharge_current = discharge_current; + return *this; +} + +//============================================================================== +double BatteryProfile::discharge_current() const +{ + return _pimpl->discharge_current; +} + //============================================================================== bool BatteryProfile::valid() const { return _pimpl->resistance > 0.0 && _pimpl->max_voltage > 0.0 && - _pimpl->exp_voltage > 0.0 && _pimpl->exp_capacity > 0.0; + _pimpl->exp_voltage > 0.0 && _pimpl->exp_capacity > 0.0 && + _pimpl->nominal_capacity > 0.0 && _pimpl->discharge_current > 0.0; } //============================================================================== @@ -108,29 +149,68 @@ class BatterySystem::Implementation { public: double nominal_voltage; - double nominal_capacity; + double capacity; double charging_current; BatteryType type; rmf_utils::optional profile; + + // Battery parameters used in get_voltage() to derive voltage of the battery + // given its state of charge. + // Ref: O. Tremblay, L. Dessaint and A. Dekkiche, "A Generic Battery Model for + // the Dynamic Simulation of Hybrid Electric Vehicles," 2007 IEEE Vehicle + // Power and Propulsion Conference, Arlington, TX, 2007, pp. 284-289, + // doi: 10.1109/VPPC.2007.4544139. + struct ProfileParams + { + double a; // V + double b; // (Ah)^-1 + double k; // V + double e0;// V + }; + + ProfileParams params = {0.0, 0.0, 0.0, 0.0}; }; //============================================================================== BatterySystem::BatterySystem( const double nominal_voltage, - const double nominal_capacity, + const double capacity, const double charging_current, BatteryType type, rmf_utils::optional profile) : _pimpl(rmf_utils::make_impl( Implementation{ nominal_voltage, - nominal_capacity, + capacity, charging_current, type, std::move(profile) })) { - // Do nothing + if (_pimpl->profile.has_value()) + { + assert(profile->valid()); + _pimpl->params.a = profile->max_voltage() - profile->exp_voltage(); + _pimpl->params.b = 3.0 / profile->exp_capacity(); + _pimpl->params.k = (profile->max_voltage() - _pimpl->nominal_voltage + + _pimpl->params.a*( + exp(-_pimpl->params.b*_pimpl->profile->nominal_capacity()) - 1)*( + _pimpl->capacity - _pimpl->profile->nominal_capacity())) / + _pimpl->profile->nominal_capacity(); + _pimpl->params.e0 = _pimpl->profile->max_voltage() + _pimpl->params.k + + _pimpl->profile->resistance() * _pimpl->profile->discharge_current() - + _pimpl->params.a; + } +} + +rmf_utils::optional BatterySystem::estimate_voltage(const double soc) const +{ + assert(soc > 0.0 && soc <= 1.0); + if (!_pimpl->profile.has_value() || !_pimpl->profile->valid()) + return rmf_utils::nullopt; + + return _pimpl->params.e0 - _pimpl->params.k*(1/soc) + _pimpl->params.a * + exp(-_pimpl->params.b * _pimpl->capacity*(1 - soc)); } //============================================================================== @@ -147,16 +227,16 @@ double BatterySystem::nominal_voltage() const } //============================================================================== -auto BatterySystem::nominal_capacity(double nom_capacity) -> BatterySystem& +auto BatterySystem::capacity(double nom_capacity) -> BatterySystem& { - _pimpl->nominal_capacity = nom_capacity; + _pimpl->capacity = nom_capacity; return *this; } //============================================================================== -double BatterySystem::nominal_capacity() const +double BatterySystem::capacity() const { - return _pimpl->nominal_capacity; + return _pimpl->capacity; } //============================================================================== @@ -204,7 +284,7 @@ rmf_utils::optional BatterySystem::profile() const bool BatterySystem::valid() const { bool valid = _pimpl->nominal_voltage > 0.0 && - _pimpl->nominal_capacity > 0.0 && _pimpl->charging_current > 0.0 && + _pimpl->capacity > 0.0 && _pimpl->charging_current > 0.0 && (_pimpl->type == BatteryType::LeadAcid || _pimpl->type == BatteryType::LiIon); if (_pimpl->profile) diff --git a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp index 01ababe2a..b7fc41876 100644 --- a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -25,16 +25,14 @@ class PowerSystem::Implementation public: std::string name; double nominal_power; - double efficiency; }; //============================================================================== PowerSystem::PowerSystem( const std::string name, - const double nominal_power, - const double efficiency) + const double nominal_power) : _pimpl(rmf_utils::make_impl( - Implementation{name, nominal_power, efficiency})) + Implementation{name, nominal_power})) { // Do nothing } @@ -65,25 +63,10 @@ double PowerSystem::nominal_power() const return _pimpl->nominal_power; } -//============================================================================== -auto PowerSystem::efficiency(double efficiency) -->PowerSystem& -{ - _pimpl->efficiency = efficiency; - return *this; -} - -//============================================================================== -double PowerSystem::efficiency() const -{ - return _pimpl->efficiency; -} - //============================================================================== bool PowerSystem::valid() const { - return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0 - && _pimpl->efficiency > 0.0; + return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0; } diff --git a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp index e469ca20a..846f1a59b 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp @@ -61,13 +61,13 @@ double SimpleDevicePowerSink::compute_change_in_charge( assert(_pimpl->battery_system.valid()); assert(_pimpl->power_system.valid()); - const double nominal_capacity = _pimpl->battery_system.nominal_capacity(); + const double capacity = _pimpl->battery_system.capacity(); const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); const double nominal_power = _pimpl->power_system.nominal_power(); const double dE = nominal_power * run_time; const double dQ = dE / nominal_voltage; - const double dSOC = dQ / (nominal_capacity * 3600.0); + const double dSOC = dQ / (capacity * 3600.0); return dSOC; } diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 35c10d9c2..1a1ad3aa6 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -79,7 +79,7 @@ double SimpleMotionPowerSink::compute_change_in_charge( assert(_pimpl->battery_system.valid()); assert(_pimpl->mechanical_system.valid()); - const double nominal_capacity = _pimpl->battery_system.nominal_capacity(); + const double capacity = _pimpl->battery_system.capacity(); const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); const double mass = _pimpl->mechanical_system.mass(); const double inertia = _pimpl->mechanical_system.inertia(); @@ -113,7 +113,6 @@ double SimpleMotionPowerSink::compute_change_in_charge( // Loss through acceleration const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; - // std::cout << "EA: " << EA << " alpha: " << alpha << " w: " << w << " v: " << v << std::endl; // Loss through friction const double EF = compute_friction_energy(friction, mass, v, sim_step); @@ -123,7 +122,7 @@ double SimpleMotionPowerSink::compute_change_in_charge( // The charge consumed const double dQ = dE / nominal_voltage; // The depleted state of charge - const double dSOC = dQ / (nominal_capacity * 3600.0); + const double dSOC = dQ / (capacity * 3600.0); return dSOC; } diff --git a/rmf_battery/test/unit/agv/test_PowerSystem.cpp b/rmf_battery/test/unit/agv/test_PowerSystem.cpp index 8f46dd1d8..7b2421aac 100644 --- a/rmf_battery/test/unit/agv/test_PowerSystem.cpp +++ b/rmf_battery/test/unit/agv/test_PowerSystem.cpp @@ -25,7 +25,6 @@ SCENARIO("Test PowerSystem") "cleaning_system", 60); REQUIRE(power_system.name() == "cleaning_system"); REQUIRE(power_system.nominal_power() - 60 == Approx(0.0)); - REQUIRE(power_system.efficiency() - 1.0 == Approx(0.0)); REQUIRE(power_system.valid()); WHEN("Name is set") @@ -33,24 +32,15 @@ SCENARIO("Test PowerSystem") power_system.name("vacuuming_system"); CHECK(power_system.name() == "vacuuming_system"); CHECK(power_system.nominal_power() - 60 == Approx(0.0)); - CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); CHECK(power_system.valid()); } WHEN("Nominal power is set") { power_system.nominal_power(80); CHECK(power_system.nominal_power() - 80 == Approx(0.0)); - CHECK(power_system.efficiency() - 1.0 == Approx(0.0)); CHECK(power_system.valid()); } - WHEN("Efficiency is set") - { - power_system.efficiency(0.80); - CHECK(power_system.nominal_power() - 60 == Approx(0.0)); - CHECK(power_system.efficiency() - 0.80 == Approx(0.0)); - CHECK(power_system.valid()); - } WHEN("A property is negative") { power_system.nominal_power(-12); diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 61b6a6bcd..928037d71 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -188,7 +188,6 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; CHECK(ok); } @@ -212,8 +211,8 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; + const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; + CHECK(ok); } WHEN("Testing turning on a spot") From 268d1659d81d18441ee3d12e9e34cda7da955030 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 13 Oct 2020 16:41:53 +0800 Subject: [PATCH 050/128] Switched to new BatterySystem API --- rmf_tasks/src/requests/ChargeBattery.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp index 3a9b170f3..667992ade 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_tasks/src/requests/ChargeBattery.cpp @@ -52,8 +52,8 @@ rmf_tasks::Request::SharedPtr ChargeBattery::make( std::shared_ptr charge_battery(new ChargeBattery()); charge_battery->_pimpl->_battery_system = rmf_battery::agv::BatterySystemPtr(new rmf_battery::agv::BatterySystem( - battery_system.nominal_capacity(), - battery_system.nominal_capacity(), + battery_system.nominal_voltage(), + battery_system.capacity(), battery_system.charging_current(), battery_system.type(), battery_system.profile())); @@ -133,7 +133,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( double delta_soc = _pimpl->_charge_soc - battery_soc; assert(delta_soc >= 0.0); double time_to_charge = - (3600 * delta_soc * _pimpl->_battery_system->nominal_capacity()) / + (3600 * delta_soc * _pimpl->_battery_system->capacity()) / _pimpl->_battery_system->charging_current(); const rmf_traffic::Time wait_until = initial_state.finish_time(); From 04707f370928c9e332c4edca9af3e3d43cbc2b76 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 13 Oct 2020 17:31:49 +0800 Subject: [PATCH 051/128] Renamed rmf_tasks to rmf_task --- {rmf_tasks => rmf_task}/CMakeLists.txt | 50 +++++------ rmf_task/cmake/rmf_task-config.cmake.in | 15 ++++ .../include/rmf_task}/Estimate.hpp | 12 +-- .../include/rmf_task}/Request.hpp | 16 ++-- .../include/rmf_task}/agv/State.hpp | 10 +-- .../include/rmf_task}/agv/StateConfig.hpp | 10 +-- .../include/rmf_task}/agv/TaskPlanner.hpp | 16 ++-- .../rmf_task}/requests/ChargeBattery.hpp | 22 ++--- .../include/rmf_task}/requests/Clean.hpp | 22 ++--- .../include/rmf_task}/requests/Delivery.hpp | 22 ++--- {rmf_tasks => rmf_task}/package.xml | 2 +- .../src/rmf_task}/Estimate.cpp | 6 +- .../src/rmf_task}/agv/State.cpp | 6 +- .../src/rmf_task}/agv/StateConfig.cpp | 6 +- .../src/rmf_task}/agv/TaskPlanner.cpp | 30 +++---- .../src/rmf_task}/requests/ChargeBattery.cpp | 10 +-- .../src/rmf_task}/requests/Clean.cpp | 10 +-- .../src/rmf_task}/requests/Delivery.cpp | 10 +-- {rmf_tasks => rmf_task}/test/main.cpp | 0 .../test/unit/agv/test_State.cpp | 14 ++-- .../test/unit/agv/test_StateConfig.cpp | 14 ++-- .../test/unit/agv/test_TaskPlanner.cpp | 84 +++++++++---------- rmf_tasks/cmake/rmf_tasks-config.cmake.in | 15 ---- 23 files changed, 201 insertions(+), 201 deletions(-) rename {rmf_tasks => rmf_task}/CMakeLists.txt (72%) create mode 100644 rmf_task/cmake/rmf_task-config.cmake.in rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/Estimate.hpp (88%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/Request.hpp (84%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/agv/State.hpp (93%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/agv/StateConfig.hpp (86%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/agv/TaskPlanner.hpp (93%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/requests/ChargeBattery.hpp (76%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/requests/Clean.hpp (79%) rename {rmf_tasks/include/rmf_tasks => rmf_task/include/rmf_task}/requests/Delivery.hpp (77%) rename {rmf_tasks => rmf_task}/package.xml (96%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/Estimate.cpp (96%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/agv/State.cpp (97%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/agv/StateConfig.cpp (94%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/agv/TaskPlanner.cpp (97%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/requests/ChargeBattery.cpp (96%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/requests/Clean.cpp (97%) rename {rmf_tasks/src => rmf_task/src/rmf_task}/requests/Delivery.cpp (97%) rename {rmf_tasks => rmf_task}/test/main.cpp (100%) rename {rmf_tasks => rmf_task}/test/unit/agv/test_State.cpp (76%) rename {rmf_tasks => rmf_task}/test/unit/agv/test_StateConfig.cpp (68%) rename {rmf_tasks => rmf_task}/test/unit/agv/test_TaskPlanner.cpp (80%) delete mode 100644 rmf_tasks/cmake/rmf_tasks-config.cmake.in diff --git a/rmf_tasks/CMakeLists.txt b/rmf_task/CMakeLists.txt similarity index 72% rename from rmf_tasks/CMakeLists.txt rename to rmf_task/CMakeLists.txt index a91cdafb3..c16af6827 100644 --- a/rmf_tasks/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5.0) -project(rmf_tasks VERSION 1.0.0) +project(rmf_task VERSION 1.0.0) set(CMAKE_EXPORT_COMPILE_COMMANDS on) @@ -34,21 +34,21 @@ find_package(rmf_cmake_uncrustify QUIET) # ===== RMF Tasks library file(GLOB lib_srcs - "src/agv/*.cpp" - "src/requests/*.cpp" - "src/*.cpp" + "src/rmf_task/agv/*.cpp" + "src/rmf_task/requests/*.cpp" + "src/rmf_task/*.cpp" ) -add_library(rmf_tasks SHARED +add_library(rmf_task SHARED ${lib_srcs} ) -target_link_libraries(rmf_tasks +target_link_libraries(rmf_task PUBLIC rmf_battery::rmf_battery ) -target_include_directories(rmf_tasks +target_include_directories(rmf_task PUBLIC $ $ @@ -59,15 +59,15 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) file(GLOB_RECURSE unit_test_srcs "test/*.cpp") ament_add_catch2( - test_rmf_tasks test/main.cpp ${unit_test_srcs} + test_rmf_task test/main.cpp ${unit_test_srcs} TIMEOUT 300) - target_link_libraries(test_rmf_tasks - rmf_tasks + target_link_libraries(test_rmf_task + rmf_task rmf_utils::rmf_utils rmf_traffic::rmf_traffic ) - target_include_directories(test_rmf_tasks + target_include_directories(test_rmf_task PRIVATE $ ) @@ -85,12 +85,12 @@ endif() # Create cmake config files include(CMakePackageConfigHelpers) -set(INSTALL_CONFIG_DIR "${CMAKE_INSTALL_LIBDIR}/rmf_tasks/cmake") -set(PACKAGE_CONFIG_VERSION_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_tasks-config-version.cmake") -set(PACKAGE_CONFIG_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_tasks-config.cmake") +set(INSTALL_CONFIG_DIR "${CMAKE_INSTALL_LIBDIR}/rmf_task/cmake") +set(PACKAGE_CONFIG_VERSION_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_task-config-version.cmake") +set(PACKAGE_CONFIG_FILE "${CMAKE_CURRENT_BINARY_DIR}/rmf_task-config.cmake") configure_package_config_file( - "${CMAKE_CURRENT_LIST_DIR}/cmake/rmf_tasks-config.cmake.in" + "${CMAKE_CURRENT_LIST_DIR}/cmake/rmf_task-config.cmake.in" ${PACKAGE_CONFIG_FILE} INSTALL_DESTINATION ${INSTALL_CONFIG_DIR} ) @@ -101,15 +101,15 @@ write_basic_package_version_file( ) install( - TARGETS rmf_tasks - EXPORT rmf_tasks-targets + TARGETS rmf_task + EXPORT rmf_task-targets RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} ) install( - DIRECTORY include/rmf_tasks + DIRECTORY include/rmf_task DESTINATION include ) @@ -121,16 +121,16 @@ install( ) install( - EXPORT rmf_tasks-targets - FILE rmf_tasks-targets.cmake - NAMESPACE rmf_tasks:: + EXPORT rmf_task-targets + FILE rmf_task-targets.cmake + NAMESPACE rmf_task:: DESTINATION ${INSTALL_CONFIG_DIR} ) export( - EXPORT rmf_tasks-targets - FILE ${CMAKE_CURRENT_BINARY_DIR}/rmf_tasks-targets.cmake - NAMESPACE rmf_tasks:: + EXPORT rmf_task-targets + FILE ${CMAKE_CURRENT_BINARY_DIR}/rmf_task-targets.cmake + NAMESPACE rmf_task:: ) -export(PACKAGE rmf_tasks) +export(PACKAGE rmf_task) diff --git a/rmf_task/cmake/rmf_task-config.cmake.in b/rmf_task/cmake/rmf_task-config.cmake.in new file mode 100644 index 000000000..a10a41b65 --- /dev/null +++ b/rmf_task/cmake/rmf_task-config.cmake.in @@ -0,0 +1,15 @@ +@PACKAGE_INIT@ + +get_filename_component(rmf_task_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) + +include(CMakeFindDependencyMacro) + +find_dependency(rmf_utils REQUIRED) +find_dependency(rmf_traffic REQUIRED) +find_dependency(rmf_battery REQUIRED) + +if(NOT TARGET rmf_task::rmf_task) + include("${rmf_task_CMAKE_DIR}/rmf_task-targets.cmake") +endif() + +check_required_components(rmf_task) diff --git a/rmf_tasks/include/rmf_tasks/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp similarity index 88% rename from rmf_tasks/include/rmf_tasks/Estimate.hpp rename to rmf_task/include/rmf_task/Estimate.hpp index d0d41f0c9..c826ab91d 100644 --- a/rmf_tasks/include/rmf_tasks/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -15,14 +15,14 @@ * */ -#ifndef INCLUDE__RMF_TASKS__ESTIMATE_HPP -#define INCLUDE__RMF_TASKS__ESTIMATE_HPP +#ifndef INCLUDE__RMF_TASK__ESTIMATE_HPP +#define INCLUDE__RMF_TASK__ESTIMATE_HPP -#include +#include #include #include -namespace rmf_tasks { +namespace rmf_task { /// Estimates for requests class Estimate @@ -55,6 +55,6 @@ class Estimate rmf_utils::impl_ptr _pimpl; }; -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__ESTIMATE_HPP +#endif // INCLUDE__RMF_TASK__ESTIMATE_HPP diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_task/include/rmf_task/Request.hpp similarity index 84% rename from rmf_tasks/include/rmf_tasks/Request.hpp rename to rmf_task/include/rmf_task/Request.hpp index c8fbbcf0a..1c24b6ee2 100644 --- a/rmf_tasks/include/rmf_tasks/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -15,19 +15,19 @@ * */ -#ifndef INCLUDE__RMF_TASKS__TASK_HPP -#define INCLUDE__RMF_TASKS__TASK_HPP +#ifndef INCLUDE__RMF_TASK__TASK_HPP +#define INCLUDE__RMF_TASK__TASK_HPP #include -#include -#include -#include +#include +#include +#include #include #include -namespace rmf_tasks { +namespace rmf_task { /// Implement this for new type of requests. class Request @@ -54,6 +54,6 @@ class Request virtual ~Request() = default; }; -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__TASK_HPP +#endif // INCLUDE__RMF_TASK__TASK_HPP diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_task/include/rmf_task/agv/State.hpp similarity index 93% rename from rmf_tasks/include/rmf_tasks/agv/State.hpp rename to rmf_task/include/rmf_task/agv/State.hpp index 08eb3d747..fbfabf060 100644 --- a/rmf_tasks/include/rmf_tasks/agv/State.hpp +++ b/rmf_task/include/rmf_task/agv/State.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASKS__AGV__STATE_HPP -#define INCLUDE__RMF_TASKS__AGV__STATE_HPP +#ifndef INCLUDE__RMF_TASK__AGV__STATE_HPP +#define INCLUDE__RMF_TASK__AGV__STATE_HPP #include @@ -26,7 +26,7 @@ #include #include -namespace rmf_tasks { +namespace rmf_task { namespace agv { /// This state representation is used for task planning. @@ -91,6 +91,6 @@ class State }; } // namespace agv -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__AGV__STATE_HPP +#endif // INCLUDE__RMF_TASK__AGV__STATE_HPP diff --git a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp b/rmf_task/include/rmf_task/agv/StateConfig.hpp similarity index 86% rename from rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp rename to rmf_task/include/rmf_task/agv/StateConfig.hpp index 0c21cda49..94c7b2352 100644 --- a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp +++ b/rmf_task/include/rmf_task/agv/StateConfig.hpp @@ -15,12 +15,12 @@ * */ -#ifndef INCLUDE__RMF_TASKS__AGV__STATECONFIG_HPP -#define INCLUDE__RMF_TASKS__AGV__STATECONFIG_HPP +#ifndef INCLUDE__RMF_TASK__AGV__STATECONFIG_HPP +#define INCLUDE__RMF_TASK__AGV__STATECONFIG_HPP #include -namespace rmf_tasks { +namespace rmf_task { namespace agv { class StateConfig @@ -47,6 +47,6 @@ class StateConfig }; } // namespace agv -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__AGV__STATE_HPP +#endif // INCLUDE__RMF_TASK__AGV__STATE_HPP diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp similarity index 93% rename from rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp rename to rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 594bd844e..06c603207 100644 --- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -15,12 +15,12 @@ * */ -#ifndef RMF_TASKS__AGV__TASKPLANNER_HPP -#define RMF_TASKS__AGV__TASKPLANNER_HPP +#ifndef RMF_TASK__AGV__TASKPLANNER_HPP +#define RMF_TASK__AGV__TASKPLANNER_HPP -#include -#include -#include +#include +#include +#include #include @@ -28,7 +28,7 @@ #include #include -namespace rmf_tasks { +namespace rmf_task { namespace agv { @@ -156,6 +156,6 @@ class TaskPlanner } // namespace agv -} // namespace rmf_tasks +} // namespace rmf_task -#endif // RMF_TASKS__AGV__TASKPLANNER_HPP +#endif // RMF_TASK__AGV__TASKPLANNER_HPP diff --git a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp similarity index 76% rename from rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp rename to rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 4bfcef0d6..af08ceead 100644 --- a/rmf_tasks/include/rmf_tasks/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASKS__REQUESTS__CHARGEBATTERY_HPP -#define INCLUDE__RMF_TASKS__REQUESTS__CHARGEBATTERY_HPP +#ifndef INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP +#define INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP #include #include @@ -27,18 +27,18 @@ #include -#include -#include -#include +#include +#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace requests { -class ChargeBattery : public rmf_tasks::Request +class ChargeBattery : public rmf_task::Request { public: - static rmf_tasks::Request::SharedPtr make( + static rmf_task::Request::SharedPtr make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr device_sink, @@ -47,7 +47,7 @@ class ChargeBattery : public rmf_tasks::Request std::size_t id() const final; - rmf_utils::optional estimate_finish( + rmf_utils::optional estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const final; @@ -62,6 +62,6 @@ class ChargeBattery : public rmf_tasks::Request }; } // namespace requests -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__REQUESTS__CHARGEBATTERY_HPP +#endif // INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp similarity index 79% rename from rmf_tasks/include/rmf_tasks/requests/Clean.hpp rename to rmf_task/include/rmf_task/requests/Clean.hpp index 53102f1df..98d148e25 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASKS__REQUESTS__CLEAN_HPP -#define INCLUDE__RMF_TASKS__REQUESTS__CLEAN_HPP +#ifndef INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP +#define INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP #include @@ -29,18 +29,18 @@ #include -#include -#include -#include +#include +#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace requests { -class Clean : public rmf_tasks::Request +class Clean : public rmf_task::Request { public: - static rmf_tasks::Request::SharedPtr make( + static rmf_task::Request::SharedPtr make( std::size_t id, std::size_t start_waypoint, std::size_t end_waypoint, @@ -54,7 +54,7 @@ class Clean : public rmf_tasks::Request std::size_t id() const final; - rmf_utils::optional estimate_finish( + rmf_utils::optional estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const final; @@ -69,6 +69,6 @@ class Clean : public rmf_tasks::Request }; } // namespace tasks -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__REQUESTS__CLEAN_HPP +#endif // INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp similarity index 77% rename from rmf_tasks/include/rmf_tasks/requests/Delivery.hpp rename to rmf_task/include/rmf_task/requests/Delivery.hpp index 0bd9a3dc1..062840ed0 100644 --- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASKS__REQUESTS__DELIVERY_HPP -#define INCLUDE__RMF_TASKS__REQUESTS__DELIVERY_HPP +#ifndef INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP +#define INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP #include @@ -28,18 +28,18 @@ #include -#include -#include -#include +#include +#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace requests { -class Delivery : public rmf_tasks::Request +class Delivery : public rmf_task::Request { public: - static rmf_tasks::Request::SharedPtr make( + static rmf_task::Request::SharedPtr make( std::size_t id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, @@ -51,7 +51,7 @@ class Delivery : public rmf_tasks::Request std::size_t id() const final; - rmf_utils::optional estimate_finish( + rmf_utils::optional estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const final; @@ -66,6 +66,6 @@ class Delivery : public rmf_tasks::Request }; } // namespace tasks -} // namespace rmf_tasks +} // namespace rmf_task -#endif // INCLUDE__RMF_TASKS__REQUESTS__DELIVERY_HPP +#endif // INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP diff --git a/rmf_tasks/package.xml b/rmf_task/package.xml similarity index 96% rename from rmf_tasks/package.xml rename to rmf_task/package.xml index 618939f87..658107312 100644 --- a/rmf_tasks/package.xml +++ b/rmf_task/package.xml @@ -1,7 +1,7 @@ - rmf_tasks + rmf_task 1.0.0 Package for managing tasks in the Robotics Middleware Framework Aaron diff --git a/rmf_tasks/src/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp similarity index 96% rename from rmf_tasks/src/Estimate.cpp rename to rmf_task/src/rmf_task/Estimate.cpp index dee397fe1..6979ac353 100644 --- a/rmf_tasks/src/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -15,9 +15,9 @@ * */ -#include +#include -namespace rmf_tasks { +namespace rmf_task { //============================================================================== class Estimate::Implementation @@ -66,4 +66,4 @@ Estimate& Estimate::wait_until(rmf_traffic::Time new_wait_until) } //============================================================================== -} // namespace rmf_tasks +} // namespace rmf_task diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp similarity index 97% rename from rmf_tasks/src/agv/State.cpp rename to rmf_task/src/rmf_task/agv/State.cpp index 9316ae563..c8ec09077 100644 --- a/rmf_tasks/src/agv/State.cpp +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -17,9 +17,9 @@ #include -#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace agv { //============================================================================== @@ -125,4 +125,4 @@ State& State::finish_time(rmf_traffic::Time new_finish_time) //============================================================================== } // namespace agv -} // namespace rmf_tasks +} // namespace rmf_task diff --git a/rmf_tasks/src/agv/StateConfig.cpp b/rmf_task/src/rmf_task/agv/StateConfig.cpp similarity index 94% rename from rmf_tasks/src/agv/StateConfig.cpp rename to rmf_task/src/rmf_task/agv/StateConfig.cpp index b187fe0d8..e1ead152e 100644 --- a/rmf_tasks/src/agv/StateConfig.cpp +++ b/rmf_task/src/rmf_task/agv/StateConfig.cpp @@ -17,9 +17,9 @@ #include -#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace agv { class StateConfig::Implementation @@ -58,4 +58,4 @@ auto StateConfig::threshold_soc(double threshold_soc) -> StateConfig& } // namespace agv -} // namespace rmf_tasks \ No newline at end of file +} // namespace rmf_task \ No newline at end of file diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp similarity index 97% rename from rmf_tasks/src/agv/TaskPlanner.cpp rename to rmf_task/src/rmf_task/agv/TaskPlanner.cpp index e3bc44e8e..2e9f7c262 100644 --- a/rmf_tasks/src/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -15,10 +15,10 @@ * */ -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -31,7 +31,7 @@ #include #include -namespace rmf_tasks { +namespace rmf_task { namespace agv { @@ -165,8 +165,8 @@ class Candidates static Candidates make( const std::vector& initial_states, const std::vector& state_configs, - const rmf_tasks::Request& request, - const rmf_tasks::Request& charge_battery_request); + const rmf_task::Request& request, + const rmf_task::Request& charge_battery_request); Candidates(const Candidates& other) { @@ -252,8 +252,8 @@ class Candidates Candidates Candidates::make( const std::vector& initial_states, const std::vector& state_configs, - const rmf_tasks::Request& request, - const rmf_tasks::Request& charge_battery_request) + const rmf_task::Request& request, + const rmf_task::Request& charge_battery_request) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) @@ -297,10 +297,10 @@ Candidates Candidates::make( struct PendingTask { PendingTask( - std::vector& initial_states, - std::vector& state_configs, - rmf_tasks::Request::SharedPtr request_, - rmf_tasks::Request::SharedPtr charge_battery_request) + std::vector& initial_states, + std::vector& state_configs, + rmf_task::Request::SharedPtr request_, + rmf_task::Request::SharedPtr charge_battery_request) : request(std::move(request_)), candidates(Candidates::make( initial_states, state_configs, *request, *charge_battery_request)), @@ -309,7 +309,7 @@ struct PendingTask // Do nothing } - rmf_tasks::Request::SharedPtr request; + rmf_task::Request::SharedPtr request; Candidates candidates; rmf_traffic::Time earliest_start_time; }; @@ -1188,4 +1188,4 @@ auto TaskPlanner::compute_cost(const Assignments& assignments) -> double } // namespace agv -} // namespace rmf_tasks +} // namespace rmf_task diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp similarity index 96% rename from rmf_tasks/src/requests/ChargeBattery.cpp rename to rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 667992ade..ae29907d3 100644 --- a/rmf_tasks/src/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -15,9 +15,9 @@ * */ -#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace requests { //============================================================================== @@ -42,7 +42,7 @@ class ChargeBattery::Implementation }; //============================================================================== -rmf_tasks::Request::SharedPtr ChargeBattery::make( +rmf_task::Request::SharedPtr ChargeBattery::make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr device_sink, @@ -78,7 +78,7 @@ std::size_t ChargeBattery::id() const } //============================================================================== -rmf_utils::optional ChargeBattery::estimate_finish( +rmf_utils::optional ChargeBattery::estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const { @@ -159,4 +159,4 @@ rmf_traffic::Time ChargeBattery::earliest_start_time() const //============================================================================== } // namespace requests -} // namespace rmf_tasks +} // namespace rmf_task diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp similarity index 97% rename from rmf_tasks/src/requests/Clean.cpp rename to rmf_task/src/rmf_task/requests/Clean.cpp index ff267b7eb..931e53f49 100644 --- a/rmf_tasks/src/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -17,9 +17,9 @@ #include -#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace requests { //============================================================================== @@ -46,7 +46,7 @@ class Clean::Implementation }; //============================================================================== -rmf_tasks::Request::SharedPtr Clean::make( +rmf_task::Request::SharedPtr Clean::make( std::size_t id, std::size_t start_waypoint, std::size_t end_waypoint, @@ -108,7 +108,7 @@ std::size_t Clean::id() const } //============================================================================== -rmf_utils::optional Clean::estimate_finish( +rmf_utils::optional Clean::estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const { @@ -222,4 +222,4 @@ rmf_traffic::Time Clean::earliest_start_time() const //============================================================================== } // namespace requests -} // namespace rmf_tasks +} // namespace rmf_task diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp similarity index 97% rename from rmf_tasks/src/requests/Delivery.cpp rename to rmf_task/src/rmf_task/requests/Delivery.cpp index 798725abe..980b3160e 100644 --- a/rmf_tasks/src/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -17,9 +17,9 @@ #include -#include +#include -namespace rmf_tasks { +namespace rmf_task { namespace requests { //============================================================================== @@ -44,7 +44,7 @@ class Delivery::Implementation }; //============================================================================== -rmf_tasks::Request::SharedPtr Delivery::make( +rmf_task::Request::SharedPtr Delivery::make( std::size_t id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, @@ -106,7 +106,7 @@ std::size_t Delivery::id() const } //============================================================================== -rmf_utils::optional Delivery::estimate_finish( +rmf_utils::optional Delivery::estimate_finish( const agv::State& initial_state, const agv::StateConfig& state_config) const { @@ -220,4 +220,4 @@ rmf_traffic::Time Delivery::earliest_start_time() const //============================================================================== } // namespace requests -} // namespace rmf_tasks +} // namespace rmf_task diff --git a/rmf_tasks/test/main.cpp b/rmf_task/test/main.cpp similarity index 100% rename from rmf_tasks/test/main.cpp rename to rmf_task/test/main.cpp diff --git a/rmf_tasks/test/unit/agv/test_State.cpp b/rmf_task/test/unit/agv/test_State.cpp similarity index 76% rename from rmf_tasks/test/unit/agv/test_State.cpp rename to rmf_task/test/unit/agv/test_State.cpp index 5b21cf471..84a558134 100644 --- a/rmf_tasks/test/unit/agv/test_State.cpp +++ b/rmf_task/test/unit/agv/test_State.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -32,11 +32,11 @@ SCENARIO("Robot States") 0, 0.0}; - std::unique_ptr basic_state; + std::unique_ptr basic_state; WHEN("Empty battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{ + CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ basic_start, 0, 0.0})); @@ -44,7 +44,7 @@ SCENARIO("Robot States") WHEN("Full battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{ + CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ basic_start, 0, 1.0})); @@ -52,7 +52,7 @@ SCENARIO("Robot States") WHEN("Half battery") { - CHECK_NOTHROW(basic_state.reset(new rmf_tasks::agv::State{ + CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ basic_start, 0, 0.5})); @@ -60,7 +60,7 @@ SCENARIO("Robot States") WHEN("Battery soc more than 1.0") { - CHECK_THROWS(basic_state.reset(new rmf_tasks::agv::State{ + CHECK_THROWS(basic_state.reset(new rmf_task::agv::State{ basic_start, 0, 1.0 + 1e-4})); @@ -68,7 +68,7 @@ SCENARIO("Robot States") WHEN("Battery soc less than 0.0") { - CHECK_THROWS(basic_state.reset(new rmf_tasks::agv::State{ + CHECK_THROWS(basic_state.reset(new rmf_task::agv::State{ basic_start, 0, 0.0 - 1e-4})); diff --git a/rmf_tasks/test/unit/agv/test_StateConfig.cpp b/rmf_task/test/unit/agv/test_StateConfig.cpp similarity index 68% rename from rmf_tasks/test/unit/agv/test_StateConfig.cpp rename to rmf_task/test/unit/agv/test_StateConfig.cpp index e205e1c98..15f50a15c 100644 --- a/rmf_tasks/test/unit/agv/test_StateConfig.cpp +++ b/rmf_task/test/unit/agv/test_StateConfig.cpp @@ -18,41 +18,41 @@ #include #include -#include +#include #include SCENARIO("Robot State Configs") { - std::unique_ptr basic_state_config; + std::unique_ptr basic_state_config; WHEN("Minimum battery threshold") { CHECK_NOTHROW( - basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.0})); + basic_state_config.reset(new rmf_task::agv::StateConfig{0.0})); } WHEN("Maximum battery threshold") { CHECK_NOTHROW( - basic_state_config.reset(new rmf_tasks::agv::StateConfig{1.0})); + basic_state_config.reset(new rmf_task::agv::StateConfig{1.0})); } WHEN("Half battery threshold") { CHECK_NOTHROW( - basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.5})); + basic_state_config.reset(new rmf_task::agv::StateConfig{0.5})); } WHEN("Below minimum battery threshold") { CHECK_THROWS( - basic_state_config.reset(new rmf_tasks::agv::StateConfig{0.0 - 1e-4})); + basic_state_config.reset(new rmf_task::agv::StateConfig{0.0 - 1e-4})); } WHEN("Above maximum battery threshold") { CHECK_THROWS( - basic_state_config.reset(new rmf_tasks::agv::StateConfig{1.0 + 1e-4})); + basic_state_config.reset(new rmf_task::agv::StateConfig{1.0 + 1e-4})); } } diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp similarity index 80% rename from rmf_tasks/test/unit/agv/test_TaskPlanner.cpp rename to rmf_task/test/unit/agv/test_TaskPlanner.cpp index 0647ce473..c1a17f4ee 100644 --- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -16,11 +16,11 @@ */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -43,7 +43,7 @@ //============================================================================== inline void display_solution( std::string parent, - const rmf_tasks::agv::TaskPlanner::Assignments& assignments, + const rmf_task::agv::TaskPlanner::Assignments& assignments, const double cost) { std::cout << parent << " cost: " << cost << std::endl; @@ -128,7 +128,7 @@ SCENARIO("Grid World") std::shared_ptr device_sink = std::make_shared(battery_system, power_system); - auto charge_battery_task = rmf_tasks::requests::ChargeBattery::make( + auto charge_battery_task = rmf_task::requests::ChargeBattery::make( battery_system, motion_sink, device_sink, planner, @@ -142,21 +142,21 @@ 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_tasks::agv::State{first_location, 13, 1.0}, - rmf_tasks::agv::State{second_location, 2, 1.0} + rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::agv::State{second_location, 2, 1.0} }; - std::vector state_configs = + std::vector state_configs = { - rmf_tasks::agv::StateConfig{0.2}, - rmf_tasks::agv::StateConfig{0.2} + rmf_task::agv::StateConfig{0.2}, + rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 1, 0, 3, @@ -166,7 +166,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 2, 15, 2, @@ -176,7 +176,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 3, 7, 9, @@ -187,9 +187,9 @@ SCENARIO("Grid World") drain_battery) }; - std::shared_ptr task_config = - std::make_shared(charge_battery_task); - rmf_tasks::agv::TaskPlanner task_planner(task_config); + std::shared_ptr task_config = + std::make_shared(charge_battery_task); + rmf_task::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); @@ -213,21 +213,21 @@ 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_tasks::agv::State{first_location, 13, 1.0}, - rmf_tasks::agv::State{second_location, 2, 1.0} + rmf_task::agv::State{first_location, 13, 1.0}, + rmf_task::agv::State{second_location, 2, 1.0} }; - std::vector state_configs = + std::vector state_configs = { - rmf_tasks::agv::StateConfig{0.2}, - rmf_tasks::agv::StateConfig{0.2} + rmf_task::agv::StateConfig{0.2}, + rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 1, 0, 3, @@ -237,7 +237,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 2, 15, 2, @@ -247,7 +247,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 3, 7, 9, @@ -257,7 +257,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 3, 7, 9, @@ -267,7 +267,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 4, 8, 11, @@ -277,7 +277,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(50000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 5, 10, 0, @@ -287,7 +287,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(50000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 6, 4, 8, @@ -297,7 +297,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(60000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 7, 8, 14, @@ -307,7 +307,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(60000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 8, 5, 11, @@ -317,7 +317,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(60000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 9, 9, 0, @@ -327,7 +327,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(60000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 10, 1, 3, @@ -337,7 +337,7 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(60000), drain_battery), - rmf_tasks::requests::Delivery::make( + rmf_task::requests::Delivery::make( 11, 0, 12, @@ -348,9 +348,9 @@ SCENARIO("Grid World") drain_battery) }; - std::shared_ptr task_config = - std::make_shared(charge_battery_task); - rmf_tasks::agv::TaskPlanner task_planner(task_config); + std::shared_ptr task_config = + std::make_shared(charge_battery_task); + rmf_task::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); diff --git a/rmf_tasks/cmake/rmf_tasks-config.cmake.in b/rmf_tasks/cmake/rmf_tasks-config.cmake.in deleted file mode 100644 index 71021c48b..000000000 --- a/rmf_tasks/cmake/rmf_tasks-config.cmake.in +++ /dev/null @@ -1,15 +0,0 @@ -@PACKAGE_INIT@ - -get_filename_component(rmf_tasks_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) - -include(CMakeFindDependencyMacro) - -find_dependency(rmf_utils REQUIRED) -find_dependency(rmf_traffic REQUIRED) -find_dependency(rmf_battery REQUIRED) - -if(NOT TARGET rmf_tasks::rmf_tasks) - include("${rmf_tasks_CMAKE_DIR}/rmf_tasks-targets.cmake") -endif() - -check_required_components(rmf_tasks) From 4206f587a20d1cc4dac841311d69b57808a9a4bc Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 15 Oct 2020 13:44:37 +0800 Subject: [PATCH 052/128] Update schedule with docking itinerary (#190) * Update schedule with docking itinerary * Use optional of reference wrapper (#191) Signed-off-by: Aaron Chong * Set docking itinerary every 1s Co-authored-by: Aaron Chong --- .../agv/RobotUpdateHandle.hpp | 8 ++++ rmf_fleet_adapter/src/full_control/main.cpp | 37 ++++++++++++++++++- .../agv/RobotUpdateHandle.cpp | 12 ++++++ 3 files changed, 56 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index afdb8475d..f0ad0937d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -22,9 +22,13 @@ #include #include +#include + #include #include +#include +#include namespace rmf_fleet_adapter { namespace agv { @@ -97,6 +101,10 @@ class RobotUpdateHandle /// value that was given to the setter. rmf_utils::optional maximum_delay() const; + /// Get the schedule participant of this robot + rmf_utils::optional> + get_participant(); + class Implementation; private: RobotUpdateHandle(); diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index f37cb4315..fd1a32100 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -38,6 +38,12 @@ // the information provided by fleet drivers. #include "../rmf_fleet_adapter/estimation.hpp" +// Public rmf_traffic API headers +#include +#include + +#include + //============================================================================== class FleetDriverRobotCommandHandle : public rmf_fleet_adapter::agv::RobotCommandHandle @@ -260,10 +266,10 @@ class FleetDriverRobotCommandHandle } else if (_dock_finished_callback) { + const auto now = std::chrono::steady_clock::now(); // If we have a _dock_finished_callback, then the robot should be docking if (state.task_id != _current_dock_request.task_id) { - const auto now = std::chrono::steady_clock::now(); if (std::chrono::milliseconds(200) < now - _dock_requested_time) { // We published the request a while ago, so we'll send it again in @@ -281,6 +287,33 @@ class FleetDriverRobotCommandHandle _travel_info.last_known_wp = *_dock_target_wp; _dock_finished_callback(); _dock_finished_callback = nullptr; + + return; + } + + // Update the schedule with the docking path of the robot + if (!state.path.empty() && + std::chrono::seconds(1) < now - _dock_schedule_time) + { + std::vector positions; + positions.push_back( + {state.location.x, state.location.y, state.location.yaw}); + for (const auto& p : state.path) + positions.push_back({p.x, p.y, p.yaw}); + + const rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions( + *_travel_info.traits, + rmf_traffic_ros2::convert(state.location.t), + positions); + assert(trajectory.size() > 1); + + if (auto participant = _travel_info.updater->get_participant()) + { + participant.value().get().set( + {rmf_traffic::Route{state.location.level_name, trajectory}}); + _dock_schedule_time = now; + } } } else @@ -309,6 +342,8 @@ class FleetDriverRobotCommandHandle rmf_fleet_msgs::msg::ModeRequest _current_dock_request; rmf_utils::optional _dock_target_wp; std::chrono::steady_clock::time_point _dock_requested_time; + std::chrono::steady_clock::time_point _dock_schedule_time = + std::chrono::steady_clock::now(); RequestCompleted _dock_finished_callback; ModeRequestPub _mode_request_pub; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index d9ba4b7f8..228bfb6cf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -188,6 +188,18 @@ RobotUpdateHandle::maximum_delay() const return rmf_utils::nullopt; } +//============================================================================== +rmf_utils::optional> + RobotUpdateHandle::get_participant() +{ + if (const auto context = _pimpl->get_context()) + { + auto& itinerary = context->itinerary(); + return itinerary; + } + return rmf_utils::nullopt; +} + //============================================================================== RobotUpdateHandle::RobotUpdateHandle() { From dff67856abc461b48f78b6a2a2a3174746002ad8 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 17 Oct 2020 14:01:39 +0800 Subject: [PATCH 053/128] Replaced task_id with request in Assignment --- rmf_task/include/rmf_task/Request.hpp | 3 +++ rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 10 +++---- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 27 ++++++++++--------- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 2 +- 4 files changed, 23 insertions(+), 19 deletions(-) diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 1c24b6ee2..19a389176 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -54,6 +54,9 @@ class Request virtual ~Request() = default; }; +using RequestPtr = std::shared_ptr; +using ConstRequestPtr = std::shared_ptr; + } // namespace rmf_task #endif // INCLUDE__RMF_TASK__TASK_HPP diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 06c603207..362288e2f 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -86,8 +86,8 @@ class TaskPlanner /// Constructor /// - /// \param[in] task_id - /// The task id for this assignment + /// \param[in] request + /// The task request for this assignment /// /// \param[in] state /// The state of the agent at the end of the assigned task @@ -95,13 +95,13 @@ class TaskPlanner /// \param[in] earliest_start_time /// The earliest time the agent will begin exececuting this task Assignment( - std::size_t task_id, + rmf_task::RequestPtr request, State state, rmf_traffic::Time earliest_start_time); - // Get a const reference to the task_id - std::size_t task_id() const; + // Get the request of this task + rmf_task::RequestPtr request() const; // Get a const reference to the state const State& state() const; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 2e9f7c262..7c0ee33f8 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -89,19 +89,19 @@ class TaskPlanner::Assignment::Implementation { public: - std::size_t task_id; + rmf_task::RequestPtr request; State state; rmf_traffic::Time earliest_start_time; }; //============================================================================== TaskPlanner::Assignment::Assignment( - std::size_t task_id, + rmf_task::RequestPtr request, State state, rmf_traffic::Time earliest_start_time) : _pimpl(rmf_utils::make_impl( Implementation{ - task_id, + std::move(request), std::move(state), earliest_start_time })) @@ -110,9 +110,9 @@ TaskPlanner::Assignment::Assignment( } //============================================================================== -std::size_t TaskPlanner::Assignment::task_id() const +rmf_task::RequestPtr TaskPlanner::Assignment::request() const { - return _pimpl->task_id; + return _pimpl->request; } //============================================================================== @@ -477,7 +477,7 @@ class Filter { // We add 1 to the task_id to differentiate between task_id == 0 and // a task being unassigned. - const std::size_t id = s.task_id() + 1; + const std::size_t id = s.request()->id() + 1; output += id << (_shift * (count++)); } } @@ -506,7 +506,7 @@ class Filter for (std::size_t j=0; j < a.size(); ++j) { - if (a[j].task_id() != b[j].task_id()) + if (a[j].request()->id() != b[j].request()->id()) return false; } } @@ -543,7 +543,7 @@ bool Filter::ignore(const Node& node) if (t < current_agent.size()) { - const auto& task_id = current_agent[t].task_id(); + const auto& task_id = current_agent[t].request()->id(); const auto agent_insertion = agent_table->agent.insert({a, nullptr}); if (agent_insertion.second) agent_insertion.first->second = std::make_unique(); @@ -606,7 +606,8 @@ class TaskPlanner::Implementation continue; // Remove charging task at end of assignments if any - if (assignments[a].back().task_id() == config->charge_battery_request()->id()) + if (assignments[a].back().request()->id() == + config->charge_battery_request()->id()) assignments[a].pop_back(); } @@ -834,7 +835,7 @@ class TaskPlanner::Implementation // Assign the unassigned task new_node->assigned_tasks[entry.candidate].push_back( - Assignment{u.first, entry.state, u.second.earliest_start_time}); + Assignment{u.second.request, entry.state, u.second.earliest_start_time}); // Erase the assigned task from unassigned tasks new_node->pop_unassigned(u.first); @@ -891,7 +892,7 @@ class TaskPlanner::Implementation new_node->assigned_tasks[entry.candidate].push_back( Assignment { - config->charge_battery_request()->id(), + config->charge_battery_request(), battery_estimate.value().finish_state(), battery_estimate.value().wait_until() }); @@ -947,7 +948,7 @@ class TaskPlanner::Implementation if (!assignments.empty()) { - if (assignments.back().task_id() == config->charge_battery_request()->id()) + if (assignments.back().request()->id() == config->charge_battery_request()->id()) return nullptr; state = assignments.back().state(); } @@ -958,7 +959,7 @@ class TaskPlanner::Implementation { new_node->assigned_tasks[agent].push_back( Assignment{ - config->charge_battery_request()->id(), + config->charge_battery_request(), estimate.value().finish_state(), estimate.value().wait_until()}); diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index c1a17f4ee..9d185fdcd 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -57,7 +57,7 @@ inline void display_solution( const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9; const rmf_traffic::Time finish_time = s.finish_time(); const double finish_seconds = finish_time.time_since_epoch().count()/1e9; - std::cout << " <" << a.task_id() << ": " << start_seconds + std::cout << " <" << a.request()->id() << ": " << start_seconds << ", "<< finish_seconds << ", " << 100* s.battery_soc() << "%>" << std::endl; } From d9e378e340d245bc22992094a72ce0b3da514c9a Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 19 Oct 2020 15:18:43 +0800 Subject: [PATCH 054/128] Renamed earliest_start_time to deployment_time in assignment. Modified expand_candidate. --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 7 ++++--- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 14 +++++++------- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 16 ++++------------ 3 files changed, 15 insertions(+), 22 deletions(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 362288e2f..3e0d74a43 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -97,7 +97,7 @@ class TaskPlanner Assignment( rmf_task::RequestPtr request, State state, - rmf_traffic::Time earliest_start_time); + rmf_traffic::Time deployment_time); // Get the request of this task @@ -106,8 +106,9 @@ class TaskPlanner // Get a const reference to the state const State& state() const; - // Get a const reference to the earliest start time - const rmf_traffic::Time& earliest_start_time() const; + // Get a const reference to the time when the robot begins executing + // this assignment + const rmf_traffic::Time& deployment_time() const; class Implementation; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 7c0ee33f8..caa005fdc 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -91,19 +91,19 @@ class TaskPlanner::Assignment::Implementation rmf_task::RequestPtr request; State state; - rmf_traffic::Time earliest_start_time; + rmf_traffic::Time deployment_time; }; //============================================================================== TaskPlanner::Assignment::Assignment( rmf_task::RequestPtr request, State state, - rmf_traffic::Time earliest_start_time) + rmf_traffic::Time deployment_time) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(request), std::move(state), - earliest_start_time + deployment_time })) { // Do nothing @@ -122,9 +122,9 @@ const State& TaskPlanner::Assignment::state() const } //============================================================================== -const rmf_traffic::Time& TaskPlanner::Assignment::earliest_start_time() const +const rmf_traffic::Time& TaskPlanner::Assignment::deployment_time() const { - return _pimpl->earliest_start_time; + return _pimpl->deployment_time; } //============================================================================== @@ -591,7 +591,7 @@ class TaskPlanner::Implementation { cost += rmf_traffic::time::to_seconds( - assignment.state().finish_time() - assignment.earliest_start_time()); + assignment.state().finish_time() - assignment.deployment_time()); } } @@ -835,7 +835,7 @@ class TaskPlanner::Implementation // Assign the unassigned task new_node->assigned_tasks[entry.candidate].push_back( - Assignment{u.second.request, entry.state, u.second.earliest_start_time}); + Assignment{u.second.request, entry.state, entry.wait_until}); // Erase the assigned task from unassigned tasks new_node->pop_unassigned(u.first); diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 9d185fdcd..7c3185bb3 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -54,10 +54,12 @@ inline void display_solution( for (const auto& a : assignments[i]) { const auto& s = a.state(); - const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9; + const double request_seconds = a.request()->earliest_start_time().time_since_epoch().count()/1e9; + const double start_seconds = a.deployment_time().time_since_epoch().count()/1e9; const rmf_traffic::Time finish_time = s.finish_time(); const double finish_seconds = finish_time.time_since_epoch().count()/1e9; - std::cout << " <" << a.request()->id() << ": " << start_seconds + std::cout << " <" << a.request()->id() << ": " << request_seconds + << ", " << start_seconds << ", "<< finish_seconds << ", " << 100* s.battery_soc() << "%>" << std::endl; } @@ -257,16 +259,6 @@ SCENARIO("Grid World") now + rmf_traffic::time::from_seconds(0), drain_battery), - rmf_task::requests::Delivery::make( - 3, - 7, - 9, - motion_sink, - device_sink, - planner, - now + rmf_traffic::time::from_seconds(0), - drain_battery), - rmf_task::requests::Delivery::make( 4, 8, From b1f4dff7443a61c656f9b4cd9aa099709b23cb72 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 19 Oct 2020 17:47:37 +0800 Subject: [PATCH 055/128] TaskPlanner::Configuration no longer takes in a charging request. The factory is used to construct a charging request. --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 41 ++++++++-- .../rmf_task/requests/ChargeBattery.hpp | 3 + rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 76 +++++++++++++++---- .../src/rmf_task/requests/ChargeBattery.cpp | 5 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 18 +++-- 5 files changed, 111 insertions(+), 32 deletions(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 3e0d74a43..0232c65ab 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -22,6 +22,12 @@ #include #include +#include +#include +#include + +#include + #include #include @@ -53,20 +59,41 @@ class TaskPlanner public: /// Constructor /// - /// \param[in] charge_battery_request - /// A pointer to the ChargeBattery request for this AGV + /// \param[in] battery_system + /// The battery system of the robot + /// + /// \param[in] motion_sink + /// The motion sink of the robot + /// + /// \param[in] device_sink + /// The ambient device sink of the robot + /// + /// \param[in] planner + /// The planner for a robot in this fleet /// /// \param[in] filter_type /// The type of filter used for planning Configuration( - Request::SharedPtr charge_battery_request, + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner, FilterType filter_type= FilterType::Hash); - /// Get the pointer to the ChargeBattery request - Request::SharedPtr charge_battery_request() const; + /// Get the battery system + rmf_battery::agv::BatterySystem& battery_system(); + + /// Set the battery_system + Configuration& battery_system(rmf_battery::agv::BatterySystem battery_system); + + /// Get the motion sink + std::shared_ptr motion_sink() const; + + /// Get the ambient device sink + std::shared_ptr ambient_sink() const; - /// Set the pointer to the ChargeBattery request - Configuration& charge_battery_request(Request::SharedPtr charge_battery); + /// Get the planner + std::shared_ptr planner() const; /// Get the filter type FilterType filter_type() const; diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index af08ceead..959cae7f6 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -43,6 +43,7 @@ class ChargeBattery : public rmf_task::Request std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, + rmf_traffic::Time start_time, bool drain_battery = true); std::size_t id() const final; @@ -61,6 +62,8 @@ class ChargeBattery : public rmf_task::Request rmf_utils::impl_ptr _pimpl; }; +using ChargeBatteryPtr = std::shared_ptr; + } // namespace requests } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index caa005fdc..da34eabbf 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -40,16 +40,26 @@ class TaskPlanner::Configuration::Implementation { public: - Request::SharedPtr charge_battery_request; + rmf_battery::agv::BatterySystem battery_system; + std::shared_ptr motion_sink; + std::shared_ptr ambient_sink; + std::shared_ptr planner; FilterType filter_type; + }; TaskPlanner::Configuration::Configuration( - Request::SharedPtr charge_battery_request, + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner, const FilterType filter_type) : _pimpl(rmf_utils::make_impl( Implementation{ - std::move(charge_battery_request), + battery_system, + std::move(motion_sink), + std::move(ambient_sink), + std::move(planner), filter_type })) { @@ -57,19 +67,37 @@ TaskPlanner::Configuration::Configuration( } //============================================================================== -Request::SharedPtr TaskPlanner::Configuration::charge_battery_request() const +rmf_battery::agv::BatterySystem& TaskPlanner::Configuration::battery_system() { - return std::move(_pimpl->charge_battery_request); + return _pimpl->battery_system; } //============================================================================== -auto TaskPlanner::Configuration::charge_battery_request( - Request::SharedPtr charge_battery_request) -> Configuration& +auto TaskPlanner::Configuration::battery_system( + rmf_battery::agv::BatterySystem battery_system) -> Configuration& { - _pimpl->charge_battery_request = std::move(charge_battery_request); + _pimpl->battery_system = battery_system; return *this; } +//============================================================================== +std::shared_ptr TaskPlanner::Configuration::motion_sink() const +{ + return _pimpl->motion_sink; +} + +//============================================================================== +std::shared_ptr TaskPlanner::Configuration::ambient_sink() const +{ + return _pimpl->ambient_sink; +} + +//============================================================================== +std::shared_ptr TaskPlanner::Configuration::planner() const +{ + return _pimpl->planner; +} + //============================================================================== TaskPlanner::FilterType TaskPlanner::Configuration::filter_type() const { @@ -582,6 +610,17 @@ class TaskPlanner::Implementation std::shared_ptr config; + RequestPtr make_charging_request(rmf_traffic::Time start_time) + { + return rmf_task::requests::ChargeBattery::make( + config->battery_system(), + config->motion_sink(), + config->ambient_sink(), + config->planner(), + start_time, + true); + } + double compute_g(const Assignments& assigned_tasks) { double cost = 0.0; @@ -606,8 +645,9 @@ class TaskPlanner::Implementation continue; // Remove charging task at end of assignments if any - if (assignments[a].back().request()->id() == - config->charge_battery_request()->id()) + // TODO(YV): Remove this after fixing the planner + if (std::dynamic_pointer_cast( + assignments[a].back().request())) assignments[a].pop_back(); } @@ -755,11 +795,13 @@ class TaskPlanner::Implementation initial_node->assigned_tasks.resize(initial_states.size()); + // TODO(YV): Come up with a better solution for charge_battery_request + auto charge_battery = make_charging_request(time_now); for (const auto& request : requests) initial_node->unassigned_tasks.insert( { request->id(), - PendingTask(initial_states, state_configs, request, config->charge_battery_request()) + PendingTask(initial_states, state_configs, request, charge_battery) }); initial_node->cost_estimate = compute_f(*initial_node, time_now); @@ -886,13 +928,14 @@ class TaskPlanner::Implementation if (add_charger) { - auto battery_estimate = config->charge_battery_request()->estimate_finish(entry.state, state_config); + auto charge_battery = make_charging_request(entry.state.finish_time()); + auto battery_estimate = charge_battery->estimate_finish(entry.state, state_config); if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( Assignment { - config->charge_battery_request(), + charge_battery, battery_estimate.value().finish_state(), battery_estimate.value().wait_until() }); @@ -948,18 +991,19 @@ class TaskPlanner::Implementation if (!assignments.empty()) { - if (assignments.back().request()->id() == config->charge_battery_request()->id()) + if (std::dynamic_pointer_cast(assignments.back().request())) return nullptr; state = assignments.back().state(); } - auto estimate = config->charge_battery_request()->estimate_finish( + auto charge_battery = make_charging_request(state.finish_time()); + auto estimate = charge_battery->estimate_finish( state, state_configs[agent]); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( Assignment{ - config->charge_battery_request(), + charge_battery, estimate.value().finish_state(), estimate.value().wait_until()}); diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ae29907d3..1d34fb9e2 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -34,6 +34,7 @@ class ChargeBattery::Implementation std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; + rmf_traffic::Time _start_time; bool _drain_battery; // soc to always charge the battery up to @@ -47,6 +48,7 @@ rmf_task::Request::SharedPtr ChargeBattery::make( std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, + rmf_traffic::Time start_time, bool drain_battery) { std::shared_ptr charge_battery(new ChargeBattery()); @@ -60,6 +62,7 @@ rmf_task::Request::SharedPtr ChargeBattery::make( charge_battery->_pimpl->_motion_sink = std::move(motion_sink); charge_battery->_pimpl->_device_sink = std::move(device_sink); charge_battery->_pimpl->_planner = std::move(planner); + charge_battery->_pimpl->_start_time = start_time; charge_battery->_pimpl->_drain_battery = drain_battery; charge_battery->_pimpl->_invariant_duration = rmf_traffic::time::from_seconds(0.0); @@ -154,7 +157,7 @@ rmf_traffic::Duration ChargeBattery::invariant_duration() const //============================================================================== rmf_traffic::Time ChargeBattery::earliest_start_time() const { - return rmf_traffic::Time::min(); + return _pimpl->_start_time; } //============================================================================== diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 7c3185bb3..360d51a6c 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -130,12 +130,6 @@ SCENARIO("Grid World") std::shared_ptr device_sink = std::make_shared(battery_system, power_system); - auto charge_battery_task = rmf_task::requests::ChargeBattery::make( - battery_system, - motion_sink, device_sink, - planner, - drain_battery); - WHEN("Planning for 3 requests and 2 agents") { const auto now = std::chrono::steady_clock::now(); @@ -190,7 +184,11 @@ SCENARIO("Grid World") }; std::shared_ptr task_config = - std::make_shared(charge_battery_task); + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); rmf_task::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( @@ -341,7 +339,11 @@ SCENARIO("Grid World") }; std::shared_ptr task_config = - std::make_shared(charge_battery_task); + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); rmf_task::agv::TaskPlanner task_planner(task_config); const auto greedy_assignments = task_planner.greedy_plan( From 9006a4b40a3357b9a0149b55994a9cbfb803749b Mon Sep 17 00:00:00 2001 From: youliang Date: Mon, 19 Oct 2020 18:08:07 +0800 Subject: [PATCH 056/128] update msgs for dispatcher (#193) --- rmf_task_msgs/CMakeLists.txt | 13 +++++++++++++ rmf_task_msgs/msg/BidNotice.msg | 7 +++++++ rmf_task_msgs/msg/BidProposal.msg | 16 ++++++++++++++++ rmf_task_msgs/msg/DispatchRequest.msg | 13 +++++++++++++ rmf_task_msgs/msg/TaskProfile.msg | 14 ++++++++++++++ rmf_task_msgs/msg/TaskSummary.msg | 20 ++++++++++++++++++-- rmf_task_msgs/msg/TaskType.msg | 9 +++++++++ rmf_task_msgs/srv/CancelTask.srv | 12 ++++++++++++ rmf_task_msgs/srv/GetTask.srv | 15 +++++++++++++++ rmf_task_msgs/srv/SubmitTask.srv | 21 +++++++++++++++++++++ 10 files changed, 138 insertions(+), 2 deletions(-) create mode 100644 rmf_task_msgs/msg/BidNotice.msg create mode 100644 rmf_task_msgs/msg/BidProposal.msg create mode 100644 rmf_task_msgs/msg/DispatchRequest.msg create mode 100644 rmf_task_msgs/msg/TaskProfile.msg create mode 100644 rmf_task_msgs/msg/TaskType.msg create mode 100644 rmf_task_msgs/srv/CancelTask.srv create mode 100644 rmf_task_msgs/srv/GetTask.srv create mode 100644 rmf_task_msgs/srv/SubmitTask.srv diff --git a/rmf_task_msgs/CMakeLists.txt b/rmf_task_msgs/CMakeLists.txt index a0e914124..498eceeab 100644 --- a/rmf_task_msgs/CMakeLists.txt +++ b/rmf_task_msgs/CMakeLists.txt @@ -25,9 +25,22 @@ set(msg_files "msg/TaskSummary.msg" "msg/Tasks.msg" "msg/Loop.msg" + # New task Msgs + "msg/TaskType.msg" + "msg/TaskProfile.msg" + "msg/BidNotice.msg" + "msg/BidProposal.msg" + "msg/DispatchRequest.msg" ) +set(srv_files + "srv/SubmitTask.srv" + "srv/CancelTask.srv" + "srv/GetTask.srv" +) + rosidl_generate_interfaces(${PROJECT_NAME} + ${srv_files} ${msg_files} DEPENDENCIES builtin_interfaces diff --git a/rmf_task_msgs/msg/BidNotice.msg b/rmf_task_msgs/msg/BidNotice.msg new file mode 100644 index 000000000..2dce6da91 --- /dev/null +++ b/rmf_task_msgs/msg/BidNotice.msg @@ -0,0 +1,7 @@ +# This msg is will be received by Fleet Adapter + +# task to bid +TaskProfile task_profile + +# Duration which bidding is open +builtin_interfaces/Duration time_window diff --git a/rmf_task_msgs/msg/BidProposal.msg b/rmf_task_msgs/msg/BidProposal.msg new file mode 100644 index 000000000..8de9bf840 --- /dev/null +++ b/rmf_task_msgs/msg/BidProposal.msg @@ -0,0 +1,16 @@ +# This msg is generated by Fleet Adpater + +string fleet_name + +# task to bid, same as the one from BidNotice +TaskProfile task_profile + +# Cost of the fleet before and after adding the new task +float64 prev_cost +float64 new_cost + +# estimated finish time of the given task +builtin_interfaces/Time finish_time + +# Selected robot (optional) +string robot_name diff --git a/rmf_task_msgs/msg/DispatchRequest.msg b/rmf_task_msgs/msg/DispatchRequest.msg new file mode 100644 index 000000000..8a239193a --- /dev/null +++ b/rmf_task_msgs/msg/DispatchRequest.msg @@ -0,0 +1,13 @@ +# Publish by Task Dispatcher Node +# This msg is will be received by a Fleet Adapter to initiate/cancel a task + +# Selected Fleet Adapter to execute/cancel the task +string fleet_name + +# Task to dispatch (should be the same as in BidNotice) +TaskProfile task_profile + +# add or cancel a task +uint8 method +uint8 ADD=1 # to add a task +uint8 CANCEL=2 # to cancel and remove a task diff --git a/rmf_task_msgs/msg/TaskProfile.msg b/rmf_task_msgs/msg/TaskProfile.msg new file mode 100644 index 000000000..b0183890c --- /dev/null +++ b/rmf_task_msgs/msg/TaskProfile.msg @@ -0,0 +1,14 @@ +# Unique ID to differentiate the task +string task_id + +# Task submission time +builtin_interfaces/Time submission_time + +# Desired start time of a task +builtin_interfaces/Time start_time + +# e.g. Delivery, Station..... +TaskType type + +# Params +BehaviorParameter[] params diff --git a/rmf_task_msgs/msg/TaskSummary.msg b/rmf_task_msgs/msg/TaskSummary.msg index ac45433db..bb75e7b38 100644 --- a/rmf_task_msgs/msg/TaskSummary.msg +++ b/rmf_task_msgs/msg/TaskSummary.msg @@ -1,16 +1,28 @@ -string task_id +# Publish by Fleet Adapter (aka DispatchStatus) + +# Fleet Adapter name +string fleet_name + +# *optional and duplicated in TaskProfile +string task_id + +TaskProfile task_profile uint32 state uint32 STATE_QUEUED=0 uint32 STATE_ACTIVE=1 uint32 STATE_COMPLETED=2 # hooray uint32 STATE_FAILED=3 # oh no +uint32 STATE_CANCELED=4 +uint32 STATE_PENDING=5 # a brief summary of the current status of the task, for UI's +# *optional string status # submission_time is when the task was submitted to rmf_core -builtin_interfaces/Time submission_time +# *optional and duplicated in TaskProfile +builtin_interfaces/Time submission_time # when rmf_core actually began processing the task builtin_interfaces/Time start_time @@ -19,3 +31,7 @@ builtin_interfaces/Time start_time # an estimate. When this message is a summary of a completed or failed task, # end_time is the actual time. builtin_interfaces/Time end_time + +# Allocated robot name +# *optional +string robot_name diff --git a/rmf_task_msgs/msg/TaskType.msg b/rmf_task_msgs/msg/TaskType.msg new file mode 100644 index 000000000..e7acd39a0 --- /dev/null +++ b/rmf_task_msgs/msg/TaskType.msg @@ -0,0 +1,9 @@ +# type of task +uint8 value + +uint8 STATION_TASK=0 +uint8 LOOP_TASK=1 +uint8 DELIVERY_TASK=2 +uint8 CHARGING_TASK=3 +uint8 CLEANING_TASK=4 +uint8 PATROL_TASK=5 diff --git a/rmf_task_msgs/srv/CancelTask.srv b/rmf_task_msgs/srv/CancelTask.srv new file mode 100644 index 000000000..9b3dd88aa --- /dev/null +++ b/rmf_task_msgs/srv/CancelTask.srv @@ -0,0 +1,12 @@ +# Submit Task | "POST" service call + +# Whom that call this srv +string requester + +# generated task ID by dispatcher node +string task_id + +--- + +# Confirmation that this service call is processed +bool success diff --git a/rmf_task_msgs/srv/GetTask.srv b/rmf_task_msgs/srv/GetTask.srv new file mode 100644 index 000000000..ce6e1fbbf --- /dev/null +++ b/rmf_task_msgs/srv/GetTask.srv @@ -0,0 +1,15 @@ +# Query list of submitted tasks | Get service call + +# Whom that call this srv +string requester + +# Input the generated task ID during submission +# if empty, provide all Submitted Tasks +string[] task_id + +--- + +bool success + +TaskSummary[] active_tasks +TaskSummary[] terminated_tasks diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv new file mode 100644 index 000000000..914aa016b --- /dev/null +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -0,0 +1,21 @@ +# Submit Task | POST service call + +# Whom that call this srv +string requester + +# Desired start time of a task +builtin_interfaces/Time start_time + +# e.g. Delivery, Station..... +TaskType type + +# Params +BehaviorParameter[] params + +--- + +# Confirmation that this service call is processed +bool success + +# generated task ID by dispatcher node +string task_id From 6da2d9f06f73f02863ee6e2fb879d3ad9ff6462e Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 19 Oct 2020 18:15:46 +0800 Subject: [PATCH 057/128] Fixed dynamic cast --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index da34eabbf..d882648e0 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -630,7 +630,7 @@ class TaskPlanner::Implementation { cost += rmf_traffic::time::to_seconds( - assignment.state().finish_time() - assignment.deployment_time()); + assignment.state().finish_time() - assignment.request()->earliest_start_time()); } } @@ -646,9 +646,9 @@ class TaskPlanner::Implementation // Remove charging task at end of assignments if any // TODO(YV): Remove this after fixing the planner - if (std::dynamic_pointer_cast( + if (std::dynamic_pointer_cast( assignments[a].back().request())) - assignments[a].pop_back(); + assignments[a].pop_back(); } return assignments; @@ -991,7 +991,8 @@ class TaskPlanner::Implementation if (!assignments.empty()) { - if (std::dynamic_pointer_cast(assignments.back().request())) + if (std::dynamic_pointer_cast( + assignments.back().request())) return nullptr; state = assignments.back().state(); } From a4c2182476ed8c841980a73e7acb7585ef250d21 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 27 Oct 2020 13:45:18 +0800 Subject: [PATCH 058/128] Using magnitude of w and alpha in compute_change_in_charge() --- .../rmf_battery/agv/SimpleMotionPowerSink.cpp | 4 +- .../test/unit/agv/test_battery_drain.cpp | 53 +++++++++++++++++++ 2 files changed, 55 insertions(+), 2 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 1a1ad3aa6..1aa8d239c 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -105,11 +105,11 @@ double SimpleMotionPowerSink::compute_change_in_charge( { const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - const double w = velocity[2]; + const double w = std::abs(velocity[2]); const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time); const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2)); - const double alpha = acceleration[2]; + const double alpha = std::abs(acceleration[2]); // Loss through acceleration const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 928037d71..65ca402f3 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -241,3 +241,56 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") CHECK(ok); } } + +SCENARIO("Testing Cleaning Request") +{ + 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; + using namespace std::chrono_literals; + + // Initializing system traits + BatterySystem battery_system{24, 40, 8.8}; + REQUIRE(battery_system.valid()); + MechanicalSystem mechanical_system{70, 40, 0.22}; + REQUIRE(mechanical_system.valid()); + PowerSystem power_system_1{"processor", 20}; + REQUIRE(power_system_1.valid()); + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {0.7, 0.5}, {0.4, 1.0}, {nullptr, nullptr}); + + const double initial_soc = 1.0; + + WHEN("Computing invariant drain for zone_3") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{104.0, -46.92, -M_PI/2.0}, + Eigen::Vector3d{104.0, -48.55, 0.0}, + Eigen::Vector3d{159.8, -48.38, M_PI/2.0}, + Eigen::Vector3d{159.8, -46.73, M_PI}, + Eigen::Vector3d{105.4, -47.04, M_PI/2.0}, + Eigen::Vector3d{105.5, -45.37, 0.0}, + Eigen::Vector3d{159.8, -45.25, 0.0}, + Eigen::Vector3d{155.0, -46.79, -M_PI/2.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); + + // std::cout << "Motion: " << dSOC_motion << "Device: " << dSOC_device << std::endl; + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + REQUIRE(remaining_soc <= 1.0); + } +} \ No newline at end of file From 9cb4a3184bc3998ba0c40295f0f093f2c522dffc Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Tue, 27 Oct 2020 19:13:06 +0800 Subject: [PATCH 059/128] Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. --- rmf_task/src/rmf_task/requests/ChargeBattery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 1d34fb9e2..fa8fdb680 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -116,7 +116,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( const auto result = _pimpl->_planner->plan(start, goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration variant_duration = finish_time - start_time; + variant_duration = finish_time - start_time; if (_pimpl->_drain_battery) { From fb937cb25cee894d625f92701626ca330316d8b5 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 4 Nov 2020 14:17:49 +0800 Subject: [PATCH 060/128] Added new Clean task, changed TaskProfile and SubmitTask to use explicit message definitions (#200) * Added new Clean task, changed TaskProfile and SubmitTask to use enum for type and explicit message definitions * Reverted to use TaskType, modified value type to uint32 --- rmf_task_msgs/CMakeLists.txt | 1 + rmf_task_msgs/msg/Clean.msg | 2 ++ rmf_task_msgs/msg/TaskProfile.msg | 14 ++++++++++---- rmf_task_msgs/msg/TaskType.msg | 15 +++++++-------- rmf_task_msgs/srv/SubmitTask.srv | 13 +++++++++---- 5 files changed, 29 insertions(+), 16 deletions(-) create mode 100644 rmf_task_msgs/msg/Clean.msg diff --git a/rmf_task_msgs/CMakeLists.txt b/rmf_task_msgs/CMakeLists.txt index 498eceeab..49f6f43cc 100644 --- a/rmf_task_msgs/CMakeLists.txt +++ b/rmf_task_msgs/CMakeLists.txt @@ -27,6 +27,7 @@ set(msg_files "msg/Loop.msg" # New task Msgs "msg/TaskType.msg" + "msg/Clean.msg" "msg/TaskProfile.msg" "msg/BidNotice.msg" "msg/BidProposal.msg" diff --git a/rmf_task_msgs/msg/Clean.msg b/rmf_task_msgs/msg/Clean.msg new file mode 100644 index 000000000..5c60c9b74 --- /dev/null +++ b/rmf_task_msgs/msg/Clean.msg @@ -0,0 +1,2 @@ +string start_waypoint + diff --git a/rmf_task_msgs/msg/TaskProfile.msg b/rmf_task_msgs/msg/TaskProfile.msg index b0183890c..571474f2d 100644 --- a/rmf_task_msgs/msg/TaskProfile.msg +++ b/rmf_task_msgs/msg/TaskProfile.msg @@ -7,8 +7,14 @@ builtin_interfaces/Time submission_time # Desired start time of a task builtin_interfaces/Time start_time -# e.g. Delivery, Station..... -TaskType type +# Task type +TaskType task_type + +# Task definitions +Station station +Loop loop +Delivery delivery +# Charge charge +Clean clean +# Patrol patrol -# Params -BehaviorParameter[] params diff --git a/rmf_task_msgs/msg/TaskType.msg b/rmf_task_msgs/msg/TaskType.msg index e7acd39a0..ac3733bbe 100644 --- a/rmf_task_msgs/msg/TaskType.msg +++ b/rmf_task_msgs/msg/TaskType.msg @@ -1,9 +1,8 @@ -# type of task -uint8 value +uint32 type +uint32 TYPE_STATION=0 +uint32 TYPE_LOOP=1 +uint32 TYPE_DELIVERY=2 +uint32 TYPE_CHARGING=3 +uint32 TYPE_CLEANING=4 +uint32 TYPE_PATROL=5 -uint8 STATION_TASK=0 -uint8 LOOP_TASK=1 -uint8 DELIVERY_TASK=2 -uint8 CHARGING_TASK=3 -uint8 CLEANING_TASK=4 -uint8 PATROL_TASK=5 diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv index 914aa016b..d17232a2e 100644 --- a/rmf_task_msgs/srv/SubmitTask.srv +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -6,11 +6,16 @@ string requester # Desired start time of a task builtin_interfaces/Time start_time -# e.g. Delivery, Station..... -TaskType type +# Task type +TaskType task_type -# Params -BehaviorParameter[] params +# Task definitions +Station station +Loop loop +Delivery delivery +# Charge charge +Clean clean +# Patrol patrol --- From d6186c31dccacb52c3b109dec0ac61f7df190d9b Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 5 Nov 2020 10:30:44 +0800 Subject: [PATCH 061/128] add eval enum to submit task srv (#201) --- rmf_task_msgs/srv/SubmitTask.srv | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv index d17232a2e..fc39faf92 100644 --- a/rmf_task_msgs/srv/SubmitTask.srv +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -17,6 +17,12 @@ Delivery delivery Clean clean # Patrol patrol +# fleet selection evaluator +uint8 evaluator +uint8 LOWEST_DIFF_COST_EVAL=0 +uint8 LOWEST_COST_EVAL=1 +uint8 QUICKEST_FINISH_EVAL=2 + --- # Confirmation that this service call is processed From 69c9b1d2b5dfaf168b202df339b64a4ea21443fd Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 5 Nov 2020 15:39:14 +0800 Subject: [PATCH 062/128] Develop/task planner integration (#199) * Added set_queue and requests methods to TaskManager * Added make_clean to convert from request to task * Commented out Delivery and Loop request subs in Adapter * Commented out Delivery and Loop related code in FleetUpdateHandle * Updated Adapter::add_fleet() * Undid changes to add_fleet which broke API/ABI. Added set_task_planner_params() to FleetUpdateHandle * Added accept_task_request to FleetUpdateHandle * Added BidNotice callback * Updated bidding topic names and started processing clean task request * Added dock related messages to rmf_fleet_msgs. Finished processing clean request. * Moved BidNotice callback into FleetUpdateHandle.cpp * RobotContext constructor taskes in State and StateConfig which are used for task planning * Added is_charger() and set_charger() methods to Waypoint API * add_robot uses get_nearest_charger() * parse_graph parses is_charger option * Added state_config() to RobotContext * Added dispatch_request_cb() * Updated set_queue() and make_clean() functions. Switching to using ConstRequestPtr everywhere * Broke Task constructor API * Added timer to TaskManager * Locking mutex when modifying task queue * Dispatcher pipeline works! * Cleaned up printouts * Update finish time after robot has completed task * Get latest state of robots before running task planner. TODO: Update battery soc * Fixed bug in compute_change_of_charge() * Using magnitude of w and alpha in compute_change_in_charge() * Added update_battery_soc() to RobotUpdateHandle which updates the current_battery_soc in RobotContext. Initial states for task planning use this updated battery_soc * Added ChargeBattery task factory * Added skeleton for WaitForCharge * Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. * Fixed return value of battery_system() in ChargeBattery * Catch exception for invalid cleaning trajectories * Populate finish_time in BidProposal * Added observable and publisher for current_battery_soc * Updated begin() for WaitForCharge phase * Fixed completion logic in WaitForCharge * Fixed typo * Updated fleet adapter launch params. Populate task id in TaskProfile in Status msg * Become stubborn while docking * Added recharge_threshold parameter to fleet adapter launch * Fix missing return value in current_battery_soc() * Populate robot_name in BidProposal msg * TaskManager queues charging tasks * Modify start in set_queue() * Added second GoToPlace phase for Clean task * Added TODO to account for travel from end of cleaning trajectory to end_waypoint * Populate robot name in TaskSummary msg * Populate TaskSummary * Changed TaskType enum names and updated FleetUpdateHandle to work with new TaskProfile msg Co-authored-by: Rushyendra Maganty Co-authored-by: Michael X. Grey --- rmf_battery/CMakeLists.txt | 4 +- .../rmf_battery/agv/SimpleMotionPowerSink.cpp | 4 +- .../test/unit/agv/test_battery_drain.cpp | 53 ++ rmf_fleet_adapter/CMakeLists.txt | 8 + .../rmf_fleet_adapter/StandardNames.hpp | 6 + .../agv/FleetUpdateHandle.hpp | 66 ++ .../agv/RobotUpdateHandle.hpp | 4 + .../launch/fleet_adapter.launch.xml | 27 + rmf_fleet_adapter/package.xml | 2 + rmf_fleet_adapter/src/full_control/main.cpp | 123 ++- .../src/rmf_fleet_adapter/Task.cpp | 40 +- .../src/rmf_fleet_adapter/Task.hpp | 29 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 249 ++++- .../src/rmf_fleet_adapter/TaskManager.hpp | 26 +- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 111 ++- .../agv/FleetUpdateHandle.cpp | 887 +++++++++++++----- .../rmf_fleet_adapter/agv/RobotContext.cpp | 51 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 33 +- .../agv/RobotUpdateHandle.cpp | 16 + .../agv/internal_FleetUpdateHandle.hpp | 193 +++- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 8 + .../agv/test/MockAdapter.cpp | 4 +- .../src/rmf_fleet_adapter/load_param.cpp | 41 + .../src/rmf_fleet_adapter/load_param.hpp | 16 + .../rmf_fleet_adapter/phases/GoToPlace.cpp | 24 +- .../phases/WaitForCharge.cpp | 156 +++ .../phases/WaitForCharge.hpp | 112 +++ .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 53 ++ .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 44 + .../src/rmf_fleet_adapter/tasks/Clean.cpp | 52 + .../src/rmf_fleet_adapter/tasks/Clean.hpp | 44 + .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 132 +-- .../src/rmf_fleet_adapter/tasks/Loop.cpp | 106 +-- rmf_fleet_adapter/test/test_Task.cpp | 19 +- rmf_fleet_msgs/CMakeLists.txt | 3 + rmf_fleet_msgs/msg/Dock.msg | 2 + rmf_fleet_msgs/msg/DockParameter.msg | 8 + rmf_fleet_msgs/msg/DockSummary.msg | 1 + rmf_task/CMakeLists.txt | 4 +- rmf_task/include/rmf_task/Request.hpp | 4 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 12 +- .../rmf_task/requests/ChargeBattery.hpp | 7 +- rmf_task/include/rmf_task/requests/Clean.hpp | 14 +- .../include/rmf_task/requests/Delivery.hpp | 5 +- rmf_task/src/rmf_task/agv/State.cpp | 2 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 26 +- .../src/rmf_task/requests/ChargeBattery.cpp | 10 +- rmf_task/src/rmf_task/requests/Clean.cpp | 62 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 2 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 8 +- rmf_task_msgs/msg/TaskType.msg | 4 +- rmf_traffic/include/rmf_traffic/agv/Graph.hpp | 9 + rmf_traffic/src/rmf_traffic/agv/Graph.cpp | 15 + 53 files changed, 2386 insertions(+), 555 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp create mode 100644 rmf_fleet_msgs/msg/Dock.msg create mode 100644 rmf_fleet_msgs/msg/DockParameter.msg create mode 100644 rmf_fleet_msgs/msg/DockSummary.msg diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 1ef124999..09f650ade 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -15,8 +15,8 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") include(GNUInstallDirs) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 1a1ad3aa6..1aa8d239c 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -105,11 +105,11 @@ double SimpleMotionPowerSink::compute_change_in_charge( { const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - const double w = velocity[2]; + const double w = std::abs(velocity[2]); const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time); const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2)); - const double alpha = acceleration[2]; + const double alpha = std::abs(acceleration[2]); // Loss through acceleration const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 928037d71..6c2fbdce2 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -241,3 +241,56 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") CHECK(ok); } } + +SCENARIO("Testing Cleaning Request") +{ + 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; + using namespace std::chrono_literals; + + // Initializing system traits + BatterySystem battery_system{24, 40, 8.8}; + REQUIRE(battery_system.valid()); + MechanicalSystem mechanical_system{70, 40, 0.22}; + REQUIRE(mechanical_system.valid()); + PowerSystem power_system_1{"processor", 20}; + REQUIRE(power_system_1.valid()); + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {0.7, 0.5}, {0.4, 1.0}, {nullptr, nullptr}); + + const double initial_soc = 1.0; + + WHEN("Computing invariant drain for zone_3") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{104.0, -46.92, -M_PI/2.0}, + Eigen::Vector3d{104.0, -48.55, 0.0}, + Eigen::Vector3d{159.8, -48.38, M_PI/2.0}, + Eigen::Vector3d{159.8, -46.73, M_PI}, + Eigen::Vector3d{105.4, -47.04, M_PI/2.0}, + Eigen::Vector3d{105.5, -45.37, 0.0}, + Eigen::Vector3d{159.8, -45.25, 0.0}, + Eigen::Vector3d{155.0, -46.79, -M_PI/2.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); + + // std::cout << "Motion: " << dSOC_motion << "Device: " << dSOC_device << std::endl; + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + REQUIRE(remaining_soc <= 1.0); + } +} diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 13c014078..15b83f8ba 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -37,6 +37,8 @@ set(dep_pkgs rmf_task_msgs rmf_traffic rmf_traffic_ros2 + rmf_battery + rmf_task std_msgs ) foreach(pkg ${dep_pkgs}) @@ -58,6 +60,8 @@ add_library(rmf_fleet_adapter SHARED target_link_libraries(rmf_fleet_adapter PUBLIC rmf_traffic_ros2::rmf_traffic_ros2 + rmf_battery::rmf_battery + rmf_task::rmf_task yaml-cpp ${rmf_fleet_msgs_LIBRARIES} ${rclcpp_LIBRARIES} @@ -78,6 +82,8 @@ target_include_directories(rmf_fleet_adapter ${rmf_fleet_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ${rmf_task_msgs_INCLUDE_DIRS} + ${rmf_battery_INCLUDE_DIRS} + ${rmf_task_INCLUDE_DIRS} PRIVATE ${rmf_door_msgs_INCLUDE_DIRS} ${rmf_lift_msgs_INCLUDE_DIRS} @@ -158,11 +164,13 @@ target_link_libraries(full_control PRIVATE rmf_fleet_adapter ${rmf_fleet_msgs_LIBRARIES} + ${rmf_task_msgs_LIBRARIES} ) target_include_directories(full_control PRIVATE ${rmf_fleet_msgs_INCLUDE_DIRS} + ${rmf_task_msgs_INCLUDE_DIRS} ) # ----------------------------------------------------------------------------- diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 6859c3b09..df718cab3 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -49,6 +49,12 @@ const std::string DeliveryTopicName = "delivery_requests"; const std::string LoopRequestTopicName = "loop_requests"; const std::string TaskSummaryTopicName = "task_summaries"; +const std::string BidNoticeTopicName = "rmf_task/bid_notice"; +const std::string BidProposalTopicName = "rmf_task/bid_proposal"; +const std::string DispatchRequestTopicName = "rmf_task/dispatch_request"; + +const std::string DockSummaryTopicName = "dock_summary"; + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 4e195e75c..a12628e08 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -22,6 +22,11 @@ #include #include +#include + +#include +#include +#include namespace rmf_fleet_adapter { namespace agv { @@ -63,6 +68,64 @@ class FleetUpdateHandle : public std::enable_shared_from_this rmf_traffic::agv::Plan::StartSet start, std::function handle)> handle_cb); + /// Set the parameters required for task planning + /// + /// \param[in] battery_system + /// Specify the battery system used by the vehicles in this fleet. + /// + /// \param[in] motion_sink + /// Specify the motion sink that describes the vehicles in this fleet. + /// + /// \param[in] ambient_sink + /// Specify the device sink for ambient sensors used by the vehicles in this fleet. + /// + /// \param[in] tool_sink + /// Specify the device sink for special tools used by the vehicles in this fleet. + /// + /// \param[in] drain_battery + /// If false, battery drain will not be considered when planning for tasks. + /// As a consequence, charging tasks will not be automatically assigned to + /// vehicles in this fleet when battery levels fall below their thresholds. + /// + /// \return true if task planner parameters were successfully updated. + bool set_task_planner_params( + std::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink, + const bool drain_battery); + + + /// Set the threshold state of charge below which the robot should + /// automatically head back to its charging dock. The user is responsible to + /// set this value such that the robot is capable of reaching its nearest + /// charging station from anywhere on the map. Default value is 0.2. + /// + /// \param[in] threshold + /// The fraction of the total battery capacity + FleetUpdateHandle& set_recharge_threshold(const double threshold); + + /// A callback function that evaluates whether a fleet will accept a task + /// request + /// + /// \param[in] request + /// Information about the task request that is being considered. + /// + /// \return true to indicate that this fleet should accept the request, false + /// to reject the request. + using AcceptTaskRequest = + std::function; + + /// Provide a callback that indicates whether this fleet will accept a + /// BidNotice request. By default all requests will be rejected. + /// + /// \note The callback function that you give should ideally be non-blocking + /// and return quickly. It's meant to check whether this fleet's vehicles are + /// compatible with the requested payload, pickup, and dropoff behavior + /// settings. The path planning feasibility will be taken care of by the + /// adapter internally. + FleetUpdateHandle& accept_task_requests(AcceptTaskRequest check); + /// A callback function that evaluates whether a fleet will accept a delivery /// request. /// @@ -71,6 +134,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// /// \return true to indicate that this fleet should accept the request, false /// to reject the request. + /// + /// \note This interface will be deprecated. Use the more general + /// AcceptTaskRequest callback using AcceptDeliveryRequest = std::function; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index f0ad0937d..9008ebd3f 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -88,6 +88,10 @@ class RobotUpdateHandle const double max_merge_lane_distance = 1.0, const double min_lane_length = 1e-8); + /// Update the current battery level of the robot by specifying its state of + /// charge as a fraction of its total charge capacity + void update_battery_soc(const double battery_soc); + /// Specify how high the delay of the current itinerary can become before it /// gets interrupted and replanned. A nullopt value will allow for an /// arbitrarily long delay to build up without being interrupted. diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml index 002b4c9c4..d086b66ed 100644 --- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml +++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml @@ -24,6 +24,20 @@ + + + + + + + + + + + + + + + + @@ -50,6 +66,17 @@ + + + + + + + + + + + diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 36b8bf26d..14c570e08 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -21,6 +21,8 @@ rmf_traffic rmf_traffic_ros2 rmf_traffic_msgs + rmf_battery + rmf_task eigen yaml-cpp diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index fd1a32100..5596792a0 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -31,6 +31,10 @@ #include #include +// RMF Task messages +#include +#include + // ROS2 utilities for rmf_traffic #include @@ -42,7 +46,12 @@ #include #include +#include +#include +#include + #include +#include //============================================================================== class FleetDriverRobotCommandHandle @@ -306,7 +315,9 @@ class FleetDriverRobotCommandHandle *_travel_info.traits, rmf_traffic_ros2::convert(state.location.t), positions); - assert(trajectory.size() > 1); + + if (trajectory.size() < 2) + return; if (auto participant = _travel_info.updater->get_participant()) { @@ -322,6 +333,11 @@ class FleetDriverRobotCommandHandle // command estimate_state(_node, state.location, _travel_info); } + + // Update battery soc + const double battery_soc = state.battery_percent / 100.0; + if (battery_soc >= 0.0 && battery_soc <= 1.0) + _travel_info.updater->update_battery_soc(battery_soc); } void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) @@ -471,17 +487,122 @@ std::shared_ptr make_fleet( for (const auto& key : connections->graph->keys()) std::cout << " -- " << key.first << std::endl; + connections->fleet = adapter->add_fleet( fleet_name, *connections->traits, *connections->graph); + // Parameters required for task planner + // Battery system + auto battery_system = std::make_shared( + rmf_fleet_adapter::get_battery_system(*node, 24.0, 40.0, 8.8)); + if (!battery_system->valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for battery system"); + + return nullptr; + } + + // Mechanical system and motion_sink + auto mechanical_system = rmf_fleet_adapter::get_mechanical_system( + *node, 70.0, 40.0, 0.22); + if (!mechanical_system.valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for mechanical system"); + + return nullptr; + } + std::shared_ptr motion_sink = + std::make_shared( + *battery_system, mechanical_system); + + // Ambient power system + const double ambient_power_drain = + rmf_fleet_adapter::get_parameter_or_default( + *node, "ambient_power_drain", 20.0); + rmf_battery::agv::PowerSystem ambient_power_system{ + "ambient", ambient_power_drain}; + if (!ambient_power_system.valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for ambient power system"); + + return nullptr; + } + std::shared_ptr ambient_sink = + std::make_shared( + *battery_system, ambient_power_system); + + // Tool power system + const double tool_power_drain = rmf_fleet_adapter::get_parameter_or_default( + *node, "tool_power_drain", 10.0); + rmf_battery::agv::PowerSystem tool_power_system{ + "ambient", tool_power_drain}; + if (!tool_power_system.valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for tool power system"); + + return nullptr; + } + std::shared_ptr tool_sink = + std::make_shared( + *battery_system, tool_power_system); + + // Drain battery + const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default( + *node, "drain_battery", false); + + // Recharge threshold + const double recharge_threshold = rmf_fleet_adapter::get_parameter_or_default( + *node, "recharge_threshold", 0.2); + + connections->fleet->set_recharge_threshold(recharge_threshold); + + if (!connections->fleet->set_task_planner_params( + battery_system, motion_sink, ambient_sink, tool_sink, drain_battery)) + { + RCLCPP_ERROR( + node->get_logger(), + "Failed to initialize task planner parameters"); + + return nullptr; + } + + std::unordered_set task_types; + if (node->declare_parameter("perform_loop", false)) + { + task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_LOOP); + } + // If the perform_deliveries parameter is true, then we just blindly accept // all delivery requests. if (node->declare_parameter("perform_deliveries", false)) { + task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_DELIVERY); connections->fleet->accept_delivery_requests( [](const rmf_task_msgs::msg::Delivery&){ return true; }); } + if (node->declare_parameter("perform_cleaning", false)) + { + task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_CLEAN); + } + + connections->fleet->accept_task_requests( + [task_types](const rmf_task_msgs::msg::TaskProfile& msg) + { + if (task_types.find(msg.task_type.type) != task_types.end()) + return true; + + return false; + }); + if (node->declare_parameter("disable_delay_threshold", false)) { connections->fleet->default_maximum_delay(rmf_utils::nullopt); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index f0a4e6d71..f2b31919a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -30,10 +30,18 @@ namespace rmf_fleet_adapter { std::shared_ptr Task::make( std::string id, PendingPhases phases, - rxcpp::schedulers::worker worker) + rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, + rmf_task::ConstRequestPtr request) { return std::make_shared( - Task(std::move(id), std::move(phases), std::move(worker))); + Task(std::move(id), + std::move(phases), + std::move(worker), + deployment_time, + finish_state, + std::move(request))); } //============================================================================== @@ -80,14 +88,38 @@ const std::string& Task::id() const return _id; } +//============================================================================== +const rmf_task::ConstRequestPtr Task::request() const +{ + return _request; +} + +//============================================================================== +const rmf_traffic::Time Task::deployment_time() const +{ + return _deployment_time; +} + +//============================================================================== +const rmf_task::agv::State Task::finish_state() const +{ + return _finish_state; +} + //============================================================================== Task::Task( std::string id, std::vector> phases, - rxcpp::schedulers::worker worker) + rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, + rmf_task::ConstRequestPtr request) : _id(std::move(id)), _pending_phases(std::move(phases)), - _worker(std::move(worker)) + _worker(std::move(worker)), + _deployment_time(deployment_time), + _finish_state(finish_state), + _request(std::move(request)) { _status_obs = _status_publisher.get_observable(); std::reverse(_pending_phases.begin(), _pending_phases.end()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index d3358da4a..36a34be6d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -22,9 +22,13 @@ #include #include +#include #include +#include +#include + #include #include @@ -93,10 +97,15 @@ class Task : public std::enable_shared_from_this using PendingPhases = std::vector>; // Make a new task + // TODO(YV) Remove default nullptr for request after refactoring Loop and + // Delivery static std::shared_ptr make( std::string id, PendingPhases phases, - rxcpp::schedulers::worker worker); + rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, + rmf_task::ConstRequestPtr request); void begin(); @@ -117,12 +126,24 @@ class Task : public std::enable_shared_from_this const std::string& id() const; + /// Get the request used to generate this task + const rmf_task::ConstRequestPtr request() const; + + /// Get the deployment time of this Task + const rmf_traffic::Time deployment_time() const; + + /// Get the finish state of this Task + const rmf_task::agv::State finish_state() const; + private: Task( std::string id, PendingPhases phases, - rxcpp::schedulers::worker worker); + rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, + rmf_task::ConstRequestPtr request); std::string _id; @@ -140,6 +161,10 @@ class Task : public std::enable_shared_from_this rmf_utils::optional _initial_time; + rmf_traffic::Time _deployment_time; + rmf_task::agv::State _finish_state; + rmf_task::ConstRequestPtr _request; + void _start_next_phase(); StatusMsg _process_summary(const StatusMsg& input_msg); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3e044d686..26f46916d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -17,6 +17,17 @@ #include "TaskManager.hpp" +#include +#include +#include + +#include + +#include "tasks/Clean.hpp" +#include "tasks/ChargeBattery.hpp" + +#include + namespace rmf_fleet_adapter { //============================================================================== @@ -38,6 +49,15 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) } }); + mgr->_timer = mgr->context()->node()->create_wall_timer( + std::chrono::seconds(1), + [w = mgr->weak_from_this()]() + { + if (auto mgr = w.lock()) + { + mgr->_begin_next_task(); + } + }); return mgr; } @@ -70,7 +90,9 @@ void TaskManager::queue_task(std::shared_ptr task, Start expected_finish) { rmf_task_msgs::msg::TaskSummary msg; msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); this->_context->node()->task_summary()->publish(msg); } } @@ -84,6 +106,26 @@ auto TaskManager::expected_finish_location() const -> StartSet return _context->location(); } +//============================================================================== +auto TaskManager::expected_finish_state() const -> State +{ + // If an active task exists, return the estimated finish state of that task + /// else update the current time and battery level for the state and return + if (_active_task) + return _context->state(); + + // Update battery soc and finish time in the current state + auto& finish_state = _context->state(); + auto location = finish_state.location(); + location.time(rmf_traffic_ros2::convert(_context->node()->now())); + finish_state.location(location); + + const double current_battery_soc = _context->current_battery_soc(); + finish_state.battery_soc(current_battery_soc); + + return finish_state; +} + //============================================================================== const agv::RobotContextPtr& TaskManager::context() { @@ -96,67 +138,184 @@ agv::ConstRobotContextPtr TaskManager::context() const return _context; } +//============================================================================== +void TaskManager::set_queue( + const std::vector& assignments) +{ + _queue.clear(); + // We use dynamic cast to determine the type of request and then call the + // appropriate make(~) function to convert the request into a task + for (std::size_t i = 0; i < assignments.size(); ++i) + { + const auto& a = assignments[i]; + auto start = _context->state().location(); + if (i != 0) + start = assignments[i-1].state().location(); + start.time(a.deployment_time()); + + if (const auto request = + std::dynamic_pointer_cast(a.request())) + { + auto task = rmf_fleet_adapter::tasks::make_clean( + request, + _context, + start, + a.deployment_time(), + a.state()); + + std::lock_guard guard(_mutex); + + _queue.push_back(task); + + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); + } + + else if (const auto request = + std::dynamic_pointer_cast( + a.request())) + { + const auto task = tasks::make_charge_battery( + request, + _context, + start, + a.deployment_time(), + a.state()); + + std::lock_guard guard(_mutex); + + _queue.push_back(task); + + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); } + + else if (const auto request = + std::dynamic_pointer_cast( + a.request())) + { + + } + + else + { + continue; + } + } +} + +//============================================================================== +const std::vector TaskManager::requests() const +{ + std::vector requests; + requests.reserve(_queue.size()); + for (const auto& task : _queue) + { + if (std::dynamic_pointer_cast( + task->request())) + continue; + + requests.push_back(task->request()); + + } + + return requests; +} + //============================================================================== void TaskManager::_begin_next_task() { + if (_active_task) + return; + if (_queue.empty()) { - _task_sub.unsubscribe(); - _expected_finish_location = rmf_utils::nullopt; + // _task_sub.unsubscribe(); + // _expected_finish_location = rmf_utils::nullopt; - RCLCPP_INFO( - _context->node()->get_logger(), - "Finished all remaining tasks for [%s]", - _context->requester_id().c_str()); + // RCLCPP_INFO( + // _context->node()->get_logger(), + // "Finished all remaining tasks for [%s]", + // _context->requester_id().c_str()); return; } - _active_task = std::move(_queue.front()); - _queue.erase(_queue.begin()); + std::lock_guard guard(_mutex); + const rmf_traffic::Time now = rmf_traffic_ros2::convert( + _context->node()->now()); + const auto next_task = _queue.front(); + const auto deployment_time = next_task->deployment_time(); - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning new task [%s] for [%s]. Remaining queue size: %d", - _active_task->id().c_str(), _context->requester_id().c_str(), - _queue.size()); - - _task_sub = _active_task->observe() - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [this, id = _active_task->id()](Task::StatusMsg msg) - { - msg.task_id = id; - _context->node()->task_summary()->publish(msg); - }, - [this, id = _active_task->id()](std::exception_ptr e) + if (now > deployment_time) { - rmf_task_msgs::msg::TaskSummary msg; - msg.state = msg.STATE_FAILED; + // Update state in RobotContext and Assign active task + _context->state(_queue.front()->finish_state()); + _active_task = std::move(_queue.front()); + _queue.erase(_queue.begin()); - try { - std::rethrow_exception(e); - } - catch(const std::exception& e) { - msg.status = e.what(); - } + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning new task [%s] for [%s]. Remaining queue size: %d", + _active_task->id().c_str(), _context->requester_id().c_str(), + _queue.size()); - msg.task_id = id; - _context->node()->task_summary()->publish(msg); - _begin_next_task(); - }, - [this, id = _active_task->id()]() - { - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = id; - msg.state = msg.STATE_COMPLETED; - this->_context->node()->task_summary()->publish(msg); + _task_sub = _active_task->observe() + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [this, id = _active_task->id()](Task::StatusMsg msg) + { + msg.task_id = id; + msg.task_profile.task_id = id; + msg.robot_name = _context->name(); + _context->node()->task_summary()->publish(msg); + }, + [this, id = _active_task->id()](std::exception_ptr e) + { + rmf_task_msgs::msg::TaskSummary msg; + msg.state = msg.STATE_FAILED; - _active_task = nullptr; - _begin_next_task(); - }); + try { + std::rethrow_exception(e); + } + catch(const std::exception& e) { + msg.status = e.what(); + } - _active_task->begin(); + msg.task_id = id; + msg.task_profile.task_id = id; + msg.robot_name = _context->name(); + _context->node()->task_summary()->publish(msg); + // _begin_next_task(); + }, + [this, id = _active_task->id()]() + { + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = id; + msg.task_profile.task_id = id; + msg.state = msg.STATE_COMPLETED; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); + + _active_task = nullptr; + }); + + _active_task->begin(); + } +} + +//============================================================================== +void TaskManager::clear_queue() +{ + std::lock_guard guard(_mutex); + _queue.clear(); } } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index c36496de2..b5c1bd224 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -23,6 +23,10 @@ #include +#include + +#include + namespace rmf_fleet_adapter { //============================================================================== @@ -39,18 +43,33 @@ class TaskManager : public std::enable_shared_from_this using Start = rmf_traffic::agv::Plan::Start; using StartSet = rmf_traffic::agv::Plan::StartSet; + using Assignment = rmf_task::agv::TaskPlanner::Assignment; + using State = rmf_task::agv::State; /// Add a task to the queue of this manager. void queue_task(std::shared_ptr task, Start expected_finish); /// The location where we expect this robot to be at the end of its current - /// task queue. + /// task queue StartSet expected_finish_location() const; const agv::RobotContextPtr& context(); agv::ConstRobotContextPtr context() const; + /// Set the queue for this task manager with assignments generated from the + /// task planner + void set_queue(const std::vector& assignments); + + /// Get the non-charging requests among pending tasks + const std::vector requests() const; + + // Callback for timer which begins next task if its deployment time has passed + void _begin_next_task(); + + // The state of the robot. + State expected_finish_state() const; + private: TaskManager(agv::RobotContextPtr context); @@ -62,7 +81,10 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _task_sub; rxcpp::subscription _emergency_sub; - void _begin_next_task(); + std::mutex _mutex; + rclcpp::TimerBase::SharedPtr _timer; + + void clear_queue(); }; using TaskManagerPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index ff647ec13..dc73081e6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -25,8 +25,8 @@ #include #include -#include -#include +// #include +// #include #include "internal_TrafficLight.hpp" @@ -66,23 +66,22 @@ class Adapter::Implementation std::shared_ptr writer; rmf_traffic_ros2::schedule::MirrorManager mirror_manager; + // using Delivery = rmf_task_msgs::msg::Delivery; + // using DeliverySub = rclcpp::Subscription::SharedPtr; + // DeliverySub delivery_sub; - using Delivery = rmf_task_msgs::msg::Delivery; - using DeliverySub = rclcpp::Subscription::SharedPtr; - DeliverySub delivery_sub; - - using Loop = rmf_task_msgs::msg::Loop; - using LoopSub = rclcpp::Subscription::SharedPtr; - LoopSub loop_sub; + // using Loop = rmf_task_msgs::msg::Loop; + // using LoopSub = rclcpp::Subscription::SharedPtr; + // LoopSub loop_sub; std::vector> fleets = {}; // TODO(MXG): This mutex probably isn't needed std::mutex mutex; - std::unordered_set received_tasks; - std::map task_times; - rclcpp::TimerBase::SharedPtr task_purge_timer; + // std::unordered_set received_tasks; + // std::map task_times; + // rclcpp::TimerBase::SharedPtr task_purge_timer; Implementation( rxcpp::schedulers::worker worker_, @@ -96,50 +95,50 @@ class Adapter::Implementation writer{std::move(writer_)}, mirror_manager{std::move(mirror_manager_)} { - const auto default_qos = rclcpp::SystemDefaultsQoS(); - delivery_sub = node->create_subscription( - DeliveryTopicName, default_qos, - [this](Delivery::SharedPtr msg) - { - std::lock_guard lock(mutex); - if (!received_tasks.insert(msg->task_id).second) - return; - - task_times.insert( - task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); - - rmf_fleet_adapter::agv::request_delivery(*msg, fleets); - }); - - loop_sub = node->create_subscription( - LoopRequestTopicName, default_qos, - [this](Loop::SharedPtr msg) - { - std::lock_guard lock(mutex); - if (!received_tasks.insert(msg->task_id).second) - return; - - task_times.insert( - task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); - - rmf_fleet_adapter::agv::request_loop(*msg, fleets); - }); - - task_purge_timer = node->create_wall_timer( - std::chrono::minutes(60), [this]() - { - // This purge of task ids is to prevent the log of tasks from growing - // infinitely. - const auto purge_end = - std::chrono::steady_clock::now() - std::chrono::minutes(60); - - auto it = task_times.begin(); - for (; it != task_times.end() && it->first < purge_end; ++it) - received_tasks.erase(it->second); - - if (it != task_times.begin()) - task_times.erase(task_times.begin(), it); - }); + // const auto default_qos = rclcpp::SystemDefaultsQoS(); + // delivery_sub = node->create_subscription( + // DeliveryTopicName, default_qos, + // [this](Delivery::SharedPtr msg) + // { + // std::lock_guard lock(mutex); + // if (!received_tasks.insert(msg->task_id).second) + // return; + + // task_times.insert( + // task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); + + // rmf_fleet_adapter::agv::request_delivery(*msg, fleets); + // }); + + // loop_sub = node->create_subscription( + // LoopRequestTopicName, default_qos, + // [this](Loop::SharedPtr msg) + // { + // std::lock_guard lock(mutex); + // if (!received_tasks.insert(msg->task_id).second) + // return; + + // task_times.insert( + // task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); + + // rmf_fleet_adapter::agv::request_loop(*msg, fleets); + // }); + + // task_purge_timer = node->create_wall_timer( + // std::chrono::minutes(60), [this]() + // { + // // This purge of task ids is to prevent the log of tasks from growing + // // infinitely. + // const auto purge_end = + // std::chrono::steady_clock::now() - std::chrono::minutes(60); + + // auto it = task_times.begin(); + // for (; it != task_times.end() && it->first < purge_end; ++it) + // received_tasks.erase(it->second); + + // if (it != task_times.begin()) + // task_times.erase(task_times.begin(), it); + // }); } static rmf_utils::unique_impl_ptr make( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 070aef3b9..da85355f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -22,6 +22,9 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" +#include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -61,217 +64,585 @@ class LiaisonNegotiator : public rmf_traffic::schedule::Negotiator } // anonymous namespace //============================================================================== -auto FleetUpdateHandle::Implementation::estimate_delivery( - const rmf_task_msgs::msg::Delivery& request) const --> rmf_utils::optional +void FleetUpdateHandle::Implementation::dock_summary_cb( + const DockSummary::SharedPtr msg) { - const auto& graph = planner->get_configuration().graph(); - const auto pickup_wp = graph.find_waypoint(request.pickup_place_name); - if (!pickup_wp) - return rmf_utils::nullopt; - - const auto dropoff_wp = graph.find_waypoint(request.dropoff_place_name); - if (!dropoff_wp) - return rmf_utils::nullopt; - - const auto pickup_goal = rmf_traffic::agv::Plan::Goal(pickup_wp->index()); - const auto dropoff_goal = rmf_traffic::agv::Plan::Goal(dropoff_wp->index()); - - // TODO(MXG): At some point we should consider parallelizing this estimation - // process and taking the existing schedule into account, but for now we'll - // try to use a very quick rough estimate. - DeliveryEstimate best; - for (const auto& element : task_managers) + for (const auto& dock : msg->docks) { - const auto& mgr = *element.second; - auto start = mgr.expected_finish_location(); - const auto pickup_plan = planner->plan(start, pickup_goal); - if (!pickup_plan) - continue; - - const auto& pickup_plan_end = pickup_plan->get_waypoints().back(); - assert(pickup_plan_end.graph_index()); - const auto dropoff_start = rmf_traffic::agv::Plan::Start( - pickup_plan_end.time(), - *pickup_plan_end.graph_index(), - pickup_plan_end.position()[2]); - - const auto dropoff_plan = planner->plan(dropoff_start, dropoff_goal); - if (!dropoff_plan) - continue; - - const auto& final_wp = dropoff_plan->get_waypoints().back(); - - const auto estimate = final_wp.time(); - rmf_traffic::agv::Plan::Start finish{ - estimate, - *final_wp.graph_index(), - final_wp.position()[2] - }; - - if (estimate < best.time) + if (dock.fleet_name == name) { - best = DeliveryEstimate{ - estimate, - element.first, - std::move(start.front()), - std::move(dropoff_start), - std::move(finish) - }; + dock_param_map.clear(); + for (const auto& param : dock.params) + dock_param_map.insert({param.start, param}); + break; } } - if (best.robot) - return best; - - return rmf_utils::nullopt; + return; } //============================================================================== -void FleetUpdateHandle::Implementation::perform_delivery( - const rmf_task_msgs::msg::Delivery& request, - const DeliveryEstimate& estimate) +void FleetUpdateHandle::Implementation::bid_notice_cb( + const BidNotice::SharedPtr msg) { - auto& mgr = *task_managers.at(estimate.robot); - mgr.queue_task( - tasks::make_delivery( - request, - estimate.robot, - *estimate.pickup_start, - *estimate.dropoff_start), - *estimate.finish); -} + if (task_managers.empty()) + return; + + if (msg->task_profile.task_id.empty()) + return; -//============================================================================== -auto FleetUpdateHandle::Implementation::estimate_loop( - const rmf_task_msgs::msg::Loop& request) const --> rmf_utils::optional -{ - if (request.robot_type != name) - return rmf_utils::nullopt; + if (bid_notice_assignments.find(msg->task_profile.task_id) + != bid_notice_assignments.end()) + return; - const std::size_t n = request.num_loops; - if (n == 0) - return rmf_utils::nullopt; + if (!accept_task) + { + RCLCPP_WARN( + node->get_logger(), + "Fleet [%s] is not configured to accept any task requests. Use " + "FleetUpdateHadndle::accept_task_requests(~) to define a callback " + "for accepting requests", name.c_str()); - const auto& graph = planner->get_configuration().graph(); - const auto loop_start_wp = graph.find_waypoint(request.start_name); - if (!loop_start_wp) - return rmf_utils::nullopt; + return; + } - const auto loop_end_wp = graph.find_waypoint(request.finish_name); - if (!loop_end_wp) - return rmf_utils::nullopt; + if (!accept_task(msg->task_profile)) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] is configured to not accept task [%s]", + name.c_str(), + msg->task_profile.task_id.c_str()); - const auto loop_start_goal = - rmf_traffic::agv::Plan::Goal(loop_start_wp->index()); + return; + } - const auto loop_end_goal = - rmf_traffic::agv::Plan::Goal(loop_end_wp->index()); + if (!task_planner + || !initialized_task_planner) + { + RCLCPP_WARN( + node->get_logger(), + "Fleet [%s] is not configured with parameters for task planning." + "Use FleetUpdateHandle::set_task_planner_params(~) to set the " + "parameters required.", name.c_str()); - LoopEstimate best; - for (const auto& element : task_managers) + return; + } + + // Determine task type and convert to request pointer + rmf_task::ConstRequestPtr new_request = nullptr; + const auto& task_profile = msg->task_profile; + const auto& task_type = task_profile.task_type; + const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.start_time); + // TODO (YV) get rid of ID field in RequestPtr + std::string id = msg->task_profile.task_id; + const auto& graph = planner->get_configuration().graph(); + + // Process Cleaning task + if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) { - LoopEstimate estimate; - estimate.robot = element.first; + if (task_profile.clean.start_waypoint.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [clean.start_waypoint] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); - const auto& mgr = *element.second; - auto start = mgr.expected_finish_location(); - const auto loop_init_plan = planner->plan(start, loop_start_goal); - if (!loop_init_plan) - continue; + return; + } - rmf_traffic::Duration init_duration = std::chrono::seconds(0); - if (loop_init_plan->get_waypoints().size() > 1) + // Check for valid start waypoint + const std::string start_wp_name = task_profile.clean.start_waypoint; + const auto start_wp = graph.find_waypoint(start_wp_name); + if (!start_wp) { - // If loop_init_plan is not empty, then that means we are not starting at - // the starting point of the loop. Therefore we will need an initial plan - // to reach the first point in the loop. - estimate.init_start = start.front(); - - init_duration = - loop_init_plan->get_waypoints().back().time() - - loop_init_plan->get_waypoints().front().time(); + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), start_wp_name.c_str(), id.c_str()); + + return; + } + + // Get dock parameters + const auto clean_param_it = dock_param_map.find(start_wp_name); + if (clean_param_it == dock_param_map.end()) + { + RCLCPP_INFO( + node->get_logger(), + "Dock param for dock_name:[%s] unavailable. Rejecting BidNotice with " + "task_id:[%s]", start_wp_name.c_str(), id.c_str()); + + return; } + const auto clean_param = clean_param_it->second; - const auto loop_forward_start = [&]() -> rmf_traffic::agv::Plan::StartSet + // Check for valid finish waypoint + const std::string finish_wp_name = clean_param.finish; + const auto finish_wp = graph.find_waypoint(finish_wp_name); + if (!finish_wp) { - if (loop_init_plan->get_waypoints().empty()) - return start; - - const auto& loop_init_wp = loop_init_plan->get_waypoints().back(); - assert(loop_init_wp.graph_index()); - return {rmf_traffic::agv::Plan::Start( - loop_init_wp.time(), - *loop_init_wp.graph_index(), - loop_init_wp.position()[2])}; - }(); - - const auto loop_forward_plan = - planner->plan(loop_forward_start, loop_end_goal); - if (!loop_forward_plan) - continue; - - // If the forward plan is empty then that means the start and end of the - // loop are the same, making it a useless request. - // TODO(MXG): We should probably make noise here instead of just ignoring - // the request. - if (loop_forward_plan->get_waypoints().empty()) - return rmf_utils::nullopt; - - estimate.loop_start = loop_forward_start.front(); - - const auto loop_duration = - loop_forward_plan->get_waypoints().back().time() - - loop_forward_plan->get_waypoints().front().time(); - - // We only need to provide this if there is supposed to be more than one - // loop. - const auto& final_wp = loop_forward_plan->get_waypoints().back(); - assert(final_wp.graph_index()); - estimate.loop_end = rmf_traffic::agv::Plan::Start{ - final_wp.time(), - *final_wp.graph_index(), - final_wp.position()[2] - }; - - const auto start_time = [&]() + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), finish_wp_name.c_str(), id.c_str()); + + return; + } + + // Interpolate docking waypoint into trajectory + std::vector positions; + for (const auto& location: clean_param.path) + positions.push_back({location.x, location.y, location.yaw}); + rmf_traffic::Trajectory cleaning_trajectory = + rmf_traffic::agv::Interpolate::positions( + planner->get_configuration().vehicle_traits(), + start_time, + positions); + + if (!(cleaning_trajectory.size() > 0)) { - if (loop_init_plan->get_waypoints().empty()) - return loop_forward_plan->get_waypoints().front().time(); + RCLCPP_INFO( + node->get_logger(), + "Unable to generate cleaning trajectory from positions specified " + " in DockSummary msg for [%s]", start_wp_name.c_str()); + + return; + } + + // TODO(YV) get rid of id field in RequestPtr + std::stringstream id_stream(id); + std::size_t request_id; + id_stream >> request_id; + + new_request = rmf_task::requests::Clean::make( + request_id, + start_wp->index(), + finish_wp->index(), + cleaning_trajectory, + motion_sink, + ambient_sink, + tool_sink, + planner, + start_time, + drain_battery); + + RCLCPP_INFO( + node->get_logger(), + "Generated Clean request"); + } - return loop_init_plan->get_waypoints().front().time(); - }(); + else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) + { + // TODO(YV) + } + else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) + { + // TODO(YV) + } + else + { + RCLCPP_INFO( + node->get_logger(), + "Invalid TaskType in TaskProfile. Rejecting BidNotice with task_id:[%s]", + id.c_str()); - estimate.time = - start_time + init_duration + (2*n - 1)*loop_duration; + return; + } + + if (!new_request) + return; - estimate.loop_end->time(estimate.time); + // Collate robot states and combine new requestptr with requestptr of + // non-charging tasks in task manager queues + std::vector states; + std::vector state_configs; + std::vector pending_requests; + pending_requests.push_back(new_request); + // Map robot index to name for BidProposal + std::unordered_map robot_name_map; + std::size_t index = 0; + for (const auto& t : task_managers) + { + states.push_back(t.second->expected_finish_state()); + state_configs.push_back(t.first->state_config()); + const auto requests = t.second->requests(); + pending_requests.insert(pending_requests.end(), requests.begin(), requests.end()); - if (estimate.time < best.time) - best = std::move(estimate); + robot_name_map.insert({index, t.first->name()}); + ++index; } - if (best.robot) - return best; + RCLCPP_INFO( + node->get_logger(), + "Planning for [%d] robot and [%d] request(s)", + states.size(), pending_requests.size()); + + // Generate new task assignments while accommodating for the new + // request + // Call greedy_plan but run optimal_plan() in a separate thread + const auto assignments = task_planner->optimal_plan( + rmf_traffic_ros2::convert(node->now()), + states, + state_configs, + pending_requests, + nullptr); + + if (assignments.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Failed to compute assignments for task_id:[%s]", id.c_str()); + + return; + } + + const double cost = task_planner->compute_cost(assignments); + + // Display assignments for debugging + std::cout << "Cost: " << cost << std::endl; + for (std::size_t i = 0; i < assignments.size(); ++i) + { + std:: cout << "--Agent: " << i << std::endl; + for (const auto& a : assignments[i]) + { + const auto& s = a.state(); + const double request_seconds = a.request()->earliest_start_time().time_since_epoch().count()/1e9; + const double start_seconds = a.deployment_time().time_since_epoch().count()/1e9; + const rmf_traffic::Time finish_time = s.finish_time(); + const double finish_seconds = finish_time.time_since_epoch().count()/1e9; + std::cout << " <" << a.request()->id() << ": " << request_seconds + << ", " << start_seconds + << ", "<< finish_seconds << ", " << 100* s.battery_soc() + << "%>" << std::endl; + } + } + std::cout << " ----------------------" << std::endl; + + // Publish BidProposal + rmf_task_msgs::msg::BidProposal bid_proposal; + bid_proposal.fleet_name = name; + bid_proposal.task_profile = task_profile; + bid_proposal.prev_cost = current_assignment_cost; + bid_proposal.new_cost = cost; + index = 0; + for (const auto& agent : assignments) + { + for (const auto& assignment : agent) + { + if (std::to_string(assignment.request()->id()) == id) + { + bid_proposal.finish_time = rmf_traffic_ros2::convert( + assignment.state().finish_time()); + if (robot_name_map.find(index) != robot_name_map.end()) + bid_proposal.robot_name = robot_name_map[index]; + break; + } + } + ++index; + } + bid_proposal_pub->publish(bid_proposal); + RCLCPP_INFO( + node->get_logger(), + "Submitted BidProposal to accommodate task [%s] with new cost [%f]", + id.c_str(), cost); + + // Store assignments in internal map + bid_notice_assignments.insert({id, assignments}); - return rmf_utils::nullopt; } + +void FleetUpdateHandle::Implementation::dispatch_request_cb( + const DispatchRequest::SharedPtr msg) +{ + if (msg->fleet_name != name) + return; + + const std::string id = msg->task_profile.task_id; + const auto task_it = bid_notice_assignments.find(id); + + if (task_it == bid_notice_assignments.end()) + return; + + RCLCPP_INFO( + node->get_logger(), + "Bid for task_id:[%s] awarded to fleet [%s]", + id.c_str(), name.c_str()); + + // We currently only support adding tasks + if (msg->method != DispatchRequest::ADD) + return; + + const auto& assignments = task_it->second; + + if (assignments.size() != task_managers.size()) + { + RCLCPP_ERROR( + node->get_logger(), + "The number of available robots do not match that in the assignments " + "for task_id:[%s]. This request will be ignored", id.c_str()); + + return; + } + + std::size_t index = 0; + for (auto& t : task_managers) + { + t.second->set_queue(assignments[index]); + ++index; + } + + current_assignment_cost = task_planner->compute_cost(assignments); + + RCLCPP_INFO( + node->get_logger(), + "Assignments updated for robots in fleet [%s]", + name.c_str()); +} + +//============================================================================== +// auto FleetUpdateHandle::Implementation::estimate_delivery( +// const rmf_task_msgs::msg::Delivery& request) const +// -> rmf_utils::optional +// { +// const auto& graph = planner->get_configuration().graph(); +// const auto pickup_wp = graph.find_waypoint(request.pickup_place_name); +// if (!pickup_wp) +// return rmf_utils::nullopt; + +// const auto dropoff_wp = graph.find_waypoint(request.dropoff_place_name); +// if (!dropoff_wp) +// return rmf_utils::nullopt; + +// const auto pickup_goal = rmf_traffic::agv::Plan::Goal(pickup_wp->index()); +// const auto dropoff_goal = rmf_traffic::agv::Plan::Goal(dropoff_wp->index()); + +// // TODO(MXG): At some point we should consider parallelizing this estimation +// // process and taking the existing schedule into account, but for now we'll +// // try to use a very quick rough estimate. +// DeliveryEstimate best; +// for (const auto& element : task_managers) +// { +// const auto& mgr = *element.second; +// auto start = mgr.expected_finish_location(); +// const auto pickup_plan = planner->plan(start, pickup_goal); +// if (!pickup_plan) +// continue; + +// const auto& pickup_plan_end = pickup_plan->get_waypoints().back(); +// assert(pickup_plan_end.graph_index()); +// const auto dropoff_start = rmf_traffic::agv::Plan::Start( +// pickup_plan_end.time(), +// *pickup_plan_end.graph_index(), +// pickup_plan_end.position()[2]); + +// const auto dropoff_plan = planner->plan(dropoff_start, dropoff_goal); +// if (!dropoff_plan) +// continue; + +// const auto& final_wp = dropoff_plan->get_waypoints().back(); + +// const auto estimate = final_wp.time(); +// rmf_traffic::agv::Plan::Start finish{ +// estimate, +// *final_wp.graph_index(), +// final_wp.position()[2] +// }; + +// if (estimate < best.time) +// { +// best = DeliveryEstimate{ +// estimate, +// element.first, +// std::move(start.front()), +// std::move(dropoff_start), +// std::move(finish) +// }; +// } +// } + +// if (best.robot) +// return best; + +// return rmf_utils::nullopt; +// } + +//============================================================================== +// void FleetUpdateHandle::Implementation::perform_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const DeliveryEstimate& estimate) +// { +// auto& mgr = *task_managers.at(estimate.robot); +// mgr.queue_task( +// tasks::make_delivery( +// request, +// estimate.robot, +// *estimate.pickup_start, +// *estimate.dropoff_start), +// *estimate.finish); +// } + +//============================================================================== +// auto FleetUpdateHandle::Implementation::estimate_loop( +// const rmf_task_msgs::msg::Loop& request) const +// -> rmf_utils::optional +// { +// if (request.robot_type != name) +// return rmf_utils::nullopt; + +// const std::size_t n = request.num_loops; +// if (n == 0) +// return rmf_utils::nullopt; + +// const auto& graph = planner->get_configuration().graph(); +// const auto loop_start_wp = graph.find_waypoint(request.start_name); +// if (!loop_start_wp) +// return rmf_utils::nullopt; + +// const auto loop_end_wp = graph.find_waypoint(request.finish_name); +// if (!loop_end_wp) +// return rmf_utils::nullopt; + +// const auto loop_start_goal = +// rmf_traffic::agv::Plan::Goal(loop_start_wp->index()); + +// const auto loop_end_goal = +// rmf_traffic::agv::Plan::Goal(loop_end_wp->index()); + +// LoopEstimate best; +// for (const auto& element : task_managers) +// { +// LoopEstimate estimate; +// estimate.robot = element.first; + +// const auto& mgr = *element.second; +// auto start = mgr.expected_finish_location(); +// const auto loop_init_plan = planner->plan(start, loop_start_goal); +// if (!loop_init_plan) +// continue; + +// rmf_traffic::Duration init_duration = std::chrono::seconds(0); +// if (loop_init_plan->get_waypoints().size() > 1) +// { +// // If loop_init_plan is not empty, then that means we are not starting at +// // the starting point of the loop. Therefore we will need an initial plan +// // to reach the first point in the loop. +// estimate.init_start = start.front(); + +// init_duration = +// loop_init_plan->get_waypoints().back().time() +// - loop_init_plan->get_waypoints().front().time(); +// } + +// const auto loop_forward_start = [&]() -> rmf_traffic::agv::Plan::StartSet +// { +// if (loop_init_plan->get_waypoints().empty()) +// return start; + +// const auto& loop_init_wp = loop_init_plan->get_waypoints().back(); +// assert(loop_init_wp.graph_index()); +// return {rmf_traffic::agv::Plan::Start( +// loop_init_wp.time(), +// *loop_init_wp.graph_index(), +// loop_init_wp.position()[2])}; +// }(); + +// const auto loop_forward_plan = +// planner->plan(loop_forward_start, loop_end_goal); +// if (!loop_forward_plan) +// continue; + +// // If the forward plan is empty then that means the start and end of the +// // loop are the same, making it a useless request. +// // TODO(MXG): We should probably make noise here instead of just ignoring +// // the request. +// if (loop_forward_plan->get_waypoints().empty()) +// return rmf_utils::nullopt; + +// estimate.loop_start = loop_forward_start.front(); + +// const auto loop_duration = +// loop_forward_plan->get_waypoints().back().time() +// - loop_forward_plan->get_waypoints().front().time(); + +// // We only need to provide this if there is supposed to be more than one +// // loop. +// const auto& final_wp = loop_forward_plan->get_waypoints().back(); +// assert(final_wp.graph_index()); +// estimate.loop_end = rmf_traffic::agv::Plan::Start{ +// final_wp.time(), +// *final_wp.graph_index(), +// final_wp.position()[2] +// }; + +// const auto start_time = [&]() +// { +// if (loop_init_plan->get_waypoints().empty()) +// return loop_forward_plan->get_waypoints().front().time(); + +// return loop_init_plan->get_waypoints().front().time(); +// }(); + +// estimate.time = +// start_time + init_duration + (2*n - 1)*loop_duration; + +// estimate.loop_end->time(estimate.time); + +// if (estimate.time < best.time) +// best = std::move(estimate); +// } + +// if (best.robot) +// return best; + +// return rmf_utils::nullopt; +// } + +//============================================================================== +// void FleetUpdateHandle::Implementation::perform_loop( +// const rmf_task_msgs::msg::Loop& request, +// const LoopEstimate& estimate) +// { +// auto& mgr = task_managers.at(estimate.robot); +// mgr->queue_task( +// tasks::make_loop( +// request, +// estimate.robot, +// estimate.init_start, +// *estimate.loop_start, +// estimate.loop_end), +// *estimate.loop_end); +// } + //============================================================================== -void FleetUpdateHandle::Implementation::perform_loop( - const rmf_task_msgs::msg::Loop& request, - const LoopEstimate& estimate) +std::size_t FleetUpdateHandle::Implementation::get_nearest_charger( + const rmf_traffic::agv::Planner::Start& start, + const std::unordered_set& charging_waypoints) { - auto& mgr = task_managers.at(estimate.robot); - mgr->queue_task( - tasks::make_loop( - request, - estimate.robot, - estimate.init_start, - *estimate.loop_start, - estimate.loop_end), - *estimate.loop_end); + assert(!charging_waypoints.empty()); + const auto& graph = planner->get_configuration().graph(); + Eigen::Vector2d p = graph.get_waypoint(start.waypoint()).get_location(); + + if (start.location().has_value()) + p = *start.location(); + + double min_dist = std::numeric_limits::max(); + std::size_t nearest_charger = 0; + for (const auto& wp : charging_waypoints) + { + const auto loc = graph.get_waypoint(wp).get_location(); + const double dist = (loc - p).norm(); + if (dist < min_dist) + { + min_dist = dist; + nearest_charger = wp; + } + } + + return nearest_charger; } //============================================================================== @@ -282,6 +653,7 @@ void FleetUpdateHandle::add_robot( rmf_traffic::agv::Plan::StartSet start, std::function)> handle_cb) { + assert(!start.empty()); rmf_traffic::schedule::ParticipantDescription description( name, _pimpl->name, @@ -297,6 +669,12 @@ void FleetUpdateHandle::add_robot( fleet = shared_from_this()]( rmf_traffic::schedule::Participant participant) { + // TODO(YV) Get the battery % of this robot + const std::size_t charger = fleet->_pimpl->get_nearest_charger( + start[0], fleet->_pimpl->charging_waypoints); + rmf_task::agv::State state = rmf_task::agv::State{start[0], charger, 1.0}; + rmf_task::agv::StateConfig state_config = rmf_task::agv::StateConfig{ + fleet->_pimpl->recharge_threshold}; auto context = std::make_shared( RobotContext{ std::move(command), @@ -306,7 +684,9 @@ void FleetUpdateHandle::add_robot( fleet->_pimpl->planner, fleet->_pimpl->node, fleet->_pimpl->worker, - fleet->_pimpl->default_maximum_delay + fleet->_pimpl->default_maximum_delay, + state, + state_config }); // We schedule the following operations on the worker to make sure we do not @@ -351,6 +731,14 @@ void FleetUpdateHandle::add_robot( }); } +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::accept_task_requests( + AcceptTaskRequest check) +{ + _pimpl->accept_task = std::move(check); + return *this; +} + //============================================================================== FleetUpdateHandle& FleetUpdateHandle::accept_delivery_requests( AcceptDeliveryRequest check) @@ -374,70 +762,113 @@ FleetUpdateHandle::default_maximum_delay() const return _pimpl->default_maximum_delay; } -//============================================================================== -FleetUpdateHandle::FleetUpdateHandle() +bool FleetUpdateHandle::set_task_planner_params( + std::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink, + const bool drain_battery) { - // Do nothing -} - -//============================================================================== -void request_delivery( - const rmf_task_msgs::msg::Delivery& request, - const std::vector>& fleets) -{ - FleetUpdateHandle::Implementation::DeliveryEstimate best; - FleetUpdateHandle::Implementation* chosen_fleet = nullptr; - - for (auto& fleet : fleets) + if (battery_system && motion_sink && ambient_sink && tool_sink) { - auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); - if (!fimpl.accept_delivery || !fimpl.accept_delivery(request)) - continue; - const auto estimate = fimpl.estimate_delivery(request); - if (!estimate) - continue; - - if (estimate->time < best.time) - { - best = *estimate; - chosen_fleet = &fimpl; - } + _pimpl->battery_system = battery_system; + _pimpl->motion_sink = motion_sink; + _pimpl->ambient_sink = ambient_sink; + _pimpl->tool_sink = tool_sink; + + std::shared_ptr task_config = + std::make_shared( + *battery_system, + motion_sink, + ambient_sink, + _pimpl->planner); + + _pimpl->task_planner = std::make_shared( + task_config); + + _pimpl->drain_battery = drain_battery; + + _pimpl->initialized_task_planner = true; + + return _pimpl->initialized_task_planner; } - if (!chosen_fleet) - return; - - chosen_fleet->perform_delivery(request, best); + return false; } //============================================================================== -void request_loop( - const rmf_task_msgs::msg::Loop& request, - const std::vector>& fleets) +FleetUpdateHandle& FleetUpdateHandle::set_recharge_threshold( + const double threshold) { - FleetUpdateHandle::Implementation::LoopEstimate best; - FleetUpdateHandle::Implementation* chosen_fleet = nullptr; - - for (auto& fleet : fleets) - { - auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); - const auto estimate = fimpl.estimate_loop(request); - if (!estimate) - continue; + _pimpl->recharge_threshold = threshold; + return *this; +} - if (estimate->time < best.time) - { - best = *estimate; - chosen_fleet = &fimpl; - } - } +//============================================================================== +FleetUpdateHandle::FleetUpdateHandle() +{ + // Do nothing +} - if (!chosen_fleet) - return; +//============================================================================== +// void request_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const std::vector>& fleets) +// { +// FleetUpdateHandle::Implementation::DeliveryEstimate best; +// FleetUpdateHandle::Implementation* chosen_fleet = nullptr; + +// for (auto& fleet : fleets) +// { +// auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); +// if (!fimpl.accept_delivery || !fimpl.accept_delivery(request)) +// continue; + +// const auto estimate = fimpl.estimate_delivery(request); +// if (!estimate) +// continue; + +// if (estimate->time < best.time) +// { +// best = *estimate; +// chosen_fleet = &fimpl; +// } +// } + +// if (!chosen_fleet) +// return; + +// chosen_fleet->perform_delivery(request, best); +// } - chosen_fleet->perform_loop(request, best); -} +//============================================================================== +// void request_loop( +// const rmf_task_msgs::msg::Loop& request, +// const std::vector>& fleets) +// { +// FleetUpdateHandle::Implementation::LoopEstimate best; +// FleetUpdateHandle::Implementation* chosen_fleet = nullptr; + +// for (auto& fleet : fleets) +// { +// auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); +// const auto estimate = fimpl.estimate_loop(request); +// if (!estimate) +// continue; + +// if (estimate->time < best.time) +// { +// best = *estimate; +// chosen_fleet = &fimpl; +// } +// } + +// if (!chosen_fleet) +// return; + +// chosen_fleet->perform_loop(request, best); +// } } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index a2d2626db..4b322710c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -213,6 +213,47 @@ RobotContext& RobotContext::maximum_delay( return *this; } +//============================================================================== +rmf_task::agv::State& RobotContext::state() +{ + return _state; +} + +//============================================================================== +RobotContext& RobotContext::state( + const rmf_task::agv::State& state) +{ + _state = state; + return *this; +} + +//============================================================================== +const rmf_task::agv::StateConfig RobotContext::state_config() const +{ + return _state_config; +} + +//============================================================================== +double RobotContext::current_battery_soc() const +{ + return _current_battery_soc; +} + +RobotContext& RobotContext::current_battery_soc(const double battery_soc) +{ + _current_battery_soc = battery_soc; + _battery_soc_publisher.get_subscriber().on_next(battery_soc); + + return *this; +} + +//============================================================================== +const rxcpp::observable& RobotContext::observe_battery_soc() const +{ + return _battery_soc_obs; +} + + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, @@ -241,7 +282,9 @@ RobotContext::RobotContext( std::shared_ptr planner, std::shared_ptr node, const rxcpp::schedulers::worker& worker, - rmf_utils::optional maximum_delay) + rmf_utils::optional maximum_delay, + rmf_task::agv::State state, + rmf_task::agv::StateConfig state_config) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), _itinerary(std::move(itinerary)), @@ -251,12 +294,16 @@ RobotContext::RobotContext( _worker(worker), _maximum_delay(maximum_delay), _requester_id( - _itinerary.description().owner() + "/" + _itinerary.description().name()) + _itinerary.description().owner() + "/" + _itinerary.description().name()), + _state(state), + _state_config(state_config) { _profile = std::make_shared( _itinerary.description().profile()); _interrupt_obs = _interrupt_publisher.get_observable(); + + _battery_soc_obs = _battery_soc_publisher.get_observable(); } } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 6f6e54f7d..95d274960 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -9,6 +9,9 @@ #include #include +#include +#include + #include #include @@ -112,6 +115,25 @@ class RobotContext const TableViewerPtr& table_viewer, const ResponderPtr& responder) final; + /// Set the state of this robot + RobotContext& state(const rmf_task::agv::State& state); + + /// Get a mutable reference to the state of this robot + rmf_task::agv::State& state(); + + /// Get the state config of this robot + const rmf_task::agv::StateConfig state_config() const; + + /// Get the current battery state of charge + double current_battery_soc() const; + + /// Set the current battery state of charge. Note: This function also + /// publishes the battery soc via _battery_soc_publisher. + RobotContext& current_battery_soc(const double battery_soc); + + // Get a reference to the battery soc observer of this robot. + const rxcpp::observable& observe_battery_soc() const; + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; @@ -124,7 +146,9 @@ class RobotContext std::shared_ptr planner, std::shared_ptr node, const rxcpp::schedulers::worker& worker, - rmf_utils::optional maximum_delay); + rmf_utils::optional maximum_delay, + rmf_task::agv::State state, + rmf_task::agv::StateConfig state_config); std::weak_ptr _command_handle; std::vector _location; @@ -144,6 +168,13 @@ class RobotContext std::string _requester_id; rmf_traffic::schedule::Negotiator* _negotiator = nullptr; + + /// Always call the current_battery_soc() setter to set a new value + double _current_battery_soc; + rxcpp::subjects::subject _battery_soc_publisher; + rxcpp::observable _battery_soc_obs; + rmf_task::agv::State _state; + rmf_task::agv::StateConfig _state_config; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 228bfb6cf..866992d87 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -162,6 +162,22 @@ void RobotUpdateHandle::update_position( } } +//============================================================================== +void RobotUpdateHandle::update_battery_soc(const double battery_soc) +{ + if (battery_soc < 0.0 || battery_soc > 1.0) + return; + + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [context, battery_soc](const auto&) + { + context->current_battery_soc(battery_soc); + }); + } +} + //============================================================================== RobotUpdateHandle& RobotUpdateHandle::maximum_delay( rmf_utils::optional value) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index f36415a48..1b1be2859 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -20,16 +20,33 @@ #include +#include +#include +#include + +#include +#include +#include + +#include + #include +#include #include "Node.hpp" #include "RobotContext.hpp" #include "../TaskManager.hpp" #include +#include +#include #include #include +#include + +#include +#include namespace rmf_fleet_adapter { namespace agv { @@ -117,38 +134,144 @@ class FleetUpdateHandle::Implementation std::shared_ptr snappable; std::shared_ptr negotiation; + // Task planner params + std::shared_ptr battery_system = nullptr; + std::shared_ptr motion_sink = nullptr; + std::shared_ptr ambient_sink = nullptr; + std::shared_ptr tool_sink = nullptr; + bool drain_battery = true; + std::shared_ptr task_planner = nullptr; + bool initialized_task_planner = false; + rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map> task_managers = {}; + // Map task id to pair of + using Assignments = rmf_task::agv::TaskPlanner::Assignments; + std::unordered_map> task_map = {}; + + // Map of dock name to dock parameters + std::unordered_map dock_param_map = {}; + + // Threshold soc for battery recharging + double recharge_threshold = 0.2; + + // TODO Support for various charging configurations + std::unordered_set charging_waypoints; + // We assume each robot has a designated charging waypoint + std::unordered_set available_charging_waypoints; + + double current_assignment_cost = 0.0; + // Map to store task id with assignments for BidNotice + std::unordered_map bid_notice_assignments = {}; + + AcceptTaskRequest accept_task = nullptr; + + using BidNotice = rmf_task_msgs::msg::BidNotice; + using BidNoticeSub = rclcpp::Subscription::SharedPtr; + BidNoticeSub bid_notice_sub = nullptr; + + using BidProposal = rmf_task_msgs::msg::BidProposal; + using BidProposalPub = rclcpp::Publisher::SharedPtr; + BidProposalPub bid_proposal_pub = nullptr; + + using DispatchRequest = rmf_task_msgs::msg::DispatchRequest; + using DispatchRequestSub = rclcpp::Subscription::SharedPtr; + DispatchRequestSub dispatch_request_sub = nullptr; + + using DockSummary = rmf_fleet_msgs::msg::DockSummary; + using DockSummarySub = rclcpp::Subscription::SharedPtr; + DockSummarySub dock_summary_sub = nullptr; + template static std::shared_ptr make(Args&&... args) { FleetUpdateHandle handle; handle._pimpl = rmf_utils::make_unique_impl( Implementation{std::forward(args)...}); + + // Create subs and pubs for bidding + auto default_qos = rclcpp::SystemDefaultsQoS(); + auto transient_qos = rclcpp::QoS(10);; transient_qos.transient_local(); + + // Publish BidProposal + handle._pimpl->bid_proposal_pub = + handle._pimpl->node->create_publisher( + BidProposalTopicName, default_qos); + + // Subscribe BidNotice + handle._pimpl->bid_notice_sub = + handle._pimpl->node->create_subscription( + BidNoticeTopicName, + default_qos, + [p = handle._pimpl.get()](const BidNotice::SharedPtr msg) + { + p->bid_notice_cb(msg); + }); + + // Subscribe DispatchRequest + handle._pimpl->dispatch_request_sub = + handle._pimpl->node->create_subscription( + DispatchRequestTopicName, + default_qos, + [p = handle._pimpl.get()](const DispatchRequest::SharedPtr msg) + { + p->dispatch_request_cb(msg); + }); + + // Subscribe DockSummary + handle._pimpl->dock_summary_sub = + handle._pimpl->node->create_subscription( + DockSummaryTopicName, + transient_qos, + [p = handle._pimpl.get()](const DockSummary::SharedPtr msg) + { + p->dock_summary_cb(msg); + }); + + // Populate charging waypoints + const std::size_t num_waypoints = + handle._pimpl->planner->get_configuration().graph().num_waypoints(); + for (std::size_t i = 0; i < num_waypoints; ++i) + handle._pimpl->charging_waypoints.insert(i); + handle._pimpl->available_charging_waypoints = + handle._pimpl->charging_waypoints; + return std::make_shared(std::move(handle)); } - struct DeliveryEstimate - { - rmf_traffic::Time time = rmf_traffic::Time::max(); - RobotContextPtr robot = nullptr; - rmf_utils::optional pickup_start; - rmf_utils::optional dropoff_start; - rmf_utils::optional finish; - }; - - struct LoopEstimate - { - rmf_traffic::Time time = rmf_traffic::Time::max(); - RobotContextPtr robot = nullptr; - rmf_utils::optional init_start; - rmf_utils::optional loop_start; - rmf_utils::optional loop_end; - }; + // struct DeliveryEstimate + // { + // rmf_traffic::Time time = rmf_traffic::Time::max(); + // RobotContextPtr robot = nullptr; + // rmf_utils::optional pickup_start; + // rmf_utils::optional dropoff_start; + // rmf_utils::optional finish; + // }; + + // struct LoopEstimate + // { + // rmf_traffic::Time time = rmf_traffic::Time::max(); + // RobotContextPtr robot = nullptr; + // rmf_utils::optional init_start; + // rmf_utils::optional loop_start; + // rmf_utils::optional loop_end; + // }; + + void dock_summary_cb(const DockSummary::SharedPtr msg); + + void bid_notice_cb(const BidNotice::SharedPtr msg); + + void dispatch_request_cb(const DispatchRequest::SharedPtr msg); + + std::size_t get_nearest_charger( + const rmf_traffic::agv::Planner::Start& start, + const std::unordered_set& charging_waypoints); static Implementation& get(FleetUpdateHandle& fleet) { @@ -161,30 +284,30 @@ class FleetUpdateHandle::Implementation } // TODO(MXG): Come up with a better design for task dispatch - rmf_utils::optional estimate_delivery( - const rmf_task_msgs::msg::Delivery& request) const; + // rmf_utils::optional estimate_delivery( + // const rmf_task_msgs::msg::Delivery& request) const; - void perform_delivery( - const rmf_task_msgs::msg::Delivery& request, - const DeliveryEstimate& estimate); + // void perform_delivery( + // const rmf_task_msgs::msg::Delivery& request, + // const DeliveryEstimate& estimate); - rmf_utils::optional estimate_loop( - const rmf_task_msgs::msg::Loop& request) const; + // rmf_utils::optional estimate_loop( + // const rmf_task_msgs::msg::Loop& request) const; - void perform_loop( - const rmf_task_msgs::msg::Loop& request, - const LoopEstimate& estimate); + // void perform_loop( + // const rmf_task_msgs::msg::Loop& request, + // const LoopEstimate& estimate); }; //============================================================================== -void request_delivery( - const rmf_task_msgs::msg::Delivery& request, - const std::vector>& fleets); - -//============================================================================== -void request_loop( - const rmf_task_msgs::msg::Loop& request, - const std::vector>& fleets); +// void request_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const std::vector>& fleets); + +// //============================================================================== +// void request_loop( +// const rmf_task_msgs::msg::Loop& request, +// const std::vector>& fleets); } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp index 85f419fe4..23acf9047 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp @@ -111,6 +111,14 @@ rmf_traffic::agv::Graph parse_graph( wp.set_passthrough_point(true); } + const YAML::Node& charger_option = options["is_charger"]; + if (charger_option) + { + const bool is_charger = charger_option.as(); + if (is_charger) + wp.set_charger(true); + } + const YAML::Node& lift_option = options["lift"]; if (lift_option) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index e90e4f7fa..af906c610 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -133,13 +133,13 @@ void MockAdapter::stop() //============================================================================== void MockAdapter::request_delivery(const rmf_task_msgs::msg::Delivery& request) { - rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); + // rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); } //============================================================================== void MockAdapter::request_loop(const rmf_task_msgs::msg::Loop& request) { - rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets); + // rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets); } } // namespace test diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp index 735783d30..16caa24aa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp @@ -88,4 +88,45 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default(rclcpp::Node& node, return traits; } + +//============================================================================== +rmf_battery::agv::BatterySystem get_battery_system( + rclcpp::Node& node, + const double default_voltage, + const double default_capacity, + const double default_charging_current) +{ + const double voltage = + get_parameter_or_default(node, "battery_voltage", default_voltage); + const double capacity = + get_parameter_or_default(node, "battery_capacity", default_capacity); + const double charging_current = + get_parameter_or_default( + node, "battery_charging_current", default_charging_current); + + rmf_battery::agv::BatterySystem battery_system{ + voltage, capacity, charging_current}; + + return battery_system; +} + +rmf_battery::agv::MechanicalSystem get_mechanical_system( + rclcpp::Node& node, + const double default_mass, + const double default_inertia, + const double default_friction) +{ + const double mass = + get_parameter_or_default(node, "mass", default_mass); + const double inertia = + get_parameter_or_default(node, "inertia", default_inertia); + const double friction = + get_parameter_or_default(node, "friction_coefficient", default_friction); + + rmf_battery::agv::MechanicalSystem mechanical_system{ + mass, inertia, friction}; + + return mechanical_system; +} + } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp index 6df2349b9..dafda60ff 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp @@ -20,6 +20,9 @@ #include +#include +#include + #include #include @@ -60,6 +63,19 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default( const double default_a_nom, const double default_alpha_nom, const double default_r_f, const double default_r_v); +//============================================================================== +rmf_battery::agv::BatterySystem get_battery_system( + rclcpp::Node& node, + const double default_voltage, + const double default_capacity, + const double default_charging_current); + +rmf_battery::agv::MechanicalSystem get_mechanical_system( + rclcpp::Node& node, + const double default_mass, + const double default_inertia, + const double default_friction); + } // namespace rmf_fleet_adapter #endif // SRC__RMF_FLEET_ADAPTER__LOAD_PARAM_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp index 176144766..83bb0bb2b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp @@ -22,6 +22,8 @@ #include "RequestLift.hpp" #include "DockRobot.hpp" +#include + namespace rmf_fleet_adapter { namespace phases { @@ -89,6 +91,16 @@ void GoToPlace::Active::respond( const TableViewerPtr& table_viewer, const ResponderPtr& responder) { + if (_subtasks) + { + if (dynamic_cast(_subtasks->current_phase().get())) + { + rmf_traffic::schedule::StubbornNegotiator(_context->itinerary()) + .respond(table_viewer, responder); + return; + } + } + auto approval_cb = [w = weak_from_this()]( const rmf_traffic::agv::Plan& plan) -> rmf_utils::optional @@ -533,9 +545,17 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan) } } - + // TODO: Make distinctions between task and subtasks to avoid passing + // dummy parameters for subtasks + rmf_traffic::Time dummy_time; + rmf_task::agv::State dummy_state{{dummy_time, 0, 0.0}, 0, 1.0}; _subtasks = Task::make( - _description, std::move(sub_phases), _context->worker()); + _description, + std::move(sub_phases), + _context->worker(), + dummy_time, + dummy_state, + nullptr); _status_subscription = _subtasks->observe() .observe_on(rxcpp::identity_same_worker(_context->worker())) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp new file mode 100644 index 000000000..15a2454f9 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -0,0 +1,156 @@ +/* + * 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 "WaitForCharge.hpp" + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +auto WaitForCharge::Active::observe() const -> const rxcpp::observable& +{ + return _status_obs; +} + +//============================================================================== +rmf_traffic::Duration WaitForCharge::Active::estimate_remaining_time() const +{ + const double capacity = _battery_system.capacity(); + const double charging_current = _battery_system.charging_current(); + const double time_estimate = + 3600.0 * capacity * (_charge_to_soc - _context->current_battery_soc()) / charging_current; + + return rmf_traffic::time::from_seconds(time_estimate); +} + +//============================================================================== +void WaitForCharge::Active::emergency_alarm(const bool value) +{ + // Assume charging station is a holding point +} + +//============================================================================== +void WaitForCharge::Active::cancel() +{ + // TODO +} + +//============================================================================== +const std::string& WaitForCharge::Active::description() const +{ + return _description; +} + +//============================================================================== +WaitForCharge::Active::Active( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc) +: _context(std::move(context)), + _battery_system(battery_system), + _charge_to_soc(charge_to_soc) +{ + _description = "Charging [" + _context->requester_id() + "] to [" + + std::to_string(100.0 * _charge_to_soc) + "]"; + + + StatusMsg initial_msg; + initial_msg.status = _description; + _status_publisher.get_subscriber().on_next(initial_msg); + const auto now = _context->node()->now(); + initial_msg.start_time = now; + initial_msg.end_time = now + estimate_remaining_time(); + + _status_obs = _status_publisher + .get_observable() + .start_with(initial_msg); + +} + +//============================================================================== +std::shared_ptr WaitForCharge::Pending::begin() +{ + auto active = + std::shared_ptr(new Active( + _context, _battery_system, _charge_to_soc)); + + active->_battery_soc_subscription = _context->observe_battery_soc() + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [a = active->weak_from_this()](const double battery_soc) + { + const auto active = a.lock(); + + if (!active) + return; + + if(active->_charge_to_soc <= battery_soc) + { + active->_status_publisher.get_subscriber().on_completed(); + } + // TODO Publish warning message to alert user if battery is not + // charging at expected rate + }); + + return active; +} + +//============================================================================== +rmf_traffic::Duration WaitForCharge::Pending::estimate_phase_duration() const +{ + return rmf_traffic::time::from_seconds(_time_estimate); +} + +//============================================================================== +const std::string& WaitForCharge::Pending::description() const +{ + return _description; +} + +//============================================================================== +WaitForCharge::Pending::Pending( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc, + double time_estimate) +: _context(std::move(context)), + _battery_system(battery_system), + _charge_to_soc(charge_to_soc), + _time_estimate(time_estimate) +{ + _description = + "Charging robot to [" + std::to_string(100.0 * charge_to_soc) + "%]"; +} + +//============================================================================== +auto WaitForCharge::make( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc) -> std::unique_ptr +{ + + const double capacity = battery_system.capacity(); + const double charging_current = battery_system.charging_current(); + const double time_estimate = + 3600.0 * capacity * (charge_to_soc - context->current_battery_soc()) / charging_current; + + return std::unique_ptr( + new Pending(context, battery_system, charge_to_soc, time_estimate)); +} + +} // namespace phases +} // namespace rmf_fleet_adapter \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp new file mode 100644 index 000000000..1bb2faa73 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -0,0 +1,112 @@ +/* + * 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 SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP +#define SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +class WaitForCharge +{ +public: + + using StatusMsg = Task::StatusMsg; + class Pending; + + class Active + : public Task::ActivePhase, + public std::enable_shared_from_this + { + public: + + // Documentation inherited from ActivePhase + const rxcpp::observable& observe() const final; + + // Documentation inherited from ActivePhase + rmf_traffic::Duration estimate_remaining_time() const final; + + // Documentation inherited from ActivePhase + void emergency_alarm(bool on) final; + + // Documentation inherited from ActivePhase + void cancel() final; + + // Documentation inherited from ActivePhase + const std::string & description() const final; + + private: + friend class Pending; + Active( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc); + + agv::RobotContextPtr _context; + rmf_battery::agv::BatterySystem _battery_system; + double _charge_to_soc; + std::string _description; + rxcpp::observable _status_obs; + rxcpp::subjects::subject _status_publisher; + rmf_rxcpp::subscription_guard _battery_soc_subscription; + + }; + + class Pending : public Task::PendingPhase + { + public: + // Documentation inherited + std::shared_ptr begin() final; + + // Documentation inherited + rmf_traffic::Duration estimate_phase_duration() const final; + + // Documentation inherited + const std::string& description() const final; + + private: + friend class WaitForCharge; + Pending( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc, + double time_estimate); + + agv::RobotContextPtr _context; + rmf_battery::agv::BatterySystem _battery_system; + double _charge_to_soc; + std::string _description; + double _time_estimate; + }; + + static std::unique_ptr make( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc); + +}; + +} // namespace phases +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp new file mode 100644 index 000000000..0414a38a2 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -0,0 +1,53 @@ +/* + * 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 "../phases/GoToPlace.hpp" + +#include "../phases/WaitForCharge.hpp" + +#include "ChargeBattery.hpp" + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_charge_battery( + const rmf_task::requests::ConstChargeBatteryRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state) +{ + rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()}; + + Task::PendingPhases phases; + phases.push_back( + phases::GoToPlace::make(context, std::move(start), goal)); + phases.push_back( + phases::WaitForCharge::make(context, request->battery_system(), 1.0)); + + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} + +} // namespace task +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp new file mode 100644 index 000000000..66ffef52d --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -0,0 +1,44 @@ +/* + * 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 SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP +#define SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +#include +#include + +#include +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_charge_battery( + const rmf_task::requests::ConstChargeBatteryRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state); + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp new file mode 100644 index 000000000..2620082eb --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -0,0 +1,52 @@ +/* + * 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 "../phases/GoToPlace.hpp" + +#include "Clean.hpp" + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_clean( + const rmf_task::requests::ConstCleanRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start clean_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state) +{ + rmf_traffic::agv::Planner::Goal clean_goal{request->start_waypoint()}; + auto end_start = request->location_after_clean(clean_start); + rmf_traffic::agv::Planner::Goal end_goal{request->end_waypoint()}; + Task::PendingPhases phases; + phases.push_back( + phases::GoToPlace::make(context, std::move(clean_start), clean_goal)); + phases.push_back( + phases::GoToPlace::make(context, std::move(end_start), end_goal)); + + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} + +} // namespace task +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp new file mode 100644 index 000000000..6c5d67ab5 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -0,0 +1,44 @@ +/* + * 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 SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP +#define SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +#include +#include + +#include +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_clean( + const rmf_task::requests::ConstCleanRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start clean_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state); + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index 09130b4e0..edf93b4d3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -1,79 +1,79 @@ -/* - * 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. - * -*/ +// /* +// * 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 "../phases/DispenseItem.hpp" -#include "../phases/IngestItem.hpp" -#include "../phases/GoToPlace.hpp" +// #include "../phases/DispenseItem.hpp" +// #include "../phases/IngestItem.hpp" +// #include "../phases/GoToPlace.hpp" -#include "Delivery.hpp" +// #include "Delivery.hpp" -namespace rmf_fleet_adapter { -namespace tasks { +// namespace rmf_fleet_adapter { +// namespace tasks { -//============================================================================== -std::shared_ptr make_delivery( - const rmf_task_msgs::msg::Delivery& request, - const agv::RobotContextPtr& context, - rmf_traffic::agv::Plan::Start pickup_start, - rmf_traffic::agv::Plan::Start dropoff_start) -{ - const auto& graph = context->navigation_graph(); +// //============================================================================== +// std::shared_ptr make_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const agv::RobotContextPtr& context, +// rmf_traffic::agv::Plan::Start pickup_start, +// rmf_traffic::agv::Plan::Start dropoff_start) +// { +// const auto& graph = context->navigation_graph(); - const auto pickup_wp = - graph.find_waypoint(request.pickup_place_name)->index(); +// const auto pickup_wp = +// graph.find_waypoint(request.pickup_place_name)->index(); - Task::PendingPhases phases; - phases.push_back( - phases::GoToPlace::make(context, std::move(pickup_start), pickup_wp)); +// Task::PendingPhases phases; +// phases.push_back( +// phases::GoToPlace::make(context, std::move(pickup_start), pickup_wp)); - phases.push_back( - std::make_unique( - context, - request.task_id, - request.pickup_dispenser, - context->itinerary().description().owner(), - request.items)); +// phases.push_back( +// std::make_unique( +// context, +// request.task_id, +// request.pickup_dispenser, +// context->itinerary().description().owner(), +// request.items)); - const auto dropoff_wp = - graph.find_waypoint(request.dropoff_place_name)->index(); +// const auto dropoff_wp = +// graph.find_waypoint(request.dropoff_place_name)->index(); - phases.push_back( - phases::GoToPlace::make(context, std::move(dropoff_start), dropoff_wp)); +// phases.push_back( +// phases::GoToPlace::make(context, std::move(dropoff_start), dropoff_wp)); - std::vector ingestor_items; - ingestor_items.reserve(request.items.size()); - for(auto& dispenser_item : request.items){ - rmf_ingestor_msgs::msg::IngestorRequestItem item{}; - item.type_guid = dispenser_item.type_guid; - item.quantity = dispenser_item.quantity; - item.compartment_name = dispenser_item.compartment_name; - ingestor_items.push_back(std::move(item)); - } +// std::vector ingestor_items; +// ingestor_items.reserve(request.items.size()); +// for(auto& dispenser_item : request.items){ +// rmf_ingestor_msgs::msg::IngestorRequestItem item{}; +// item.type_guid = dispenser_item.type_guid; +// item.quantity = dispenser_item.quantity; +// item.compartment_name = dispenser_item.compartment_name; +// ingestor_items.push_back(std::move(item)); +// } - phases.push_back( - std::make_unique( - context, - request.task_id, - request.dropoff_ingestor, - context->itinerary().description().owner(), - ingestor_items)); +// phases.push_back( +// std::make_unique( +// context, +// request.task_id, +// request.dropoff_ingestor, +// context->itinerary().description().owner(), +// ingestor_items)); - return Task::make(request.task_id, std::move(phases), context->worker()); -} +// return Task::make(request.task_id, std::move(phases), context->worker()); +// } -} // namespace task -} // namespace rmf_fleet_adapter +// } // namespace task +// } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index 8593261fc..b5f9397cb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -1,68 +1,68 @@ -/* - * 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. - * -*/ +// /* +// * 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 "Loop.hpp" +// #include "Loop.hpp" -#include "../phases/GoToPlace.hpp" +// #include "../phases/GoToPlace.hpp" -namespace rmf_fleet_adapter { -namespace tasks { +// namespace rmf_fleet_adapter { +// namespace tasks { -//============================================================================== -std::shared_ptr make_loop( - const rmf_task_msgs::msg::Loop& request, - const agv::RobotContextPtr& context, - rmf_utils::optional init_start, - rmf_traffic::agv::Plan::Start loop_start, - rmf_utils::optional loop_end) -{ - const auto& graph = context->navigation_graph(); +// //============================================================================== +// std::shared_ptr make_loop( +// const rmf_task_msgs::msg::Loop& request, +// const agv::RobotContextPtr& context, +// rmf_utils::optional init_start, +// rmf_traffic::agv::Plan::Start loop_start, +// rmf_utils::optional loop_end) +// { +// const auto& graph = context->navigation_graph(); - Task::PendingPhases phases; +// Task::PendingPhases phases; - const auto start_wp = - graph.find_waypoint(request.start_name)->index(); +// const auto start_wp = +// graph.find_waypoint(request.start_name)->index(); - const auto end_wp = - graph.find_waypoint(request.finish_name)->index(); +// const auto end_wp = +// graph.find_waypoint(request.finish_name)->index(); - if (init_start) - { - phases.push_back( - phases::GoToPlace::make(context, *init_start, start_wp)); - } +// if (init_start) +// { +// phases.push_back( +// phases::GoToPlace::make(context, *init_start, start_wp)); +// } - phases.push_back( - phases::GoToPlace::make(context, loop_start, end_wp)); +// phases.push_back( +// phases::GoToPlace::make(context, loop_start, end_wp)); - for (std::size_t i=1; i < request.num_loops; ++i) - { - assert(loop_end); +// for (std::size_t i=1; i < request.num_loops; ++i) +// { +// assert(loop_end); - phases.push_back( - phases::GoToPlace::make(context, *loop_end, start_wp)); +// phases.push_back( +// phases::GoToPlace::make(context, *loop_end, start_wp)); - phases.push_back( - phases::GoToPlace::make(context, loop_start, end_wp)); - } +// phases.push_back( +// phases::GoToPlace::make(context, loop_start, end_wp)); +// } - return Task::make(request.task_id, std::move(phases), context->worker()); -} +// return Task::make(request.task_id, std::move(phases), context->worker()); +// } -} // namespace tasks -} // naemspace rmf_fleet_adapter +// } // namespace tasks +// } // naemspace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp index 83f14cbb7..2378174f6 100644 --- a/rmf_fleet_adapter/test/test_Task.cpp +++ b/rmf_fleet_adapter/test/test_Task.cpp @@ -180,7 +180,10 @@ class MockSubtaskPhase : _subtasks( rmf_fleet_adapter::Task::make( "subtasks", std::move(phases), - rxcpp::schedulers::make_event_loop().create_worker())) + rxcpp::schedulers::make_event_loop().create_worker(), + std::chrono::steady_clock::now(), + {{std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0}, + nullptr)) { _desc = "subtasks"; _status_obs = _status_publisher.get_observable(); @@ -290,10 +293,15 @@ SCENARIO("Test simple task") phases.push_back(std::make_unique("B", count, 15, dt)); phases.push_back(std::make_unique("C", count, 18, dt)); + // Dummy parameters + rmf_traffic::Time deployment_time; + rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; + std::shared_ptr task = rmf_fleet_adapter::Task::make( "test_Task", std::move(phases), - rxcpp::schedulers::make_event_loop().create_worker()); + rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, + finish_state, nullptr); std::promise completed_promise; auto completed_future = completed_promise.get_future(); @@ -378,9 +386,14 @@ SCENARIO("Test nested task") phases.push_back( std::make_unique(std::move(c_phases))); + // Dummy parameters + rmf_traffic::Time deployment_time; + rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; + const auto task = rmf_fleet_adapter::Task::make( "test_NestedTask", std::move(phases), - rxcpp::schedulers::make_event_loop().create_worker()); + rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, + finish_state, nullptr); std::promise completed_promise; auto completed_future = completed_promise.get_future(); diff --git a/rmf_fleet_msgs/CMakeLists.txt b/rmf_fleet_msgs/CMakeLists.txt index 4a20f5222..831bca927 100644 --- a/rmf_fleet_msgs/CMakeLists.txt +++ b/rmf_fleet_msgs/CMakeLists.txt @@ -25,6 +25,9 @@ set(msg_files "msg/DestinationRequest.msg" "msg/PathRequest.msg" "msg/ModeParameter.msg" + "msg/DockParameter.msg" + "msg/Dock.msg" + "msg/DockSummary.msg" ) # set(srv_files diff --git a/rmf_fleet_msgs/msg/Dock.msg b/rmf_fleet_msgs/msg/Dock.msg new file mode 100644 index 000000000..05837cd2b --- /dev/null +++ b/rmf_fleet_msgs/msg/Dock.msg @@ -0,0 +1,2 @@ +string fleet_name +DockParameter[] params \ No newline at end of file diff --git a/rmf_fleet_msgs/msg/DockParameter.msg b/rmf_fleet_msgs/msg/DockParameter.msg new file mode 100644 index 000000000..42c017b94 --- /dev/null +++ b/rmf_fleet_msgs/msg/DockParameter.msg @@ -0,0 +1,8 @@ +# The name of the waypoint where the docking begins +string start + +# The name of the waypoint where the docking ends +string finish + +# The points in the docking path +Location[] path \ No newline at end of file diff --git a/rmf_fleet_msgs/msg/DockSummary.msg b/rmf_fleet_msgs/msg/DockSummary.msg new file mode 100644 index 000000000..1d4dea1c1 --- /dev/null +++ b/rmf_fleet_msgs/msg/DockSummary.msg @@ -0,0 +1 @@ +Dock[] docks \ No newline at end of file diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index c16af6827..4af624e8a 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -18,8 +18,8 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") include(GNUInstallDirs) diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 19a389176..1cfbe52a7 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -54,8 +54,8 @@ class Request virtual ~Request() = default; }; -using RequestPtr = std::shared_ptr; -using ConstRequestPtr = std::shared_ptr; +using RequestPtr = std::shared_ptr; +using ConstRequestPtr = std::shared_ptr; } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 0232c65ab..a6e820fb1 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -122,20 +122,20 @@ class TaskPlanner /// \param[in] earliest_start_time /// The earliest time the agent will begin exececuting this task Assignment( - rmf_task::RequestPtr request, + rmf_task::ConstRequestPtr request, State state, rmf_traffic::Time deployment_time); // Get the request of this task - rmf_task::RequestPtr request() const; + rmf_task::ConstRequestPtr request() const; // Get a const reference to the state const State& state() const; - // Get a const reference to the time when the robot begins executing + // Get the time when the robot begins executing // this assignment - const rmf_traffic::Time& deployment_time() const; + const rmf_traffic::Time deployment_time() const; class Implementation; @@ -158,7 +158,7 @@ class TaskPlanner rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests); + std::vector requests); /// Get the optimal planner based assignments for a set of initial states and /// requests @@ -170,7 +170,7 @@ class TaskPlanner rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests, + std::vector requests, std::function interrupter); double compute_cost(const Assignments& assignments); diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 959cae7f6..0435bfea4 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -38,7 +38,7 @@ class ChargeBattery : public rmf_task::Request { public: - static rmf_task::Request::SharedPtr make( + static ConstRequestPtr make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr device_sink, @@ -56,13 +56,16 @@ class ChargeBattery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + const rmf_battery::agv::BatterySystem& battery_system() const; + class Implementation; private: ChargeBattery(); rmf_utils::impl_ptr _pimpl; }; -using ChargeBatteryPtr = std::shared_ptr; +using ChargeBatteryRequestPtr = std::shared_ptr; +using ConstChargeBatteryRequestPtr = std::shared_ptr; } // namespace requests } // namespace rmf_task diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 98d148e25..3a1c22743 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -40,7 +40,7 @@ class Clean : public rmf_task::Request { public: - static rmf_task::Request::SharedPtr make( + static ConstRequestPtr make( std::size_t id, std::size_t start_waypoint, std::size_t end_waypoint, @@ -62,13 +62,23 @@ class Clean : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + const std::size_t start_waypoint() const; + + const std::size_t end_waypoint() const; + + rmf_traffic::agv::Planner::Start location_after_clean( + rmf_traffic::agv::Planner::Start start) const; + class Implementation; private: Clean(); rmf_utils::impl_ptr _pimpl; }; -} // namespace tasks +using CleanRequestPtr = std::shared_ptr; +using ConstCleanRequestPtr = std::shared_ptr; + +} // namespace requests } // namespace rmf_task #endif // INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 062840ed0..bc796f07a 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -39,7 +39,7 @@ class Delivery : public rmf_task::Request { public: - static rmf_task::Request::SharedPtr make( + static ConstRequestPtr make( std::size_t id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, @@ -65,6 +65,9 @@ class Delivery : public rmf_task::Request rmf_utils::impl_ptr _pimpl; }; +using DeliveryRequestPtr = std::shared_ptr; +using ConstDeliveryRequestPtr = std::shared_ptr; + } // namespace tasks } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp index c8ec09077..82d002d49 100644 --- a/rmf_task/src/rmf_task/agv/State.cpp +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -37,7 +37,7 @@ class State::Implementation { if (_battery_soc < 0.0 || _battery_soc > 1.0) throw std::invalid_argument( - "Battery State of Charge needs be between 0.0 and 1.0."); + "Battery State of Charge needs to be between 0.0 and 1.0."); } rmf_traffic::agv::Plan::Start _location; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d882648e0..cf8a3b8d0 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -117,14 +117,14 @@ class TaskPlanner::Assignment::Implementation { public: - rmf_task::RequestPtr request; + rmf_task::ConstRequestPtr request; State state; rmf_traffic::Time deployment_time; }; //============================================================================== TaskPlanner::Assignment::Assignment( - rmf_task::RequestPtr request, + rmf_task::ConstRequestPtr request, State state, rmf_traffic::Time deployment_time) : _pimpl(rmf_utils::make_impl( @@ -138,7 +138,7 @@ TaskPlanner::Assignment::Assignment( } //============================================================================== -rmf_task::RequestPtr TaskPlanner::Assignment::request() const +rmf_task::ConstRequestPtr TaskPlanner::Assignment::request() const { return _pimpl->request; } @@ -150,7 +150,7 @@ const State& TaskPlanner::Assignment::state() const } //============================================================================== -const rmf_traffic::Time& TaskPlanner::Assignment::deployment_time() const +const rmf_traffic::Time TaskPlanner::Assignment::deployment_time() const { return _pimpl->deployment_time; } @@ -327,8 +327,8 @@ struct PendingTask PendingTask( std::vector& initial_states, std::vector& state_configs, - rmf_task::Request::SharedPtr request_, - rmf_task::Request::SharedPtr charge_battery_request) + rmf_task::ConstRequestPtr request_, + rmf_task::ConstRequestPtr charge_battery_request) : request(std::move(request_)), candidates(Candidates::make( initial_states, state_configs, *request, *charge_battery_request)), @@ -337,7 +337,7 @@ struct PendingTask // Do nothing } - rmf_task::Request::SharedPtr request; + rmf_task::ConstRequestPtr request; Candidates candidates; rmf_traffic::Time earliest_start_time; }; @@ -610,7 +610,7 @@ class TaskPlanner::Implementation std::shared_ptr config; - RequestPtr make_charging_request(rmf_traffic::Time start_time) + ConstRequestPtr make_charging_request(rmf_traffic::Time start_time) { return rmf_task::requests::ChargeBattery::make( config->battery_system(), @@ -658,7 +658,7 @@ class TaskPlanner::Implementation rmf_traffic::Time time_now, std::vector& initial_states, const std::vector& state_configs, - const std::vector& requests, + const std::vector& requests, const std::function interrupter, bool greedy) { @@ -694,7 +694,7 @@ class TaskPlanner::Implementation if (node->unassigned_tasks.empty()) return prune_assignments(complete_assignments); - std::vector new_tasks; + std::vector new_tasks; for (const auto& u : node->unassigned_tasks) new_tasks.push_back(u.second.request); @@ -788,7 +788,7 @@ class TaskPlanner::Implementation ConstNodePtr make_initial_node( std::vector initial_states, std::vector state_configs, - std::vector requests, + std::vector requests, rmf_traffic::Time time_now) { auto initial_node = std::make_shared(); @@ -1198,7 +1198,7 @@ auto TaskPlanner::greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests) -> Assignments + std::vector requests) -> Assignments { return _pimpl->complete_solve( time_now, @@ -1213,7 +1213,7 @@ auto TaskPlanner::optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests, + std::vector requests, std::function interrupter) -> Assignments { return _pimpl->complete_solve( diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 1d34fb9e2..855976a2c 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -43,7 +43,7 @@ class ChargeBattery::Implementation }; //============================================================================== -rmf_task::Request::SharedPtr ChargeBattery::make( +rmf_task::ConstRequestPtr ChargeBattery::make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr device_sink, @@ -116,7 +116,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( const auto result = _pimpl->_planner->plan(start, goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration variant_duration = finish_time - start_time; + variant_duration = finish_time - start_time; if (_pimpl->_drain_battery) { @@ -143,6 +143,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( state.finish_time( wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge)); + state.battery_soc(_pimpl->_charge_soc); return Estimate(state, wait_until); @@ -161,5 +162,10 @@ rmf_traffic::Time ChargeBattery::earliest_start_time() const } //============================================================================== +const rmf_battery::agv::BatterySystem& ChargeBattery::battery_system() const +{ + return *_pimpl->_battery_system; +} + } // namespace requests } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 931e53f49..f292d00e8 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -46,7 +46,7 @@ class Clean::Implementation }; //============================================================================== -rmf_task::Request::SharedPtr Clean::make( +rmf_task::ConstRequestPtr Clean::make( std::size_t id, std::size_t start_waypoint, std::size_t end_waypoint, @@ -89,8 +89,9 @@ rmf_task::Request::SharedPtr Clean::make( const double dSOC_cleaning = clean->_pimpl->cleaning_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(clean->_pimpl->invariant_duration)); + clean->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_ambient + - dSOC_cleaning; + dSOC_cleaning; } return clean; @@ -122,6 +123,7 @@ rmf_utils::optional Clean::estimate_finish( initial_state.battery_soc()}; 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(); @@ -130,15 +132,10 @@ rmf_utils::optional Clean::estimate_finish( if (initial_state.waypoint() != _pimpl->start_waypoint) { - // Compute plan to cleaning start waypoint along with battery drain - rmf_traffic::agv::Planner::Start start{ - start_time, - initial_state.waypoint(), - 0.0}; - rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; - const auto result_to_start = _pimpl->planner->plan(start, goal); + const auto result_to_start = _pimpl->planner->plan( + initial_state.location(), goal); // We assume we can always compute a plan const auto& trajectory = result_to_start->get_itinerary().back().trajectory(); @@ -153,12 +150,18 @@ rmf_utils::optional Clean::estimate_finish( _pimpl->ambient_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_ambient; - if (battery_soc <= state_config.threshold_soc()) return rmf_utils::nullopt; } } + if (_pimpl->start_waypoint != _pimpl->end_waypoint) + { + // TODO(YV) Account for battery drain and duration when robot moves from + // end of cleaning trajectory to its end_waypoint. We currently define the + // end_waypoint near the start_waypoint in the nav graph for minimum error + } + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; const rmf_traffic::Time wait_until = initial_state.finish_time() > ideal_start ? @@ -166,7 +169,7 @@ rmf_utils::optional Clean::estimate_finish( // Factor in invariants state.finish_time( - wait_until + variant_duration + _pimpl->invariant_duration); + wait_until + variant_duration + _pimpl->invariant_duration + end_duration); if (_pimpl->drain_battery) { @@ -220,6 +223,43 @@ rmf_traffic::Time Clean::earliest_start_time() const return _pimpl->start_time; } +//============================================================================== +const std::size_t Clean::start_waypoint() const +{ + return _pimpl->start_waypoint; +} + +//============================================================================== +const std::size_t Clean::end_waypoint() const +{ + return _pimpl->end_waypoint; +} + +//============================================================================== +rmf_traffic::agv::Planner::Start Clean::location_after_clean( + rmf_traffic::agv::Planner::Start start) const +{ + if (start.waypoint() == _pimpl->start_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; + + const auto result = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const double orientation = trajectory.back().position()[2]; + + rmf_traffic::agv::Planner::Start location_after_clean{ + finish_time + _pimpl->invariant_duration, + _pimpl->start_waypoint, + orientation}; + + return location_after_clean; + +} + //============================================================================== } // namespace requests } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 980b3160e..230a5e0c1 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -44,7 +44,7 @@ class Delivery::Implementation }; //============================================================================== -rmf_task::Request::SharedPtr Delivery::make( +rmf_task::ConstRequestPtr Delivery::make( std::size_t id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 360d51a6c..b28970437 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -150,7 +150,7 @@ SCENARIO("Grid World") rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { rmf_task::requests::Delivery::make( 1, @@ -198,7 +198,7 @@ SCENARIO("Grid World") const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - + display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost); @@ -225,7 +225,7 @@ SCENARIO("Grid World") rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { rmf_task::requests::Delivery::make( 1, @@ -353,7 +353,7 @@ SCENARIO("Grid World") const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - + display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost); diff --git a/rmf_task_msgs/msg/TaskType.msg b/rmf_task_msgs/msg/TaskType.msg index ac3733bbe..a180229b5 100644 --- a/rmf_task_msgs/msg/TaskType.msg +++ b/rmf_task_msgs/msg/TaskType.msg @@ -2,7 +2,7 @@ uint32 type uint32 TYPE_STATION=0 uint32 TYPE_LOOP=1 uint32 TYPE_DELIVERY=2 -uint32 TYPE_CHARGING=3 -uint32 TYPE_CLEANING=4 +uint32 TYPE_CHARGE_BATTERY=3 +uint32 TYPE_CLEAN=4 uint32 TYPE_PATROL=5 diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index c83dcb086..c6221cfd0 100644 --- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp +++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp @@ -78,6 +78,15 @@ class Graph /// Set this Waypoint to be a parking spot. Waypoint& set_parking_spot(bool _is_parking_spot); + + /// Returns true if this Waypoint is a charger spot. Robots are routed to + /// these spots when their batteries charge levels drop below the threshold + /// value. + bool is_charger() const; + + /// Set this Waypoint to be a parking spot. + Waypoint& set_charger(bool _is_charger); + /// The index of this waypoint within the Graph. This cannot be changed /// after the waypoint is created. std::size_t index() const; diff --git a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp index 0b919d493..79bc39bcf 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp @@ -44,6 +44,8 @@ class Graph::Waypoint::Implementation bool parking_spot = false; + bool charger = false; + template static Waypoint make(Args&& ... args) { @@ -125,6 +127,19 @@ auto Graph::Waypoint::set_parking_spot(bool _is_parking_spot) -> Waypoint& return *this; } +//============================================================================== +bool Graph::Waypoint::is_charger() const +{ + return _pimpl->charger; +} + +//============================================================================== +auto Graph::Waypoint::set_charger(bool _is_charger) -> Waypoint& +{ + _pimpl->charger = _is_charger; + return *this; +} + //============================================================================== std::size_t Graph::Waypoint::index() const { From 5e4a6bba7aeaeb62216d1ba444b2756f33e3050e Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 5 Nov 2020 18:18:09 +0800 Subject: [PATCH 063/128] ChargeBattery task creates WaitForCharge phase with 0.99 max threshold. Fixed valid() check in PowerSystem --- rmf_battery/src/rmf_battery/agv/PowerSystem.cpp | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp index b7fc41876..b5afc1f15 100644 --- a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -66,7 +66,7 @@ double PowerSystem::nominal_power() const //============================================================================== bool PowerSystem::valid() const { - return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0; + return !_pimpl->name.empty() && _pimpl->nominal_power >= 0.0; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 0414a38a2..9b3733acd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -38,7 +38,7 @@ std::shared_ptr make_charge_battery( phases.push_back( phases::GoToPlace::make(context, std::move(start), goal)); phases.push_back( - phases::WaitForCharge::make(context, request->battery_system(), 1.0)); + phases::WaitForCharge::make(context, request->battery_system(), 0.99)); return Task::make( std::to_string(request->id()), From febea6d2addbb32e43ce56913cfec084b04ad406 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 6 Nov 2020 10:33:30 +0800 Subject: [PATCH 064/128] Added equality constraint in PowerSystem::valid() --- rmf_battery/src/rmf_battery/agv/PowerSystem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp index b7fc41876..6026a51f2 100644 --- a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -66,9 +66,9 @@ double PowerSystem::nominal_power() const //============================================================================== bool PowerSystem::valid() const { - return !_pimpl->name.empty() && _pimpl->nominal_power > 0.0; + return !_pimpl->name.empty() && _pimpl->nominal_power >= 0.0; } } // namespace agv -} // namespace rmf_battery \ No newline at end of file +} // namespace rmf_battery From 15cbc5bfb800a139c47809e1f641a58df66ba9c3 Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Fri, 6 Nov 2020 14:23:20 +0800 Subject: [PATCH 065/128] Add unit test to detect implicit charging task at the start of Assignments (#208) * Add unit test to detect implicit charging task in the beginning Test checks for discrepancy in state of charge to see if charging has been taken into account without being explicitly added to Assignments. * Fix false negative in unit test case for implicit charging task Checks if task is a charging task before returning an error. --- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 129 ++++++++++++++++++++ 1 file changed, 129 insertions(+) diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 360d51a6c..5f2819ab2 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -40,6 +40,38 @@ #include + +//============================================================================== +inline bool check_implicit_charging_task_start( + const rmf_task::agv::TaskPlanner::Assignments& assignments, + const double initial_soc) +{ + bool implicit_charging_task_added = false; + for (const auto& agent : assignments) + { + if (!agent.size()) + { + continue; + } + + const auto& s = agent[0].state(); + auto is_charge_request = + std::dynamic_pointer_cast( + agent[0].request()); + + // No task should consume more charge than (1.0 - initial_soc) + // in the current test, so we are guaranted to find any occurrence + // of an implicit charging task. + if (!is_charge_request && s.battery_soc() > initial_soc) + { + implicit_charging_task_added = true; + break; + } + } + + return implicit_charging_task_added; +} + //============================================================================== inline void display_solution( std::string parent, @@ -359,4 +391,101 @@ SCENARIO("Grid World") REQUIRE(optimal_cost <= greedy_cost); } + + WHEN("Initial charge is low") + { + const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + const double initial_soc = 0.3; + + 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 = + { + rmf_task::agv::State{first_location, 13, initial_soc}, + rmf_task::agv::State{second_location, 2, initial_soc} + }; + + std::vector state_configs = + { + rmf_task::agv::StateConfig{0.2}, + rmf_task::agv::StateConfig{0.2} + }; + + std::vector requests = + { + rmf_task::requests::Delivery::make( + 1, + 0, + 3, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + 2, + 15, + 2, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + 3, + 9, + 4, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + 4, + 8, + 11, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery) + }; + + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + rmf_task::agv::TaskPlanner task_planner(task_config); + + const auto greedy_assignments = task_planner.greedy_plan( + now, initial_states, state_configs, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); + + const auto optimal_assignments = task_planner.optimal_plan( + now, initial_states, state_configs, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); + + display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Optimal", optimal_assignments, optimal_cost); + + REQUIRE(optimal_cost <= greedy_cost); + + // Checks if Assignments take into account a charging task in the beginning + // without explicitly including the task in Assignments. + bool implicit_charging_task_added = check_implicit_charging_task_start( + greedy_assignments, initial_soc); + REQUIRE(!implicit_charging_task_added); + + implicit_charging_task_added = check_implicit_charging_task_start( + optimal_assignments, initial_soc); + REQUIRE(!implicit_charging_task_added); + } + } \ No newline at end of file From 4a9ada755469994e059558d6c946d37fdb5dae99 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 6 Nov 2020 17:13:43 +0800 Subject: [PATCH 066/128] Deal with implicit charging before first assignment (#209) --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 59 +++++++++++++++++++---- 1 file changed, 50 insertions(+), 9 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d882648e0..dd113a6cb 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -185,6 +185,8 @@ class Candidates std::size_t candidate; State state; rmf_traffic::Time wait_until; + State previous_state; + bool require_charge_battery = false; }; // Map finish time to Entry @@ -243,14 +245,16 @@ class Candidates void update_candidate( std::size_t candidate, State state, - rmf_traffic::Time wait_until) + rmf_traffic::Time wait_until, + State previous_state, + bool require_charge_battery) { const auto it = _candidate_map.at(candidate); _value_map.erase(it); _candidate_map[candidate] = _value_map.insert( { state.finish_time(), - Entry{candidate, state, wait_until} + Entry{candidate, state, wait_until, previous_state, require_charge_battery} }); } @@ -293,7 +297,12 @@ Candidates Candidates::make( { initial_map.insert({ finish.value().finish_state().finish_time(), - Entry{i, finish.value().finish_state(), finish.value().wait_until()}}); + Entry{ + i, + finish.value().finish_state(), + finish.value().wait_until(), + state, + false}}); } else { @@ -306,7 +315,12 @@ Candidates Candidates::make( assert(new_finish.has_value()); initial_map.insert( {new_finish.value().finish_state().finish_time(), - Entry{i, new_finish.value().finish_state(), new_finish.value().wait_until()}}); + Entry{ + i, + new_finish.value().finish_state(), + new_finish.value().wait_until(), + state, + true}}); } else { @@ -865,6 +879,7 @@ class TaskPlanner::Implementation { const auto& entry = it->second; + const auto& state_config = state_configs[entry.candidate]; if (parent->latest_time + segmentation_threshold < entry.wait_until) { @@ -875,7 +890,28 @@ class TaskPlanner::Implementation auto new_node = std::make_shared(*parent); - // Assign the unassigned task + // Assign the unassigned task after checking for implicit charging requests + if (entry.require_charge_battery) + { + // Check if a battery task already precedes the latest assignment + auto& assignments = new_node->assigned_tasks[entry.candidate]; + if (assignments.empty() || !std::dynamic_pointer_cast( + assignments.back().request())) + { + auto charge_battery = make_charging_request(entry.previous_state.finish_time()); + auto battery_estimate = charge_battery->estimate_finish(entry.previous_state, state_config); + if (battery_estimate.has_value()) + { + assignments.push_back( + Assignment + { + charge_battery, + battery_estimate.value().finish_state(), + battery_estimate.value().wait_until() + }); + } + } + } new_node->assigned_tasks[entry.candidate].push_back( Assignment{u.second.request, entry.state, entry.wait_until}); @@ -883,7 +919,6 @@ class TaskPlanner::Implementation new_node->pop_unassigned(u.first); // Update states of unassigned tasks for the candidate - const auto& state_config = state_configs[entry.candidate]; bool add_charger = false; for (auto& new_u : new_node->unassigned_tasks) { @@ -896,7 +931,9 @@ class TaskPlanner::Implementation new_u.second.candidates.update_candidate( entry.candidate, finish.value().finish_state(), - finish.value().wait_until()); + finish.value().wait_until(), + entry.state, + false); } else { @@ -946,7 +983,7 @@ class TaskPlanner::Implementation if (finish.has_value()) { new_u.second.candidates.update_candidate( - entry.candidate, finish.value().finish_state(), finish.value().wait_until()); + entry.candidate, finish.value().finish_state(), finish.value().wait_until(), entry.state, false); } else { @@ -1016,7 +1053,11 @@ class TaskPlanner::Implementation if (finish.has_value()) { new_u.second.candidates.update_candidate( - agent, finish.value().finish_state(), finish.value().wait_until()); + agent, + finish.value().finish_state(), + finish.value().wait_until(), + state, + false); } else { From 005e88bada328e2989f0b29aeabdd9b7fc6df877 Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Tue, 10 Nov 2020 12:08:34 +0800 Subject: [PATCH 067/128] Reduce heuristic overestimation and ignore charging task costs in TaskPlanner (#207) * Improved heuristic to reduce overestimation Takes into account earliest start time when computing the cost of a task, more closely mimicing g(n). * Add field to Assignment determining whether to include in final cost Enables charging tasks to be ignored from final calculation. Adds miscellaneous comments explaining h(n) calculation. * Remove additional include_in_cost field from Assignment Use dynamic_pointer_cast to identify charging requests instead. * Add testcase illustrating difference in costs with new g(n) and h(n) * Include smallest variant cost in h(n) computation Include the smallest possible variant cost among the unasigned tasks for each candidate when computing the heuristic cost. --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 1 - rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 83 ++++++---- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 155 ++++++++++++++++++ 3 files changed, 207 insertions(+), 32 deletions(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 0232c65ab..965727f37 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -125,7 +125,6 @@ class TaskPlanner rmf_task::RequestPtr request, State state, rmf_traffic::Time deployment_time); - // Get the request of this task rmf_task::RequestPtr request() const; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index dd113a6cb..b937ca52f 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -163,7 +163,8 @@ namespace { struct Invariant { std::size_t task_id; - double invariant_cost; + double earliest_start_time; + double earliest_finish_time; }; // ============================================================================ @@ -171,7 +172,7 @@ struct InvariantLess { bool operator()(const Invariant& a, const Invariant& b) const { - return a.invariant_cost < b.invariant_cost; + return a.earliest_finish_time < b.earliest_finish_time; } }; @@ -375,10 +376,16 @@ struct Node unassigned_invariants.clear(); for (const auto& u : unassigned_tasks) { + double earliest_start_time = rmf_traffic::time::to_seconds( + u.second.earliest_start_time.time_since_epoch()); + double earliest_finish_time = earliest_start_time + + rmf_traffic::time::to_seconds(u.second.request->invariant_duration()); + unassigned_invariants.insert( Invariant{ - u.first, - rmf_traffic::time::to_seconds(u.second.request->invariant_duration()) + u.first, + earliest_start_time, + earliest_finish_time }); } } @@ -418,6 +425,10 @@ struct LowestCostEstimate }; //============================================================================== +// Sorts and distributes tasks among agents based on the earliest finish time +// possible for each task (i.e. not accounting for any variant costs). Guaranteed +// to underestimate actual cost when the earliest start times for each task are +// similar (enforced by the segmentation_threshold). class InvariantHeuristicQueue { public: @@ -428,21 +439,21 @@ class InvariantHeuristicQueue std::sort(initial_values.begin(), initial_values.end()); for (const auto value : initial_values) - _stacks.push_back({value}); + _stacks.push_back({{0, value}}); } - void add(double new_value) + void add(const double earliest_start_time, const double earliest_finish_time) { - // Add the new value to the smallest stack - const double value = _stacks[0].back() + new_value; - _stacks[0].push_back(value); + double prev_end_value = _stacks[0].back().end; + double new_end_value = prev_end_value + (earliest_finish_time - earliest_start_time); + _stacks[0].push_back({earliest_start_time, new_end_value}); // Find the largest stack that is still smaller than the current front const auto next_it = _stacks.begin() + 1; auto end_it = next_it; for (; end_it != _stacks.end(); ++end_it) { - if (value <= end_it->back()) + if (new_end_value <= end_it->back().end) break; } @@ -462,15 +473,20 @@ class InvariantHeuristicQueue // NOTE: We start iterating from i=1 because i=0 represents a component of // the cost that is already accounted for by g(n) and the variant // component of h(n) - for (std::size_t i=1; i < stack.size(); ++i) - total_cost += stack[i]; + for (std::size_t i = 1; i < stack.size(); ++i) + { + // Set lower bound of 0 to account for case where optimistically calculated + // end time is smaller than earliest start time + total_cost += std::max(0.0, (stack[i].end - stack[i].start)); + } } return total_cost; } private: - std::vector> _stacks; + struct element { double start; double end; }; + std::vector> _stacks; }; // ============================================================================ @@ -642,6 +658,12 @@ class TaskPlanner::Implementation { for (const auto& assignment : agent) { + if (std::dynamic_pointer_cast( + assignment.request())) + { + continue; // Ignore charging tasks in cost + } + cost += rmf_traffic::time::to_seconds( assignment.state().finish_time() - assignment.request()->earliest_start_time()); @@ -742,31 +764,30 @@ class TaskPlanner::Implementation double compute_h(const Node& node, const rmf_traffic::Time time_now) { - std::vector initial_queue_values; - initial_queue_values.resize( - node.assigned_tasks.size(), std::numeric_limits::infinity()); + std::vector initial_queue_values( + node.assigned_tasks.size(), std::numeric_limits::infinity()); + // Determine the earliest possible time an agent can begin the invariant + // portion of any of its next tasks for (const auto& u : node.unassigned_tasks) { - // We subtract the invariant duration here because otherwise its - // contribution to the cost estimate will be duplicated in the next section, - // which could result in an overestimate. - const rmf_traffic::Time variant_time = + const rmf_traffic::Time earliest_deployment_time = u.second.candidates.best_finish_time() - u.second.request->invariant_duration(); - const double variant_value = - rmf_traffic::time::to_seconds(variant_time - time_now); + const double earliest_deployment_time_s = + rmf_traffic::time::to_seconds( + earliest_deployment_time.time_since_epoch()); const auto& range = u.second.candidates.best_candidates(); for (auto it = range.begin; it != range.end; ++it) { const std::size_t candidate = it->second.candidate; - if (variant_value < initial_queue_values[candidate]) - initial_queue_values[candidate] = variant_value; + if (earliest_deployment_time_s < initial_queue_values[candidate]) + initial_queue_values[candidate] = earliest_deployment_time_s; } } - for (std::size_t i=0; i < initial_queue_values.size(); ++i) + for (std::size_t i = 0; i < initial_queue_values.size(); ++i) { auto& value = initial_queue_values[i]; if (std::isinf(value)) @@ -775,11 +796,10 @@ class TaskPlanner::Implementation // any unassigned tasks that want to use it. const auto& assignments = node.assigned_tasks[i]; if (assignments.empty()) - value = 0.0; + value = rmf_traffic::time::to_seconds(time_now.time_since_epoch()); else - value = - rmf_traffic::time::to_seconds( - assignments.back().state().finish_time() - time_now); + value = rmf_traffic::time::to_seconds( + assignments.back().state().finish_time().time_since_epoch()); } } @@ -789,8 +809,9 @@ class TaskPlanner::Implementation // to it in order of smallest to largest. If that assumption is not met, then // the final cost that's calculated may be invalid. for (const auto& u : node.unassigned_invariants) - queue.add(u.invariant_cost); - + { + queue.add(u.earliest_start_time, u.earliest_finish_time); + } return queue.compute_cost(); } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 5f2819ab2..1ff21bd7b 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -488,4 +488,159 @@ SCENARIO("Grid World") REQUIRE(!implicit_charging_task_added); } + WHEN("Planning for 11 requests and 2 agents no.2") + { + const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + 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 = + { + rmf_task::agv::State{first_location, 9, 1.0}, + rmf_task::agv::State{second_location, 2, 1.0} + }; + + std::vector state_configs = + { + rmf_task::agv::StateConfig{0.2}, + rmf_task::agv::StateConfig{0.2} + }; + + std::vector requests = + { + rmf_task::requests::Delivery::make( + 1, + 6, + 3, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + 2, + 10, + 7, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + 3, + 2, + 12, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + 4, + 8, + 11, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery), + + rmf_task::requests::Delivery::make( + 5, + 10, + 6, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery), + + rmf_task::requests::Delivery::make( + 6, + 2, + 9, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + 7, + 3, + 4, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + 8, + 5, + 11, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + 9, + 9, + 1, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + 10, + 1, + 5, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + 11, + 13, + 10, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery) + }; + + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + rmf_task::agv::TaskPlanner task_planner(task_config); + + const auto greedy_assignments = task_planner.greedy_plan( + now, initial_states, state_configs, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); + + const auto optimal_assignments = task_planner.optimal_plan( + now, initial_states, state_configs, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); + + display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Optimal", optimal_assignments, optimal_cost); + + REQUIRE(optimal_cost <= greedy_cost); + } + } \ No newline at end of file From 54e1c32054580e6b2e49ddb43e9b024584b55da7 Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 12 Nov 2020 15:50:01 +0800 Subject: [PATCH 068/128] Loop and Delivery integration with dispatcher framework (#211) * Added creation of delivery request * Modified make and extended API for Delivery request * Added items() and dropoff_start() to Delivery request * Deleted old Delivery task implementation * Loop request skeleton * Implemented Loop request * Parse loop request in FleetUpdateHandle * Integrated Loop requests * Cleanup make_loop --- .../src/rmf_fleet_adapter/TaskManager.cpp | 42 ++- .../agv/FleetUpdateHandle.cpp | 182 ++++++++++- .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 139 ++++---- .../src/rmf_fleet_adapter/tasks/Delivery.hpp | 11 +- .../src/rmf_fleet_adapter/tasks/Loop.cpp | 113 ++++--- .../src/rmf_fleet_adapter/tasks/Loop.hpp | 11 +- rmf_task/CMakeLists.txt | 7 + .../include/rmf_task/requests/Delivery.hpp | 20 ++ rmf_task/include/rmf_task/requests/Loop.hpp | 88 ++++++ rmf_task/package.xml | 1 + rmf_task/src/rmf_task/requests/Clean.cpp | 8 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 112 +++++-- rmf_task/src/rmf_task/requests/Loop.cpp | 299 ++++++++++++++++++ rmf_task/test/unit/agv/test_TaskPlanner.cpp | 54 ++++ 14 files changed, 922 insertions(+), 165 deletions(-) create mode 100644 rmf_task/include/rmf_task/requests/Loop.hpp create mode 100644 rmf_task/src/rmf_task/requests/Loop.cpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 26f46916d..67539f596 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -20,11 +20,14 @@ #include #include #include +#include #include #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" +#include "tasks/Delivery.hpp" +#include "tasks/Loop.hpp" #include @@ -195,13 +198,50 @@ void TaskManager::set_queue( msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); } + this->_context->node()->task_summary()->publish(msg); + + } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + const auto task = tasks::make_delivery( + request, + _context, + start, + a.deployment_time(), + a.state()); + + _queue.push_back(task); + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); + + } + + else if (const auto request = + std::dynamic_pointer_cast(a.request())) + { + const auto task = tasks::make_loop( + request, + _context, + start, + a.deployment_time(), + a.state()); + + _queue.push_back(task); + + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); } else diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index da85355f3..adcb882d9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -22,6 +22,14 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" +#include +#include +#include + +#include +#include +#include + #include #include @@ -236,11 +244,180 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { - // TODO(YV) + const auto& delivery = task_profile.delivery; + if (delivery.pickup_place_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.pickup_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.pickup_dispenser.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.pickup_dispenser] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.dropoff_place_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.dropoff_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.dropoff_place_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.dropoff_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.dropoff_ingestor.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.dropoff_ingestor] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + const auto pickup_wp = graph.find_waypoint(delivery.pickup_place_name); + if (!pickup_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), delivery.pickup_place_name.c_str(), id.c_str()); + + return; + } + + const auto dropoff_wp = graph.find_waypoint(delivery.dropoff_place_name); + if (!dropoff_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), delivery.dropoff_place_name.c_str(), id.c_str()); + + return; + } + + // TODO(YV) get rid of id field in RequestPtr + std::stringstream id_stream(id); + std::size_t request_id; + id_stream >> request_id; + + new_request = rmf_task::requests::Delivery::make( + request_id, + pickup_wp->index(), + delivery.pickup_dispenser, + dropoff_wp->index(), + delivery.dropoff_ingestor, + delivery.items, + motion_sink, + ambient_sink, + planner, + start_time, + drain_battery); + + RCLCPP_INFO( + node->get_logger(), + "Generated Delivery request"); + } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { - // TODO(YV) + const auto& loop = task_profile.loop; + if (loop.start_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [loop.start_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (loop.finish_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [loop.finish_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (loop.num_loops < 1) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [loop.num_loops] in TaskProfile is invalid." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + const auto start_wp = graph.find_waypoint(loop.start_name); + if (!start_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), loop.start_name.c_str(), id.c_str()); + + return; + } + + const auto finish_wp = graph.find_waypoint(loop.finish_name); + if (!finish_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), loop.finish_name.c_str(), id.c_str()); + + return; + } + + // TODO(YV) get rid of id field in RequestPtr + std::stringstream id_stream(id); + std::size_t request_id; + id_stream >> request_id; + + new_request = rmf_task::requests::Loop::make( + request_id, + start_wp->index(), + finish_wp->index(), + loop.num_loops, + motion_sink, + ambient_sink, + planner, + start_time, + drain_battery); + + RCLCPP_INFO( + node->get_logger(), + "Generated Loop request"); } else { @@ -354,7 +531,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } - void FleetUpdateHandle::Implementation::dispatch_request_cb( const DispatchRequest::SharedPtr msg) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index edf93b4d3..a25cce7f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -1,79 +1,84 @@ -// /* -// * 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. -// * -// */ +/* + * 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 "../phases/DispenseItem.hpp" -// #include "../phases/IngestItem.hpp" -// #include "../phases/GoToPlace.hpp" +#include "../phases/DispenseItem.hpp" +#include "../phases/IngestItem.hpp" +#include "../phases/GoToPlace.hpp" -// #include "Delivery.hpp" +#include "Delivery.hpp" -// namespace rmf_fleet_adapter { -// namespace tasks { +#include -// //============================================================================== -// std::shared_ptr make_delivery( -// const rmf_task_msgs::msg::Delivery& request, -// const agv::RobotContextPtr& context, -// rmf_traffic::agv::Plan::Start pickup_start, -// rmf_traffic::agv::Plan::Start dropoff_start) -// { -// const auto& graph = context->navigation_graph(); +namespace rmf_fleet_adapter { +namespace tasks { -// const auto pickup_wp = -// graph.find_waypoint(request.pickup_place_name)->index(); +//============================================================================== +std::shared_ptr make_delivery( + const rmf_task::requests::ConstDeliveryRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start pickup_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state) +{ + Task::PendingPhases phases; + phases.push_back( + phases::GoToPlace::make( + context, std::move(pickup_start), request->pickup_waypoint())); -// Task::PendingPhases phases; -// phases.push_back( -// phases::GoToPlace::make(context, std::move(pickup_start), pickup_wp)); + phases.push_back( + std::make_unique( + context, + std::to_string(request->id()), + request->pickup_dispenser(), + context->itinerary().description().owner(), + request->items())); -// phases.push_back( -// std::make_unique( -// context, -// request.task_id, -// request.pickup_dispenser, -// context->itinerary().description().owner(), -// request.items)); + auto dropoff_start = request->dropoff_start(pickup_start); + phases.push_back( + phases::GoToPlace::make( + context, std::move(dropoff_start), request->dropoff_waypoint())); -// const auto dropoff_wp = -// graph.find_waypoint(request.dropoff_place_name)->index(); -// phases.push_back( -// phases::GoToPlace::make(context, std::move(dropoff_start), dropoff_wp)); + std::vector ingestor_items; + ingestor_items.reserve(request->items().size()); + for(const auto& dispenser_item : request->items()){ + rmf_ingestor_msgs::msg::IngestorRequestItem item{}; + item.type_guid = dispenser_item.type_guid; + item.quantity = dispenser_item.quantity; + item.compartment_name = dispenser_item.compartment_name; + ingestor_items.push_back(std::move(item)); + } -// std::vector ingestor_items; -// ingestor_items.reserve(request.items.size()); -// for(auto& dispenser_item : request.items){ -// rmf_ingestor_msgs::msg::IngestorRequestItem item{}; -// item.type_guid = dispenser_item.type_guid; -// item.quantity = dispenser_item.quantity; -// item.compartment_name = dispenser_item.compartment_name; -// ingestor_items.push_back(std::move(item)); -// } + phases.push_back( + std::make_unique( + context, + std::to_string(request->id()), + request->dropoff_ingestor(), + context->itinerary().description().owner(), + ingestor_items)); -// phases.push_back( -// std::make_unique( -// context, -// request.task_id, -// request.dropoff_ingestor, -// context->itinerary().description().owner(), -// ingestor_items)); + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} -// return Task::make(request.task_id, std::move(phases), context->worker()); -// } - -// } // namespace task -// } // namespace rmf_fleet_adapter +} // namespace task +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp index 49dfe995a..22b03ecd2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp @@ -21,19 +21,18 @@ #include "../Task.hpp" #include "../agv/RobotContext.hpp" -#include +#include namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -// TODO(MXG): This is a sloppy design. We should have a task estimator + factory -// interface to handle the task dispatch and creation pipeline more elegantly. std::shared_ptr make_delivery( - const rmf_task_msgs::msg::Delivery& request, + const rmf_task::requests::ConstDeliveryRequestPtr request, const agv::RobotContextPtr& context, - rmf_traffic::agv::Plan::Start pickup_start, - rmf_traffic::agv::Plan::Start dropoff_start); + const rmf_traffic::agv::Plan::Start pickup_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index b5f9397cb..0ff429944 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -1,68 +1,67 @@ -// /* -// * 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. -// * -// */ +/* + * 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 "Loop.hpp" +#include "Loop.hpp" -// #include "../phases/GoToPlace.hpp" +#include "../phases/GoToPlace.hpp" -// namespace rmf_fleet_adapter { -// namespace tasks { +namespace rmf_fleet_adapter { +namespace tasks { -// //============================================================================== -// std::shared_ptr make_loop( -// const rmf_task_msgs::msg::Loop& request, -// const agv::RobotContextPtr& context, -// rmf_utils::optional init_start, -// rmf_traffic::agv::Plan::Start loop_start, -// rmf_utils::optional loop_end) -// { -// const auto& graph = context->navigation_graph(); +//============================================================================== +std::shared_ptr make_loop( + const rmf_task::requests::ConstLoopRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state) +{ -// Task::PendingPhases phases; + Task::PendingPhases phases; + const auto loop_start = request->loop_start(start); + const auto loop_end = request->loop_end(loop_start); -// const auto start_wp = -// graph.find_waypoint(request.start_name)->index(); + phases.push_back( + phases::GoToPlace::make( + context, std::move(start), request->start_waypoint())); -// const auto end_wp = -// graph.find_waypoint(request.finish_name)->index(); + phases.push_back( + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); -// if (init_start) -// { -// phases.push_back( -// phases::GoToPlace::make(context, *init_start, start_wp)); -// } + for (std::size_t i = 1; i < request->num_loops(); ++i) + { + phases.push_back( + phases::GoToPlace::make( + context, loop_end, request->start_waypoint())); + phases.push_back( + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); + } -// phases.push_back( -// phases::GoToPlace::make(context, loop_start, end_wp)); + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} -// for (std::size_t i=1; i < request.num_loops; ++i) -// { -// assert(loop_end); - -// phases.push_back( -// phases::GoToPlace::make(context, *loop_end, start_wp)); - -// phases.push_back( -// phases::GoToPlace::make(context, loop_start, end_wp)); -// } - -// return Task::make(request.task_id, std::move(phases), context->worker()); -// } - -// } // namespace tasks -// } // naemspace rmf_fleet_adapter +} // namespace tasks +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp index 6ba1df91c..19c3bf17d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp @@ -21,17 +21,18 @@ #include "../Task.hpp" #include "../agv/RobotContext.hpp" -#include +#include namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_loop(const rmf_task_msgs::msg::Loop& request, +std::shared_ptr make_loop( + const rmf_task::requests::ConstLoopRequestPtr request, const agv::RobotContextPtr& context, - rmf_utils::optional init_start, - rmf_traffic::agv::Plan::Start loop_start, - rmf_utils::optional loop_end); + const rmf_traffic::agv::Plan::Start start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 4af624e8a..f7e1f2138 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -27,6 +27,7 @@ include(GNUInstallDirs) find_package(rmf_utils REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_battery REQUIRED) +find_package(rmf_dispenser_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(ament_cmake_catch2 QUIET) @@ -46,6 +47,8 @@ add_library(rmf_task SHARED target_link_libraries(rmf_task PUBLIC rmf_battery::rmf_battery + PRIVATE + ${rmf_dispenser_msgs_LIBRARIES} ) target_include_directories(rmf_task @@ -53,6 +56,8 @@ target_include_directories(rmf_task $ $ ${EIGEN3_INCLUDE_DIRS} + PRIVATE + ${rmf_dispenser_msgs_INCLUDE_DIRS} ) if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) @@ -65,11 +70,13 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) rmf_task rmf_utils::rmf_utils rmf_traffic::rmf_traffic + ${rmf_dispenser_msgs_LIBRARIES} ) target_include_directories(test_rmf_task PRIVATE $ + ${rmf_dispenser_msgs_INCLUDE_DIRS} ) find_file(uncrustify_config_file NAMES "share/format/rmf_code_style.cfg") diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index bc796f07a..0b89f5a0f 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -32,6 +32,8 @@ #include #include +#include + namespace rmf_task { namespace requests { @@ -39,10 +41,16 @@ class Delivery : public rmf_task::Request { public: + using DispenserRequestItem = rmf_dispenser_msgs::msg::DispenserRequestItem; + using Start = rmf_traffic::agv::Planner::Start; + static ConstRequestPtr make( std::size_t id, std::size_t pickup_waypoint, + std::string pickup_dispenser, std::size_t dropoff_waypoint, + std::string dropoff_ingestor, + std::vector items, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -59,6 +67,18 @@ class Delivery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + std::size_t pickup_waypoint() const; + + const std::string& pickup_dispenser() const; + + std::size_t dropoff_waypoint() const; + + const std::string& dropoff_ingestor() const; + + const std::vector& items() const; + + Start dropoff_start(const Start& start) const; + class Implementation; private: Delivery(); diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp new file mode 100644 index 000000000..1c1860ba5 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -0,0 +1,88 @@ +/* + * 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 INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP +#define INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +namespace rmf_task { +namespace requests { + +class Loop : public rmf_task::Request +{ +public: + + using Start = rmf_traffic::agv::Planner::Start; + + static ConstRequestPtr make( + std::size_t id, + std::size_t start_waypoint, + std::size_t finish_waypoint, + std::size_t num_loops, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner, + rmf_traffic::Time start_time, + bool drain_battery = true); + + std::size_t id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::StateConfig& state_config) const final; + + rmf_traffic::Duration invariant_duration() const final; + + rmf_traffic::Time earliest_start_time() const final; + + std::size_t start_waypoint() const; + + std::size_t finish_waypoint() const; + + std::size_t num_loops() const; + + Start loop_start(const Start& start) const; + + Start loop_end(const Start& start) const; + + class Implementation; +private: + Loop(); + rmf_utils::impl_ptr _pimpl; +}; + +using LoopRequestPtr = std::shared_ptr; +using ConstLoopRequestPtr = std::shared_ptr; + +} // namespace tasks +} // namespace rmf_task + +#endif // INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP diff --git a/rmf_task/package.xml b/rmf_task/package.xml index 658107312..d9deef5a9 100644 --- a/rmf_task/package.xml +++ b/rmf_task/package.xml @@ -10,6 +10,7 @@ rmf_battery rmf_utils + rmf_dispenser_msgs eigen diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index f292d00e8..66ca57f14 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -245,10 +245,10 @@ rmf_traffic::agv::Planner::Start Clean::location_after_clean( rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; const auto result = _pimpl->planner->plan(start, goal); - // We assume we can always compute a plan - const auto& trajectory = - result->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); + // We assume we can always compute a plan + const auto& trajectory = + result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); const double orientation = trajectory.back().position()[2]; rmf_traffic::agv::Planner::Start location_after_clean{ diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 230a5e0c1..fb15278f5 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -32,7 +32,10 @@ class Delivery::Implementation std::size_t _id; std::size_t _pickup_waypoint; + std::string _pickup_dispenser; std::size_t _dropoff_waypoint; + std::string _dropoff_ingestor; + std::vector _items; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; @@ -47,7 +50,10 @@ class Delivery::Implementation rmf_task::ConstRequestPtr Delivery::make( std::size_t id, std::size_t pickup_waypoint, + std::string pickup_dispenser, std::size_t dropoff_waypoint, + std::string dropoff_ingestor, + std::vector items, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -57,7 +63,10 @@ rmf_task::ConstRequestPtr Delivery::make( std::shared_ptr delivery(new Delivery()); delivery->_pimpl->_id = id; delivery->_pimpl->_pickup_waypoint = pickup_waypoint; + delivery->_pimpl->_pickup_dispenser = std::move(pickup_dispenser); delivery->_pimpl->_dropoff_waypoint = dropoff_waypoint; + delivery->_pimpl->_dropoff_ingestor = std::move(dropoff_ingestor); + delivery->_pimpl->_items = std::move(items); delivery->_pimpl->_motion_sink = std::move(motion_sink); delivery->_pimpl->_device_sink = std::move(device_sink); delivery->_pimpl->_planner = std::move(planner); @@ -65,30 +74,35 @@ rmf_task::ConstRequestPtr Delivery::make( delivery->_pimpl->_start_time = start_time; // Calculate duration of invariant component of task - const auto plan_start_time = std::chrono::steady_clock::now(); - rmf_traffic::agv::Planner::Start start{ - plan_start_time, - delivery->_pimpl->_pickup_waypoint, - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; - const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); - - const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - - delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + delivery->_pimpl->_invariant_duration = rmf_traffic::Duration{0}; delivery->_pimpl->_invariant_battery_drain = 0.0; - if (delivery->_pimpl->_drain_battery) + if (delivery->_pimpl->_pickup_waypoint != delivery->_pimpl->_dropoff_waypoint) { - // Compute battery drain - const double dSOC_motion = - delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); - const double dSOC_device = - delivery->_pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); - delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + const auto plan_start_time = std::chrono::steady_clock::now(); + rmf_traffic::agv::Planner::Start start{ + plan_start_time, + delivery->_pimpl->_pickup_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; + const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); + + const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + + delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + + if (delivery->_pimpl->_drain_battery) + { + // Compute battery drain + const double dSOC_motion = + delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + delivery->_pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); + delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + } } return delivery; @@ -143,7 +157,7 @@ rmf_utils::optional Delivery::estimate_finish( const auto& finish_time = *trajectory.finish_time(); variant_duration = finish_time - start_time; - if(_pimpl->_drain_battery) + if (_pimpl->_drain_battery) { // Compute battery drain dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); @@ -218,6 +232,60 @@ rmf_traffic::Time Delivery::earliest_start_time() const return _pimpl->_start_time; } +//============================================================================== +std::size_t Delivery::pickup_waypoint() const +{ + return _pimpl->_pickup_waypoint; +} + +//============================================================================== +const std::string& Delivery::pickup_dispenser() const +{ + return _pimpl->_pickup_dispenser; +} + +//============================================================================== +const std::string& Delivery::dropoff_ingestor() const +{ + return _pimpl->_dropoff_ingestor; +} + +//============================================================================== +std::size_t Delivery::dropoff_waypoint() const +{ + return _pimpl->_dropoff_waypoint; +} + +//============================================================================== +const std::vector& Delivery::items() const +{ + return _pimpl->_items; +} + +//============================================================================== +Delivery::Start Delivery::dropoff_start(const Delivery::Start& start) const +{ + if (start.waypoint() == _pimpl->_pickup_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->_pickup_waypoint}; + + const auto result = _pimpl->_planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const double orientation = trajectory.back().position()[2]; + + rmf_traffic::agv::Planner::Start dropoff_start{ + finish_time, + _pimpl->_pickup_waypoint, + orientation}; + + return dropoff_start; + +} + //============================================================================== } // namespace requests } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp new file mode 100644 index 000000000..877673965 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -0,0 +1,299 @@ +/* + * 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 + +namespace rmf_task { +namespace requests { + +//============================================================================== +class Loop::Implementation +{ +public: + + Implementation() + {} + + std::size_t id; + std::size_t start_waypoint; + std::size_t finish_waypoint; + std::size_t num_loops; + std::shared_ptr motion_sink; + std::shared_ptr ambient_sink; + std::shared_ptr planner; + bool drain_battery; + rmf_traffic::Time start_time; + + rmf_traffic::Duration invariant_duration; + double invariant_battery_drain; +}; + +//============================================================================== +ConstRequestPtr Loop::make( + std::size_t id, + std::size_t start_waypoint, + std::size_t finish_waypoint, + std::size_t num_loops, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner, + rmf_traffic::Time start_time, + bool drain_battery) +{ + std::shared_ptr loop(new Loop()); + loop->_pimpl->id = id; + loop->_pimpl->start_waypoint = start_waypoint; + loop->_pimpl->finish_waypoint = finish_waypoint; + loop->_pimpl->num_loops = num_loops; + loop->_pimpl->motion_sink = std::move(motion_sink); + loop->_pimpl->ambient_sink = std::move(ambient_sink); + loop->_pimpl->planner = std::move(planner); + loop->_pimpl->drain_battery = drain_battery; + loop->_pimpl->start_time = start_time; + + // Calculate the invariant duration and battery drain for this task + loop->_pimpl->invariant_duration = rmf_traffic::Duration{0}; + loop->_pimpl->invariant_battery_drain = 0.0; + if (loop->_pimpl->start_waypoint != loop->_pimpl->finish_waypoint) + { + rmf_traffic::agv::Planner::Start loop_start{ + start_time, + loop->_pimpl->start_waypoint, + 0.0}; + rmf_traffic::agv::Planner::Goal loop_end_goal{ + loop->_pimpl->finish_waypoint}; + + const auto forward_loop_plan = loop->_pimpl->planner->plan( + loop_start, loop_end_goal); + + const auto trajectory = + forward_loop_plan->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const auto forward_duration = finish_time - start_time; + loop->_pimpl->invariant_duration = (2 * num_loops - 1) * forward_duration; + + if (loop->_pimpl->drain_battery) + { + // Compute battery drain + const double dSOC_motion = + loop->_pimpl->motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + loop->_pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(forward_duration)); + const double forward_battery_drain = dSOC_motion + dSOC_device; + loop->_pimpl->invariant_battery_drain = + (2 * num_loops - 1) * forward_battery_drain; + } + } + + return loop; + +} + +//============================================================================== +Loop::Loop() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +std::size_t Loop::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional Loop::estimate_finish( + const agv::State& initial_state, + const agv::StateConfig& state_config) 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; + + // Check if a plan has to be generated from finish location to start_waypoint + if (initial_state.waypoint() != _pimpl->start_waypoint) + { + // Compute plan to start_waypoint along with battery drain + rmf_traffic::agv::Planner::Start init_start{ + start_time, + initial_state.waypoint(), + 0.0}; + + rmf_traffic::agv::Planner::Goal loop_start_goal{_pimpl->start_waypoint}; + const auto plan_to_start = _pimpl->planner->plan(init_start, loop_start_goal); + // We assume we can always compute a plan + const auto& trajectory = plan_to_start->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if (_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = + _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + + if (battery_soc <= state_config.threshold_soc()) + return rmf_utils::nullopt; + } + + // Compute wait_until + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; + const rmf_traffic::Time wait_until = + initial_state.finish_time() > ideal_start ? + initial_state.finish_time() : ideal_start; + + // Compute finish time + const rmf_traffic::Time state_finish_time = + wait_until + variant_duration + _pimpl->invariant_duration; + + // Subtract invariant battery drain and check if robot can return to its charger + if (_pimpl->drain_battery) + { + battery_soc -= _pimpl->invariant_battery_drain; + if (battery_soc <= state_config.threshold_soc()) + return rmf_utils::nullopt; + + if ( _pimpl->finish_waypoint != initial_state.charging_waypoint()) + { + rmf_traffic::agv::Planner::Start retreat_start{ + state_finish_time, + _pimpl->finish_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal charger_goal{ + initial_state.charging_waypoint()}; + + const auto result_to_charger = _pimpl->planner->plan( + retreat_start, charger_goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state_finish_time; + + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + const double retreat_battery_drain = dSOC_motion + dSOC_device; + + if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + return rmf_utils::nullopt; + } + } + + // Return Estimate + rmf_traffic::agv::Planner::Start location{ + state_finish_time, + _pimpl->finish_waypoint, + initial_state.location().orientation()}; + agv::State state{ + std::move(location), + initial_state.charging_waypoint(), + battery_soc}; + + return Estimate(state, wait_until); +} + +//============================================================================== +rmf_traffic::Duration Loop::invariant_duration() const +{ + return _pimpl->invariant_duration; +} + +//============================================================================== +rmf_traffic::Time Loop::earliest_start_time() const +{ + return _pimpl->start_time; +} + +//============================================================================== +std::size_t Loop::start_waypoint() const +{ + return _pimpl->start_waypoint; +} + +//============================================================================== +std::size_t Loop::finish_waypoint() const +{ + return _pimpl->finish_waypoint; +} + +//============================================================================== +std::size_t Loop::num_loops() const +{ + return _pimpl->num_loops; +} + +//============================================================================== +Loop::Start Loop::loop_start(const Loop::Start& start) const +{ + if (start.waypoint() == _pimpl->start_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; + + const auto result = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const double orientation = trajectory.back().position()[2]; + + rmf_traffic::agv::Planner::Start loop_start{ + finish_time, + _pimpl->start_waypoint, + orientation}; + + return loop_start; +} + +//============================================================================== +Loop::Start Loop::loop_end(const Loop::Start& start) const +{ + if (start.waypoint() == _pimpl->finish_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->finish_waypoint}; + + const auto result = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const double orientation = trajectory.back().position()[2]; + + rmf_traffic::agv::Planner::Start loop_end{ + finish_time, + _pimpl->finish_waypoint, + orientation}; + + return loop_end; +} + +} // namespace requests +} // namespace rmf_task diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 67b380428..f27c7d069 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -187,7 +187,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -197,7 +200,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -207,7 +213,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 7, + "dispenser", 9, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -262,7 +271,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -272,7 +284,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -282,7 +297,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 7, + "dispenser", 9, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -292,7 +310,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -302,7 +323,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 5, 10, + "dispenser", 0, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -312,7 +336,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 6, 4, + "dispenser", 8, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -322,7 +349,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 7, 8, + "dispenser", 14, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -332,7 +362,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 8, 5, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -342,7 +375,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 9, 9, + "dispenser", 0, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -352,7 +388,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 10, 1, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -362,7 +401,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 11, 0, + "dispenser", 12, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -418,7 +460,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -428,7 +473,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -438,7 +486,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 9, + "dispenser", 4, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -448,7 +499,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, From 7ae6fd77a4021bd416cb47e114b3cd4d06c51c06 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 12 Nov 2020 18:04:54 +0800 Subject: [PATCH 069/128] Updated tests after merging in heuristic changes for task planner --- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 81 ++++++++++++++++++--- 1 file changed, 72 insertions(+), 9 deletions(-) diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index f4c558b5a..b9bcde181 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -232,15 +232,22 @@ SCENARIO("Grid World") planner); rmf_task::agv::TaskPlanner task_planner(task_config); + auto start_time = std::chrono::steady_clock::now(); const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); + auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Greedy solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Greedy", greedy_assignments, greedy_cost); + start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - - display_solution("Greedy", greedy_assignments, greedy_cost); + finish_time = std::chrono::steady_clock::now(); + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Optimal", optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); @@ -420,15 +427,22 @@ SCENARIO("Grid World") planner); rmf_task::agv::TaskPlanner task_planner(task_config); + auto start_time = std::chrono::steady_clock::now(); const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); + auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Greedy solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Greedy", greedy_assignments, greedy_cost); + start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - - display_solution("Greedy", greedy_assignments, greedy_cost); + finish_time = std::chrono::steady_clock::now(); + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Optimal", optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); @@ -518,17 +532,25 @@ SCENARIO("Grid World") planner); rmf_task::agv::TaskPlanner task_planner(task_config); + auto start_time = std::chrono::steady_clock::now(); const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); + auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Greedy solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Greedy", greedy_assignments, greedy_cost); + start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - - display_solution("Greedy", greedy_assignments, greedy_cost); + finish_time = std::chrono::steady_clock::now(); + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Optimal", optimal_assignments, optimal_cost); + REQUIRE(optimal_cost <= greedy_cost); // Checks if Assignments take into account a charging task in the beginning @@ -562,12 +584,15 @@ SCENARIO("Grid World") rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { rmf_task::requests::Delivery::make( 1, 6, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -577,7 +602,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 10, + "dispenser", 7, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -587,7 +615,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 2, + "dispenser", 12, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -597,7 +628,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -607,7 +641,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 5, 10, + "dispenser", 6, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -617,7 +654,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 6, 2, + "dispenser", 9, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -627,7 +667,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 7, 3, + "dispenser", 4, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -637,7 +680,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 8, 5, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -647,7 +693,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 9, 9, + "dispenser", 1, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -657,7 +706,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 10, 1, + "dispenser", 5, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -667,7 +719,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 11, 13, + "dispenser", 10, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -683,17 +738,25 @@ SCENARIO("Grid World") planner); rmf_task::agv::TaskPlanner task_planner(task_config); + auto start_time = std::chrono::steady_clock::now(); const auto greedy_assignments = task_planner.greedy_plan( now, initial_states, state_configs, requests); const double greedy_cost = task_planner.compute_cost(greedy_assignments); + auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Greedy solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Greedy", greedy_assignments, greedy_cost); + start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - - display_solution("Greedy", greedy_assignments, greedy_cost); + finish_time = std::chrono::steady_clock::now(); + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Optimal", optimal_assignments, optimal_cost); + REQUIRE(optimal_cost <= greedy_cost); } From cb122b7bf46f51cb2e5f713c268a804a77cc19f2 Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Thu, 12 Nov 2020 18:25:31 +0800 Subject: [PATCH 070/128] Change id field of Request to string and modify TaskPlanner to use internal ids instead (#203) * Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. * Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. --- rmf_task/include/rmf_task/Request.hpp | 2 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 2 +- .../rmf_task/requests/ChargeBattery.hpp | 4 +- rmf_task/include/rmf_task/requests/Clean.hpp | 5 +- .../include/rmf_task/requests/Delivery.hpp | 5 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 158 +++++++++++------- .../src/rmf_task/requests/ChargeBattery.cpp | 5 +- rmf_task/src/rmf_task/requests/Clean.cpp | 6 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 6 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 58 +++---- 10 files changed, 150 insertions(+), 101 deletions(-) diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 19a389176..e31cf5209 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -37,7 +37,7 @@ class Request using SharedPtr = std::shared_ptr; /// Get the id of the task - virtual std::size_t id() const = 0; + virtual std::string id() const = 0; /// Estimate the state of the robot when the task is finished along with the /// time the robot has to wait before commencing the task diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 965727f37..f564ea259 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -125,7 +125,7 @@ class TaskPlanner rmf_task::RequestPtr request, State state, rmf_traffic::Time deployment_time); - + // Get the request of this task rmf_task::RequestPtr request() const; diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 959cae7f6..0031e09c2 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -18,6 +18,8 @@ #ifndef INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP #define INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP +#include + #include #include @@ -46,7 +48,7 @@ class ChargeBattery : public rmf_task::Request rmf_traffic::Time start_time, bool drain_battery = true); - std::size_t id() const final; + std::string id() const final; rmf_utils::optional estimate_finish( const agv::State& initial_state, diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 98d148e25..e8aa02c58 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -19,6 +19,7 @@ #define INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP #include +#include #include #include @@ -41,7 +42,7 @@ class Clean : public rmf_task::Request public: static rmf_task::Request::SharedPtr make( - std::size_t id, + std::string id, std::size_t start_waypoint, std::size_t end_waypoint, rmf_traffic::Trajectory& cleaning_path, @@ -52,7 +53,7 @@ class Clean : public rmf_task::Request rmf_traffic::Time start_time, bool drain_battery = true); - std::size_t id() const final; + std::string id() const final; rmf_utils::optional estimate_finish( const agv::State& initial_state, diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 062840ed0..9a37702a5 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -19,6 +19,7 @@ #define INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP #include +#include #include #include @@ -40,7 +41,7 @@ class Delivery : public rmf_task::Request public: static rmf_task::Request::SharedPtr make( - std::size_t id, + std::string id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, std::shared_ptr motion_sink, @@ -49,7 +50,7 @@ class Delivery : public rmf_task::Request rmf_traffic::Time start_time, bool drain_battery = true); - std::size_t id() const final; + std::string id() const final; rmf_utils::optional estimate_finish( const agv::State& initial_state, diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index b937ca52f..bb2964fa3 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -138,7 +138,7 @@ TaskPlanner::Assignment::Assignment( } //============================================================================== -rmf_task::RequestPtr TaskPlanner::Assignment::request() const +rmf_task::RequestPtr TaskPlanner::Assignment::request() const { return _pimpl->request; } @@ -194,10 +194,10 @@ class Candidates using Map = std::multimap; static Candidates make( - const std::vector& initial_states, - const std::vector& state_configs, - const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request); + const std::vector& initial_states, + const std::vector& state_configs, + const rmf_task::Request& request, + const rmf_task::Request& charge_battery_request); Candidates(const Candidates& other) { @@ -283,10 +283,10 @@ class Candidates }; Candidates Candidates::make( - const std::vector& initial_states, - const std::vector& state_configs, - const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request) + const std::vector& initial_states, + const std::vector& state_configs, + const rmf_task::Request& request, + const rmf_task::Request& charge_battery_request) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) @@ -329,7 +329,7 @@ Candidates Candidates::make( << "] and request [" << request.id() << " ]" << std::endl; assert(false); } - } + } } @@ -360,7 +360,13 @@ struct PendingTask // ============================================================================ struct Node { - using AssignedTasks = TaskPlanner::Assignments; + struct AssignmentWrapper + { + std::size_t internal_id; + TaskPlanner::Assignment assignment; + }; + + using AssignedTasks = std::vector>; using UnassignedTasks = std::unordered_map; using InvariantSet = std::multiset; @@ -370,6 +376,13 @@ struct Node double cost_estimate; rmf_traffic::Time latest_time; InvariantSet unassigned_invariants; + std::size_t next_available_internal_id = 1; + + // ID 0 is reserved for charging tasks + std::size_t get_available_internal_id(bool charging_task = false) + { + return charging_task ? 0 : next_available_internal_id++; + } void sort_invariants() { @@ -397,7 +410,7 @@ struct Node bool popped_invariant = false; InvariantSet::iterator erase_it; for (auto it = unassigned_invariants.begin(); - it != unassigned_invariants.end(); ++it) + it != unassigned_invariants.end(); ++it) { if (it->task_id == task_id) { @@ -535,7 +548,7 @@ class Filter { // We add 1 to the task_id to differentiate between task_id == 0 and // a task being unassigned. - const std::size_t id = s.request()->id() + 1; + const std::size_t id = s.internal_id + 1; output += id << (_shift * (count++)); } } @@ -564,8 +577,10 @@ class Filter for (std::size_t j=0; j < a.size(); ++j) { - if (a[j].request()->id() != b[j].request()->id()) + if (a[j].internal_id != b[j].internal_id) + { return false; + } } } @@ -601,7 +616,7 @@ bool Filter::ignore(const Node& node) if (t < current_agent.size()) { - const auto& task_id = current_agent[t].request()->id(); + const auto& task_id = current_agent[t].internal_id; const auto agent_insertion = agent_table->agent.insert({a, nullptr}); if (agent_insertion.second) agent_insertion.first->second = std::make_unique(); @@ -631,8 +646,19 @@ bool Filter::ignore(const Node& node) const rmf_traffic::Duration segmentation_threshold = rmf_traffic::time::from_seconds(1.0); -} // anonymous namespace +inline double compute_g_assignment(const TaskPlanner::Assignment& assignment) +{ + if (std::dynamic_pointer_cast( + assignment.request())) + { + return 0.0; // Ignore charging tasks in cost + } + return rmf_traffic::time::to_seconds(assignment.state().finish_time() + - assignment.request()->earliest_start_time()); +} + +} // anonymous namespace class TaskPlanner::Implementation { @@ -658,22 +684,15 @@ class TaskPlanner::Implementation { for (const auto& assignment : agent) { - if (std::dynamic_pointer_cast( - assignment.request())) - { - continue; // Ignore charging tasks in cost - } - - cost += - rmf_traffic::time::to_seconds( - assignment.state().finish_time() - assignment.request()->earliest_start_time()); + cost += compute_g_assignment(assignment); } } return cost; } - Assignments prune_assignments(Assignments& assignments) + TaskPlanner::Assignments prune_assignments( + TaskPlanner::Assignments& assignments) { for (std::size_t a = 0; a < assignments.size(); ++a) { @@ -684,7 +703,7 @@ class TaskPlanner::Implementation // TODO(YV): Remove this after fixing the planner if (std::dynamic_pointer_cast( assignments[a].back().request())) - assignments[a].pop_back(); + assignments[a].pop_back(); } return assignments; @@ -702,7 +721,7 @@ class TaskPlanner::Implementation auto node = make_initial_node(initial_states, state_configs, requests, time_now); - Node::AssignedTasks complete_assignments; + TaskPlanner::Assignments complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); while (node) @@ -722,13 +741,14 @@ class TaskPlanner::Implementation const auto& new_assignments = node->assigned_tasks[i]; for (const auto& a : new_assignments) { - all_assignments.push_back(a); - // all_assignments.back().task_id = task_id_map.at(a.task_id); + all_assignments.push_back(a.assignment); } } if (node->unassigned_tasks.empty()) + { return prune_assignments(complete_assignments); + } std::vector new_tasks; for (const auto& u : node->unassigned_tasks) @@ -747,7 +767,7 @@ class TaskPlanner::Implementation if (assignments.empty()) estimates[i] = initial_states[i]; else - estimates[i] = assignments.back().state(); + estimates[i] = assignments.back().assignment.state(); } node = make_initial_node(estimates, state_configs, new_tasks, time_now); @@ -759,7 +779,15 @@ class TaskPlanner::Implementation double compute_g(const Node& node) { - return compute_g(node.assigned_tasks); + double cost = 0.0; + for (const auto& agent : node.assigned_tasks) + { + for (const auto& assignment : agent) + { + cost += compute_g_assignment(assignment.assignment); + } + } + return cost; } double compute_h(const Node& node, const rmf_traffic::Time time_now) @@ -799,7 +827,7 @@ class TaskPlanner::Implementation value = rmf_traffic::time::to_seconds(time_now.time_since_epoch()); else value = rmf_traffic::time::to_seconds( - assignments.back().state().finish_time().time_since_epoch()); + assignments.back().assignment.state().finish_time().time_since_epoch()); } } @@ -833,11 +861,16 @@ class TaskPlanner::Implementation // TODO(YV): Come up with a better solution for charge_battery_request auto charge_battery = make_charging_request(time_now); for (const auto& request : requests) + { + // Generate a unique internal id for the request. Currently, multiple + // requests with the same string id will be assigned different internal ids + std::size_t internal_id = initial_node->get_available_internal_id(); initial_node->unassigned_tasks.insert( { - request->id(), + internal_id, PendingTask(initial_states, state_configs, request, charge_battery) }); + } initial_node->cost_estimate = compute_f(*initial_node, time_now); @@ -881,7 +914,7 @@ class TaskPlanner::Implementation if (a.empty()) continue; - const auto finish_time = a.back().state().finish_time(); + const auto finish_time = a.back().assignment.state().finish_time(); if (latest < finish_time) latest = finish_time; } @@ -917,24 +950,29 @@ class TaskPlanner::Implementation // Check if a battery task already precedes the latest assignment auto& assignments = new_node->assigned_tasks[entry.candidate]; if (assignments.empty() || !std::dynamic_pointer_cast( - assignments.back().request())) + assignments.back().assignment.request())) { auto charge_battery = make_charging_request(entry.previous_state.finish_time()); auto battery_estimate = charge_battery->estimate_finish(entry.previous_state, state_config); if (battery_estimate.has_value()) { assignments.push_back( - Assignment - { - charge_battery, - battery_estimate.value().finish_state(), - battery_estimate.value().wait_until() - }); - } + Node::AssignmentWrapper + { u.first, + Assignment + { + charge_battery, + battery_estimate.value().finish_state(), + battery_estimate.value().wait_until() + } + } + ); + } } } new_node->assigned_tasks[entry.candidate].push_back( - Assignment{u.second.request, entry.state, entry.wait_until}); + Node::AssignmentWrapper{u.first, + Assignment{u.second.request, entry.state, entry.wait_until}}); // Erase the assigned task from unassigned tasks new_node->pop_unassigned(u.first); @@ -991,12 +1029,13 @@ class TaskPlanner::Implementation if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( - Assignment - { - charge_battery, - battery_estimate.value().finish_state(), - battery_estimate.value().wait_until() - }); + { new_node->get_available_internal_id(true), + Assignment + { + charge_battery, + battery_estimate.value().finish_state(), + battery_estimate.value().wait_until() + }}); for (auto& new_u : new_node->unassigned_tasks) { const auto finish = @@ -1050,9 +1089,9 @@ class TaskPlanner::Implementation if (!assignments.empty()) { if (std::dynamic_pointer_cast( - assignments.back().request())) + assignments.back().assignment.request())) return nullptr; - state = assignments.back().state(); + state = assignments.back().assignment.state(); } auto charge_battery = make_charging_request(state.finish_time()); @@ -1061,11 +1100,16 @@ class TaskPlanner::Implementation if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( - Assignment{ - charge_battery, - estimate.value().finish_state(), - estimate.value().wait_until()}); - + Node::AssignmentWrapper + { + new_node->get_available_internal_id(true), + Assignment + { + charge_battery, + estimate.value().finish_state(), + estimate.value().wait_until() + } + }); for (auto& new_u : new_node->unassigned_tasks) { const auto finish = diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index fa8fdb680..0df6dac0d 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -15,6 +15,7 @@ * */ +#include #include namespace rmf_task { @@ -29,7 +30,7 @@ class ChargeBattery::Implementation {} // fixed id for now - std::size_t _id = 1001; + std::string _id = "Charge"; rmf_battery::agv::BatterySystemPtr _battery_system; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; @@ -75,7 +76,7 @@ ChargeBattery::ChargeBattery() {} //============================================================================== -std::size_t ChargeBattery::id() const +std::string ChargeBattery::id() const { return _pimpl->_id; } diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 931e53f49..d4e27fe3b 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -30,7 +30,7 @@ class Clean::Implementation Implementation() {} - std::size_t id; + std::string id; std::size_t start_waypoint; std::size_t end_waypoint; rmf_traffic::Trajectory cleaning_path; @@ -47,7 +47,7 @@ class Clean::Implementation //============================================================================== rmf_task::Request::SharedPtr Clean::make( - std::size_t id, + std::string id, std::size_t start_waypoint, std::size_t end_waypoint, rmf_traffic::Trajectory& cleaning_path, @@ -102,7 +102,7 @@ Clean::Clean() {} //============================================================================== -std::size_t Clean::id() const +std::string Clean::id() const { return _pimpl->id; } diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 980b3160e..6c525ed1f 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -30,7 +30,7 @@ class Delivery::Implementation Implementation() {} - std::size_t _id; + std::string _id; std::size_t _pickup_waypoint; std::size_t _dropoff_waypoint; std::shared_ptr _motion_sink; @@ -45,7 +45,7 @@ class Delivery::Implementation //============================================================================== rmf_task::Request::SharedPtr Delivery::make( - std::size_t id, + std::string id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, std::shared_ptr motion_sink, @@ -100,7 +100,7 @@ Delivery::Delivery() {} //============================================================================== -std::size_t Delivery::id() const +std::string Delivery::id() const { return _pimpl->_id; } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 1ff21bd7b..d00b5bc65 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -185,7 +185,7 @@ SCENARIO("Grid World") std::vector requests = { rmf_task::requests::Delivery::make( - 1, + "1", 0, 3, motion_sink, @@ -195,7 +195,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 2, + "2", 15, 2, motion_sink, @@ -205,7 +205,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 3, + "3", 7, 9, motion_sink, @@ -260,7 +260,7 @@ SCENARIO("Grid World") std::vector requests = { rmf_task::requests::Delivery::make( - 1, + "1", 0, 3, motion_sink, @@ -270,7 +270,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 2, + "2", 15, 2, motion_sink, @@ -280,7 +280,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 3, + "3", 7, 9, motion_sink, @@ -290,7 +290,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 4, + "4", 8, 11, motion_sink, @@ -300,7 +300,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 5, + "5", 10, 0, motion_sink, @@ -310,7 +310,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 6, + "6", 4, 8, motion_sink, @@ -320,7 +320,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 7, + "7", 8, 14, motion_sink, @@ -330,7 +330,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 8, + "8", 5, 11, motion_sink, @@ -340,7 +340,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 9, + "9", 9, 0, motion_sink, @@ -350,7 +350,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 10, + "10", 1, 3, motion_sink, @@ -360,7 +360,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 11, + "11", 0, 12, motion_sink, @@ -416,7 +416,7 @@ SCENARIO("Grid World") std::vector requests = { rmf_task::requests::Delivery::make( - 1, + "1", 0, 3, motion_sink, @@ -426,7 +426,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 2, + "2", 15, 2, motion_sink, @@ -436,7 +436,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 3, + "3", 9, 4, motion_sink, @@ -446,7 +446,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 4, + "4", 8, 11, motion_sink, @@ -511,7 +511,7 @@ SCENARIO("Grid World") std::vector requests = { rmf_task::requests::Delivery::make( - 1, + "1", 6, 3, motion_sink, @@ -521,7 +521,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 2, + "2", 10, 7, motion_sink, @@ -531,7 +531,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 3, + "3", 2, 12, motion_sink, @@ -541,7 +541,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 4, + "4", 8, 11, motion_sink, @@ -551,7 +551,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 5, + "5", 10, 6, motion_sink, @@ -561,7 +561,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 6, + "6", 2, 9, motion_sink, @@ -571,7 +571,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 7, + "7", 3, 4, motion_sink, @@ -581,7 +581,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 8, + "8", 5, 11, motion_sink, @@ -591,7 +591,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 9, + "9", 9, 1, motion_sink, @@ -601,7 +601,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 10, + "10", 1, 5, motion_sink, @@ -611,7 +611,7 @@ SCENARIO("Grid World") drain_battery), rmf_task::requests::Delivery::make( - 11, + "11", 13, 10, motion_sink, From d5784073db98adcbb5947dd43a13f04abc72c662 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 12 Nov 2020 19:19:15 +0800 Subject: [PATCH 071/128] Updated FleetUpdateHandle and Tasks to use string request id --- .../agv/FleetUpdateHandle.cpp | 23 ++++--------------- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 2 +- .../src/rmf_fleet_adapter/tasks/Clean.cpp | 2 +- .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 6 ++--- .../src/rmf_fleet_adapter/tasks/Loop.cpp | 2 +- 5 files changed, 10 insertions(+), 25 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index adcb882d9..8305140d1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -220,13 +220,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - // TODO(YV) get rid of id field in RequestPtr - std::stringstream id_stream(id); - std::size_t request_id; - id_stream >> request_id; - new_request = rmf_task::requests::Clean::make( - request_id, + id, start_wp->index(), finish_wp->index(), cleaning_trajectory, @@ -319,13 +314,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - // TODO(YV) get rid of id field in RequestPtr - std::stringstream id_stream(id); - std::size_t request_id; - id_stream >> request_id; - new_request = rmf_task::requests::Delivery::make( - request_id, + id, pickup_wp->index(), delivery.pickup_dispenser, dropoff_wp->index(), @@ -399,13 +389,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - // TODO(YV) get rid of id field in RequestPtr - std::stringstream id_stream(id); - std::size_t request_id; - id_stream >> request_id; - new_request = rmf_task::requests::Loop::make( - request_id, + id, start_wp->index(), finish_wp->index(), loop.num_loops, @@ -509,7 +494,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( { for (const auto& assignment : agent) { - if (std::to_string(assignment.request()->id()) == id) + if (assignment.request()->id() == id) { bid_proposal.finish_time = rmf_traffic_ros2::convert( assignment.state().finish_time()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 9b3733acd..9ef580863 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -41,7 +41,7 @@ std::shared_ptr make_charge_battery( phases::WaitForCharge::make(context, request->battery_system(), 0.99)); return Task::make( - std::to_string(request->id()), + request->id(), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp index 2620082eb..c26fbebc8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -40,7 +40,7 @@ std::shared_ptr make_clean( phases::GoToPlace::make(context, std::move(end_start), end_goal)); return Task::make( - std::to_string(request->id()), + request->id(), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index a25cce7f3..6b669de62 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -42,7 +42,7 @@ std::shared_ptr make_delivery( phases.push_back( std::make_unique( context, - std::to_string(request->id()), + request->id(), request->pickup_dispenser(), context->itinerary().description().owner(), request->items())); @@ -66,13 +66,13 @@ std::shared_ptr make_delivery( phases.push_back( std::make_unique( context, - std::to_string(request->id()), + request->id(), request->dropoff_ingestor(), context->itinerary().description().owner(), ingestor_items)); return Task::make( - std::to_string(request->id()), + request->id(), std::move(phases), context->worker(), deployment_time, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index 0ff429944..a8098cf2c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -55,7 +55,7 @@ std::shared_ptr make_loop( } return Task::make( - std::to_string(request->id()), + request->id(), std::move(phases), context->worker(), deployment_time, From 92d3202ae0f583ec87740486f574856edd94f88a Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Mon, 16 Nov 2020 10:13:48 +0800 Subject: [PATCH 072/128] Cache estimated values to speed up TaskPlanner (#212) * Squashed commit of the following: commit b69a5de28b8c9dd434ec407eb18d813c40863971 Merge: 6206e41 005e88b Author: mrushyendra Date: Tue Nov 10 13:24:30 2020 +0800 Merge branch 'feature/task_planner' into refactor_id_w_map commit 005e88bada328e2989f0b29aeabdd9b7fc6df877 Author: Rushyendra Maganty Date: Tue Nov 10 12:08:34 2020 +0800 Reduce heuristic overestimation and ignore charging task costs in TaskPlanner (#207) * Improved heuristic to reduce overestimation Takes into account earliest start time when computing the cost of a task, more closely mimicing g(n). * Add field to Assignment determining whether to include in final cost Enables charging tasks to be ignored from final calculation. Adds miscellaneous comments explaining h(n) calculation. * Remove additional include_in_cost field from Assignment Use dynamic_pointer_cast to identify charging requests instead. * Add testcase illustrating difference in costs with new g(n) and h(n) * Include smallest variant cost in h(n) computation Include the smallest possible variant cost among the unasigned tasks for each candidate when computing the heuristic cost. commit 6206e41a71763de0d3bff29a0845b54a0e849c9c Author: mrushyendra Date: Tue Nov 3 18:20:24 2020 +0800 Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. commit d00b3f3f2d97958d5618da5ef8fac962cd65a8c9 Author: mrushyendra Date: Tue Nov 3 10:06:46 2020 +0800 Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. * Cache request estimate values in TaskPlanner Create new EstimateCache class and memoize the duration and charge consumed when travelling betwen any 2 waypoints on the graph. * Recreate TaskPlanner in tests for more accurate run time measurements Also adds other stylistic changes including renaming of CacheElem to CacheElement. --- rmf_task/include/rmf_task/Estimate.hpp | 31 +++++ rmf_task/include/rmf_task/Request.hpp | 3 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 2 + .../rmf_task/requests/ChargeBattery.hpp | 3 +- rmf_task/include/rmf_task/requests/Clean.hpp | 3 +- .../include/rmf_task/requests/Delivery.hpp | 3 +- rmf_task/include/rmf_task/requests/Loop.hpp | 4 +- rmf_task/src/rmf_task/Estimate.cpp | 41 +++++++ rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 49 +++++--- .../src/rmf_task/requests/ChargeBattery.cpp | 53 +++++---- rmf_task/src/rmf_task/requests/Clean.cpp | 105 ++++++++++------- rmf_task/src/rmf_task/requests/Delivery.cpp | 107 ++++++++++------- rmf_task/src/rmf_task/requests/Loop.cpp | 109 +++++++++++------- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 6 + 14 files changed, 353 insertions(+), 166 deletions(-) diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index c826ab91d..20d720a5f 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -18,6 +18,9 @@ #ifndef INCLUDE__RMF_TASK__ESTIMATE_HPP #define INCLUDE__RMF_TASK__ESTIMATE_HPP +#include +#include + #include #include #include @@ -55,6 +58,34 @@ class Estimate rmf_utils::impl_ptr _pimpl; }; +/// Stores computed estimates between pairs of waypoints +class EstimateCache +{ +public: + /// Constructs an empty EstimateCache + EstimateCache(); + + /// Struct containing the estimated duration and charge required to travel between + /// a waypoint pair. + struct CacheElement + { + rmf_traffic::Duration duration; + double dsoc; // Positive if charge is consumed + }; + + /// Returns the saved estimate values for the path between the supplied waypoints, + /// if present. + std::optional get(std::pair waypoints) const; + + /// Saves the estimated duration and change in charge between the supplied waypoints. + void set(std::pair waypoints, + rmf_traffic::Duration duration, double dsoc); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + } // namespace rmf_task #endif // INCLUDE__RMF_TASK__ESTIMATE_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 440b8355d..03eb533cb 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -43,7 +43,8 @@ class Request /// time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const = 0; + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const = 0; /// Estimate the invariant component of the task's duration virtual rmf_traffic::Duration invariant_duration() const = 0; diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 74a3ce968..4610426de 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -174,6 +174,8 @@ class TaskPlanner double compute_cost(const Assignments& assignments); + const std::shared_ptr estimate_cache() const; + class Implementation; private: diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 5d5d7f566..1ddb8f1e8 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -52,7 +52,8 @@ class ChargeBattery : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const final; + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 978910917..bfc35510d 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -57,7 +57,8 @@ class Clean : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const final; + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index fb32728b3..e5534a544 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -62,7 +62,8 @@ class Delivery : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const final; + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 3bab8f873..e9f4cf2c3 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -19,6 +19,7 @@ #define INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP #include +#include #include #include @@ -57,7 +58,8 @@ class Loop : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const final; + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 6979ac353..12945d413 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include @@ -66,4 +67,44 @@ Estimate& Estimate::wait_until(rmf_traffic::Time new_wait_until) } //============================================================================== +class EstimateCache::Implementation +{ + public: + struct PairHash + { + size_t operator()(const std::pair& p) const + { + return std::hash()(p.first) ^ std::hash()(p.second); + } + }; + + using Cache = std::unordered_map, + CacheElement, PairHash>; + Cache _cache; +}; + +//============================================================================== +EstimateCache::EstimateCache() + : _pimpl(rmf_utils::make_impl()) +{} + +//============================================================================== +std::optional EstimateCache::get( + std::pair waypoints) const +{ + auto it = _pimpl->_cache.find(waypoints); + if (it != _pimpl->_cache.end()) + { + return it->second; + } + return std::nullopt; +} + +//============================================================================== +void EstimateCache::set(std::pair waypoints, + rmf_traffic::Duration duration, double dsoc) +{ + _pimpl->_cache[waypoints] = CacheElement{duration, dsoc}; +} + } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 3033d060d..eefc24517 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -197,7 +197,8 @@ class Candidates const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request); + const rmf_task::Request& charge_battery_request, + const std::shared_ptr estimate_cache); Candidates(const Candidates& other) { @@ -286,14 +287,16 @@ Candidates Candidates::make( const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request) + const rmf_task::Request& charge_battery_request, + const std::shared_ptr estimate_cache) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) { const auto& state = initial_states[i]; const auto& state_config = state_configs[i]; - const auto finish = request.estimate_finish(state, state_config); + const auto finish = request.estimate_finish( + state, state_config, estimate_cache); if (finish.has_value()) { initial_map.insert({ @@ -308,11 +311,12 @@ Candidates Candidates::make( else { auto battery_estimate = - charge_battery_request.estimate_finish(state, state_config); + charge_battery_request.estimate_finish( + state, state_config, estimate_cache); if (battery_estimate.has_value()) { auto new_finish = request.estimate_finish( - battery_estimate.value().finish_state(), state_config); + battery_estimate.value().finish_state(), state_config, estimate_cache); assert(new_finish.has_value()); initial_map.insert( {new_finish.value().finish_state().finish_time(), @@ -343,10 +347,11 @@ struct PendingTask std::vector& initial_states, std::vector& state_configs, rmf_task::ConstRequestPtr request_, - rmf_task::ConstRequestPtr charge_battery_request) + rmf_task::ConstRequestPtr charge_battery_request, + std::shared_ptr estimate_cache) : request(std::move(request_)), - candidates(Candidates::make( - initial_states, state_configs, *request, *charge_battery_request)), + candidates(Candidates::make(initial_states, state_configs, + *request, *charge_battery_request, estimate_cache)), earliest_start_time(request->earliest_start_time()) { // Do nothing @@ -665,6 +670,7 @@ class TaskPlanner::Implementation public: std::shared_ptr config; + std::shared_ptr estimate_cache; ConstRequestPtr make_charging_request(rmf_traffic::Time start_time) { @@ -868,7 +874,7 @@ class TaskPlanner::Implementation initial_node->unassigned_tasks.insert( { internal_id, - PendingTask(initial_states, state_configs, request, charge_battery) + PendingTask(initial_states, state_configs, request, charge_battery, estimate_cache) }); } @@ -953,7 +959,8 @@ class TaskPlanner::Implementation assignments.back().assignment.request())) { auto charge_battery = make_charging_request(entry.previous_state.finish_time()); - auto battery_estimate = charge_battery->estimate_finish(entry.previous_state, state_config); + auto battery_estimate = charge_battery->estimate_finish( + entry.previous_state, state_config, estimate_cache); if (battery_estimate.has_value()) { assignments.push_back( @@ -983,7 +990,7 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish( - entry.state, state_config); + entry.state, state_config, estimate_cache); if (finish.has_value()) { @@ -1025,7 +1032,8 @@ class TaskPlanner::Implementation if (add_charger) { auto charge_battery = make_charging_request(entry.state.finish_time()); - auto battery_estimate = charge_battery->estimate_finish(entry.state, state_config); + auto battery_estimate = charge_battery->estimate_finish( + entry.state, state_config, estimate_cache); if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( @@ -1039,7 +1047,8 @@ class TaskPlanner::Implementation for (auto& new_u : new_node->unassigned_tasks) { const auto finish = - new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), state_config); + new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), + state_config, estimate_cache); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -1096,7 +1105,7 @@ class TaskPlanner::Implementation auto charge_battery = make_charging_request(state.finish_time()); auto estimate = charge_battery->estimate_finish( - state, state_configs[agent]); + state, state_configs[agent], estimate_cache); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( @@ -1113,8 +1122,8 @@ class TaskPlanner::Implementation for (auto& new_u : new_node->unassigned_tasks) { const auto finish = - new_u.second.request->estimate_finish( - estimate.value().finish_state(), state_configs[agent]); + new_u.second.request->estimate_finish(estimate.value().finish_state(), + state_configs[agent], estimate_cache); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -1294,7 +1303,8 @@ class TaskPlanner::Implementation TaskPlanner::TaskPlanner(std::shared_ptr config) : _pimpl(rmf_utils::make_impl( Implementation{ - std::move(config) + std::move(config), + std::make_shared() })) { // Do nothing @@ -1336,7 +1346,10 @@ auto TaskPlanner::compute_cost(const Assignments& assignments) -> double return _pimpl->compute_g(assignments); } - +const std::shared_ptr TaskPlanner::estimate_cache() const +{ + return _pimpl->estimate_cache; +} } // namespace agv diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 702b0bfc4..6eec494fa 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -84,7 +84,8 @@ std::string ChargeBattery::id() const //============================================================================== rmf_utils::optional ChargeBattery::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const { if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) return rmf_utils::nullopt; @@ -102,30 +103,42 @@ rmf_utils::optional ChargeBattery::estimate_finish( const auto start_time = initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); + double dSOC_motion = 0.0; + double dSOC_device = 0.0; rmf_traffic::Duration variant_duration(0); if (initial_state.waypoint() != initial_state.charging_waypoint()) { - // Compute plan to charging waypoint along with battery drain - rmf_traffic::agv::Planner::Start start{ - start_time, - initial_state.waypoint(), - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{initial_state.charging_waypoint()}; - - const auto result = _pimpl->_planner->plan(start, goal); - const auto& trajectory = result->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if (_pimpl->_drain_battery) + 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 { - const double dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge( - trajectory); - const double dSOC_device = _pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; + // Compute plan to charging waypoint along with battery drain + rmf_traffic::agv::Planner::Goal goal{endpoints.second}; + const auto result = _pimpl->_planner->plan( + initial_state.location(), goal); + const auto& trajectory = result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if (_pimpl->_drain_battery) + { + dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge( + trajectory); + dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + + estimate_cache->set(endpoints, variant_duration, + dSOC_motion + dSOC_device); } // If a robot cannot reach its charging dock given its initial battery soc diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 2cfa9a74b..e7519c7b7 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -110,7 +110,8 @@ std::string Clean::id() const //============================================================================== rmf_utils::optional Clean::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), @@ -131,27 +132,42 @@ rmf_utils::optional Clean::estimate_finish( if (initial_state.waypoint() != _pimpl->start_waypoint) { - rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; - - const auto result_to_start = _pimpl->planner->plan( - initial_state.location(), goal); - // We assume we can always compute a plan - const auto& trajectory = - result_to_start->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if(_pimpl->drain_battery) + const auto endpoints = std::make_pair(initial_state.waypoint(), + _pimpl->start_waypoint); + const auto& cache_result = estimate_cache->get(endpoints); + if (cache_result) + { + variant_duration = cache_result->duration; + battery_soc = battery_soc - cache_result->dsoc; + } + else { - // Compute battery drain - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_ambient = - _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_ambient; - if (battery_soc <= state_config.threshold_soc()) - return rmf_utils::nullopt; + rmf_traffic::agv::Planner::Goal goal{endpoints.second}; + + const auto result_to_start = _pimpl->planner->plan( + initial_state.location(), goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_start->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if(_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_ambient = + _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_ambient; + } + + estimate_cache->set(endpoints, variant_duration, + dSOC_motion + dSOC_ambient); } + + if (battery_soc <= state_config.threshold_soc()) + return rmf_utils::nullopt; } if (_pimpl->start_waypoint != _pimpl->end_waypoint) @@ -180,25 +196,36 @@ rmf_utils::optional Clean::estimate_finish( double retreat_battery_drain = 0.0; if ( _pimpl->end_waypoint != state.charging_waypoint()) { - rmf_traffic::agv::Planner::Start start{ - state.finish_time(), - _pimpl->end_waypoint, - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{state.charging_waypoint()}; - - const auto result_to_charger = _pimpl->planner->plan(start, goal); - // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - state.finish_time(); - - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - retreat_battery_drain = dSOC_motion + dSOC_ambient; + const auto endpoints = std::make_pair(_pimpl->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 = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state.finish_time(); + + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_ambient; + estimate_cache->set(endpoints, retreat_duration, retreat_battery_drain); + } } if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index d3896fa28..0f372b6a6 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -122,7 +122,8 @@ std::string Delivery::id() const //============================================================================== rmf_utils::optional Delivery::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), @@ -142,29 +143,38 @@ rmf_utils::optional Delivery::estimate_finish( if (initial_state.waypoint() != _pimpl->_pickup_waypoint) { - // Compute plan to pickup waypoint along with battery drain - rmf_traffic::agv::Planner::Start start{ - start_time, - initial_state.waypoint(), - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{_pimpl->_pickup_waypoint}; - - const auto result_to_pickup = _pimpl->_planner->plan(start, goal); - // We assume we can always compute a plan - const auto& trajectory = - result_to_pickup->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if (_pimpl->_drain_battery) + const auto endpoints = std::make_pair(initial_state.waypoint(), + _pimpl->_pickup_waypoint); + const auto& cache_result = estimate_cache->get(endpoints); + // Use previously memoized values if possible + if (cache_result) { - // Compute battery drain - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); - dSOC_device = - _pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; + variant_duration = cache_result->duration; + 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 = _pimpl->_planner->plan( + initial_state.location(), goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_pickup->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if (_pimpl->_drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); + dSOC_device = + _pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + estimate_cache->set(endpoints, variant_duration, + dSOC_motion + dSOC_device); } if (battery_soc <= state_config.threshold_soc()) @@ -190,25 +200,38 @@ rmf_utils::optional Delivery::estimate_finish( double retreat_battery_drain = 0.0; if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) { - rmf_traffic::agv::Planner::Start start{ - state.finish_time(), - _pimpl->_dropoff_waypoint, - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{state.charging_waypoint()}; - - const auto result_to_charger = _pimpl->_planner->plan(start, goal); - // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - state.finish_time(); - - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); - dSOC_device = _pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - retreat_battery_drain = dSOC_motion + dSOC_device; + const auto endpoints = std::make_pair(_pimpl->_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 = _pimpl->_planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state.finish_time(); + + dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_device; + + estimate_cache->set(endpoints, retreat_duration, + retreat_battery_drain); + } } if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 6e2f34e60..0fd83ea8a 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -120,7 +120,8 @@ std::string Loop::id() const //============================================================================== rmf_utils::optional Loop::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config) const + const agv::StateConfig& state_config, + const std::shared_ptr estimate_cache) const { rmf_traffic::Duration variant_duration(0); @@ -133,27 +134,38 @@ rmf_utils::optional Loop::estimate_finish( // Check if a plan has to be generated from finish location to start_waypoint if (initial_state.waypoint() != _pimpl->start_waypoint) { - // Compute plan to start_waypoint along with battery drain - rmf_traffic::agv::Planner::Start init_start{ - start_time, - initial_state.waypoint(), - 0.0}; - - rmf_traffic::agv::Planner::Goal loop_start_goal{_pimpl->start_waypoint}; - const auto plan_to_start = _pimpl->planner->plan(init_start, loop_start_goal); - // We assume we can always compute a plan - const auto& trajectory = plan_to_start->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if (_pimpl->drain_battery) + auto endpoints = std::make_pair(initial_state.waypoint(), + _pimpl->start_waypoint); + const auto& cache_result = estimate_cache->get(endpoints); + // Use previously memoized values if possible + if (cache_result) { - // Compute battery drain - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_device = - _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; + variant_duration = cache_result->duration; + 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 = _pimpl->planner->plan( + initial_state.location(), loop_start_goal); + // We assume we can always compute a plan + const auto& trajectory = plan_to_start->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if (_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = + _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + + estimate_cache->set(endpoints, variant_duration, + dSOC_motion + dSOC_device); } if (battery_soc <= state_config.threshold_soc()) @@ -171,6 +183,7 @@ rmf_utils::optional Loop::estimate_finish( wait_until + variant_duration + _pimpl->invariant_duration; // Subtract invariant battery drain and check if robot can return to its charger + double retreat_battery_drain = 0.0; if (_pimpl->drain_battery) { battery_soc -= _pimpl->invariant_battery_drain; @@ -179,27 +192,39 @@ rmf_utils::optional Loop::estimate_finish( if ( _pimpl->finish_waypoint != initial_state.charging_waypoint()) { - rmf_traffic::agv::Planner::Start retreat_start{ - state_finish_time, - _pimpl->finish_waypoint, - 0.0}; - - rmf_traffic::agv::Planner::Goal charger_goal{ - initial_state.charging_waypoint()}; - - const auto result_to_charger = _pimpl->planner->plan( - retreat_start, charger_goal); - // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - state_finish_time; - - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - const double retreat_battery_drain = dSOC_motion + dSOC_device; + const auto endpoints = std::make_pair(_pimpl->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 = _pimpl->planner->plan( + retreat_start, charger_goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state_finish_time; + + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_device; + estimate_cache->set(endpoints, retreat_duration, + retreat_battery_drain); + } if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) return rmf_utils::nullopt; diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index aba86f56c..538c5e97d 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -241,6 +241,9 @@ SCENARIO("Grid World") << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Greedy", greedy_assignments, greedy_cost); + // Create new TaskPlanner to reset cache so that measured run times + // remain independent of one another + task_planner = rmf_task::agv::TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); @@ -436,6 +439,7 @@ SCENARIO("Grid World") << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Greedy", greedy_assignments, greedy_cost); + task_planner = rmf_task::agv::TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); @@ -541,6 +545,7 @@ SCENARIO("Grid World") << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Greedy", greedy_assignments, greedy_cost); + task_planner = rmf_task::agv::TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); @@ -747,6 +752,7 @@ SCENARIO("Grid World") << (finish_time - start_time).count() / 1e9 << std::endl; display_solution("Greedy", greedy_assignments, greedy_cost); + task_planner = rmf_task::agv::TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); From 15fdae97554321d545ba04ab9479f84684556ecb Mon Sep 17 00:00:00 2001 From: youliang Date: Mon, 23 Nov 2020 09:57:05 +0800 Subject: [PATCH 073/128] few liners to populate start and estimated finish time (#216) --- .../src/rmf_fleet_adapter/TaskManager.cpp | 63 +++++++++---------- 1 file changed, 30 insertions(+), 33 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 67539f596..9e8f9f563 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -155,10 +155,12 @@ void TaskManager::set_queue( if (i != 0) start = assignments[i-1].state().location(); start.time(a.deployment_time()); + rmf_task_msgs::msg::TaskType task_type_msg; if (const auto request = std::dynamic_pointer_cast(a.request())) { + task_type_msg.type = task_type_msg.TYPE_CLEAN; auto task = rmf_fleet_adapter::tasks::make_clean( request, _context, @@ -169,19 +171,13 @@ void TaskManager::set_queue( std::lock_guard guard(_mutex); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + task_type_msg.type = task_type_msg.TYPE_CHARGE_BATTERY; const auto task = tasks::make_charge_battery( request, _context, @@ -192,20 +188,13 @@ void TaskManager::set_queue( std::lock_guard guard(_mutex); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); - } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + task_type_msg.type = task_type_msg.TYPE_DELIVERY; const auto task = tasks::make_delivery( request, _context, @@ -214,19 +203,12 @@ void TaskManager::set_queue( a.state()); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); - } else if (const auto request = std::dynamic_pointer_cast(a.request())) { + task_type_msg.type = task_type_msg.TYPE_LOOP; const auto task = tasks::make_loop( request, _context, @@ -235,19 +217,25 @@ void TaskManager::set_queue( a.state()); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); } else { continue; } + + // publish queued task + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); + msg.task_profile.task_type = task_type_msg; + msg.start_time = rmf_traffic_ros2::convert( + _queue.back()->deployment_time()); + msg.start_time = rmf_traffic_ros2::convert( + _queue.back()->finish_state().finish_time()); + this->_context->node()->task_summary()->publish(msg); } } @@ -261,11 +249,8 @@ const std::vector TaskManager::requests() const if (std::dynamic_pointer_cast( task->request())) continue; - requests.push_back(task->request()); - } - return requests; } @@ -315,6 +300,10 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.robot_name = _context->name(); + msg.start_time = rmf_traffic_ros2::convert( + _active_task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + _active_task->finish_state().finish_time()); _context->node()->task_summary()->publish(msg); }, [this, id = _active_task->id()](std::exception_ptr e) @@ -332,6 +321,10 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.robot_name = _context->name(); + msg.start_time = rmf_traffic_ros2::convert( + _active_task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + _active_task->finish_state().finish_time()); _context->node()->task_summary()->publish(msg); // _begin_next_task(); }, @@ -342,6 +335,10 @@ void TaskManager::_begin_next_task() msg.task_profile.task_id = id; msg.state = msg.STATE_COMPLETED; msg.robot_name = _context->name(); + msg.start_time = rmf_traffic_ros2::convert( + _active_task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + _active_task->finish_state().finish_time()); this->_context->node()->task_summary()->publish(msg); _active_task = nullptr; From bb363a403c3e7781ac2b35c0238d2da9af950c70 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 23 Nov 2020 10:33:33 +0800 Subject: [PATCH 074/128] Cleaned up commented code --- .../agv/FleetUpdateHandle.cpp | 274 ------------------ .../agv/internal_FleetUpdateHandle.hpp | 42 --- 2 files changed, 316 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 8305140d1..126f29a18 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -564,220 +564,6 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( name.c_str()); } -//============================================================================== -// auto FleetUpdateHandle::Implementation::estimate_delivery( -// const rmf_task_msgs::msg::Delivery& request) const -// -> rmf_utils::optional -// { -// const auto& graph = planner->get_configuration().graph(); -// const auto pickup_wp = graph.find_waypoint(request.pickup_place_name); -// if (!pickup_wp) -// return rmf_utils::nullopt; - -// const auto dropoff_wp = graph.find_waypoint(request.dropoff_place_name); -// if (!dropoff_wp) -// return rmf_utils::nullopt; - -// const auto pickup_goal = rmf_traffic::agv::Plan::Goal(pickup_wp->index()); -// const auto dropoff_goal = rmf_traffic::agv::Plan::Goal(dropoff_wp->index()); - -// // TODO(MXG): At some point we should consider parallelizing this estimation -// // process and taking the existing schedule into account, but for now we'll -// // try to use a very quick rough estimate. -// DeliveryEstimate best; -// for (const auto& element : task_managers) -// { -// const auto& mgr = *element.second; -// auto start = mgr.expected_finish_location(); -// const auto pickup_plan = planner->plan(start, pickup_goal); -// if (!pickup_plan) -// continue; - -// const auto& pickup_plan_end = pickup_plan->get_waypoints().back(); -// assert(pickup_plan_end.graph_index()); -// const auto dropoff_start = rmf_traffic::agv::Plan::Start( -// pickup_plan_end.time(), -// *pickup_plan_end.graph_index(), -// pickup_plan_end.position()[2]); - -// const auto dropoff_plan = planner->plan(dropoff_start, dropoff_goal); -// if (!dropoff_plan) -// continue; - -// const auto& final_wp = dropoff_plan->get_waypoints().back(); - -// const auto estimate = final_wp.time(); -// rmf_traffic::agv::Plan::Start finish{ -// estimate, -// *final_wp.graph_index(), -// final_wp.position()[2] -// }; - -// if (estimate < best.time) -// { -// best = DeliveryEstimate{ -// estimate, -// element.first, -// std::move(start.front()), -// std::move(dropoff_start), -// std::move(finish) -// }; -// } -// } - -// if (best.robot) -// return best; - -// return rmf_utils::nullopt; -// } - -//============================================================================== -// void FleetUpdateHandle::Implementation::perform_delivery( -// const rmf_task_msgs::msg::Delivery& request, -// const DeliveryEstimate& estimate) -// { -// auto& mgr = *task_managers.at(estimate.robot); -// mgr.queue_task( -// tasks::make_delivery( -// request, -// estimate.robot, -// *estimate.pickup_start, -// *estimate.dropoff_start), -// *estimate.finish); -// } - -//============================================================================== -// auto FleetUpdateHandle::Implementation::estimate_loop( -// const rmf_task_msgs::msg::Loop& request) const -// -> rmf_utils::optional -// { -// if (request.robot_type != name) -// return rmf_utils::nullopt; - -// const std::size_t n = request.num_loops; -// if (n == 0) -// return rmf_utils::nullopt; - -// const auto& graph = planner->get_configuration().graph(); -// const auto loop_start_wp = graph.find_waypoint(request.start_name); -// if (!loop_start_wp) -// return rmf_utils::nullopt; - -// const auto loop_end_wp = graph.find_waypoint(request.finish_name); -// if (!loop_end_wp) -// return rmf_utils::nullopt; - -// const auto loop_start_goal = -// rmf_traffic::agv::Plan::Goal(loop_start_wp->index()); - -// const auto loop_end_goal = -// rmf_traffic::agv::Plan::Goal(loop_end_wp->index()); - -// LoopEstimate best; -// for (const auto& element : task_managers) -// { -// LoopEstimate estimate; -// estimate.robot = element.first; - -// const auto& mgr = *element.second; -// auto start = mgr.expected_finish_location(); -// const auto loop_init_plan = planner->plan(start, loop_start_goal); -// if (!loop_init_plan) -// continue; - -// rmf_traffic::Duration init_duration = std::chrono::seconds(0); -// if (loop_init_plan->get_waypoints().size() > 1) -// { -// // If loop_init_plan is not empty, then that means we are not starting at -// // the starting point of the loop. Therefore we will need an initial plan -// // to reach the first point in the loop. -// estimate.init_start = start.front(); - -// init_duration = -// loop_init_plan->get_waypoints().back().time() -// - loop_init_plan->get_waypoints().front().time(); -// } - -// const auto loop_forward_start = [&]() -> rmf_traffic::agv::Plan::StartSet -// { -// if (loop_init_plan->get_waypoints().empty()) -// return start; - -// const auto& loop_init_wp = loop_init_plan->get_waypoints().back(); -// assert(loop_init_wp.graph_index()); -// return {rmf_traffic::agv::Plan::Start( -// loop_init_wp.time(), -// *loop_init_wp.graph_index(), -// loop_init_wp.position()[2])}; -// }(); - -// const auto loop_forward_plan = -// planner->plan(loop_forward_start, loop_end_goal); -// if (!loop_forward_plan) -// continue; - -// // If the forward plan is empty then that means the start and end of the -// // loop are the same, making it a useless request. -// // TODO(MXG): We should probably make noise here instead of just ignoring -// // the request. -// if (loop_forward_plan->get_waypoints().empty()) -// return rmf_utils::nullopt; - -// estimate.loop_start = loop_forward_start.front(); - -// const auto loop_duration = -// loop_forward_plan->get_waypoints().back().time() -// - loop_forward_plan->get_waypoints().front().time(); - -// // We only need to provide this if there is supposed to be more than one -// // loop. -// const auto& final_wp = loop_forward_plan->get_waypoints().back(); -// assert(final_wp.graph_index()); -// estimate.loop_end = rmf_traffic::agv::Plan::Start{ -// final_wp.time(), -// *final_wp.graph_index(), -// final_wp.position()[2] -// }; - -// const auto start_time = [&]() -// { -// if (loop_init_plan->get_waypoints().empty()) -// return loop_forward_plan->get_waypoints().front().time(); - -// return loop_init_plan->get_waypoints().front().time(); -// }(); - -// estimate.time = -// start_time + init_duration + (2*n - 1)*loop_duration; - -// estimate.loop_end->time(estimate.time); - -// if (estimate.time < best.time) -// best = std::move(estimate); -// } - -// if (best.robot) -// return best; - -// return rmf_utils::nullopt; -// } - -//============================================================================== -// void FleetUpdateHandle::Implementation::perform_loop( -// const rmf_task_msgs::msg::Loop& request, -// const LoopEstimate& estimate) -// { -// auto& mgr = task_managers.at(estimate.robot); -// mgr->queue_task( -// tasks::make_loop( -// request, -// estimate.robot, -// estimate.init_start, -// *estimate.loop_start, -// estimate.loop_end), -// *estimate.loop_end); -// } - //============================================================================== std::size_t FleetUpdateHandle::Implementation::get_nearest_charger( const rmf_traffic::agv::Planner::Start& start, @@ -972,65 +758,5 @@ FleetUpdateHandle::FleetUpdateHandle() // Do nothing } -//============================================================================== -// void request_delivery( -// const rmf_task_msgs::msg::Delivery& request, -// const std::vector>& fleets) -// { -// FleetUpdateHandle::Implementation::DeliveryEstimate best; -// FleetUpdateHandle::Implementation* chosen_fleet = nullptr; - -// for (auto& fleet : fleets) -// { -// auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); -// if (!fimpl.accept_delivery || !fimpl.accept_delivery(request)) -// continue; - -// const auto estimate = fimpl.estimate_delivery(request); -// if (!estimate) -// continue; - -// if (estimate->time < best.time) -// { -// best = *estimate; -// chosen_fleet = &fimpl; -// } -// } - -// if (!chosen_fleet) -// return; - -// chosen_fleet->perform_delivery(request, best); -// } - -//============================================================================== -// void request_loop( -// const rmf_task_msgs::msg::Loop& request, -// const std::vector>& fleets) -// { -// FleetUpdateHandle::Implementation::LoopEstimate best; -// FleetUpdateHandle::Implementation* chosen_fleet = nullptr; - -// for (auto& fleet : fleets) -// { -// auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); -// const auto estimate = fimpl.estimate_loop(request); -// if (!estimate) -// continue; - -// if (estimate->time < best.time) -// { -// best = *estimate; -// chosen_fleet = &fimpl; -// } -// } - -// if (!chosen_fleet) -// return; - -// chosen_fleet->perform_loop(request, best); -// } - - } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 1b1be2859..2272e2310 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -245,24 +245,6 @@ class FleetUpdateHandle::Implementation return std::make_shared(std::move(handle)); } - // struct DeliveryEstimate - // { - // rmf_traffic::Time time = rmf_traffic::Time::max(); - // RobotContextPtr robot = nullptr; - // rmf_utils::optional pickup_start; - // rmf_utils::optional dropoff_start; - // rmf_utils::optional finish; - // }; - - // struct LoopEstimate - // { - // rmf_traffic::Time time = rmf_traffic::Time::max(); - // RobotContextPtr robot = nullptr; - // rmf_utils::optional init_start; - // rmf_utils::optional loop_start; - // rmf_utils::optional loop_end; - // }; - void dock_summary_cb(const DockSummary::SharedPtr msg); void bid_notice_cb(const BidNotice::SharedPtr msg); @@ -283,32 +265,8 @@ class FleetUpdateHandle::Implementation return *fleet._pimpl; } - // TODO(MXG): Come up with a better design for task dispatch - // rmf_utils::optional estimate_delivery( - // const rmf_task_msgs::msg::Delivery& request) const; - - // void perform_delivery( - // const rmf_task_msgs::msg::Delivery& request, - // const DeliveryEstimate& estimate); - - // rmf_utils::optional estimate_loop( - // const rmf_task_msgs::msg::Loop& request) const; - - // void perform_loop( - // const rmf_task_msgs::msg::Loop& request, - // const LoopEstimate& estimate); }; -//============================================================================== -// void request_delivery( -// const rmf_task_msgs::msg::Delivery& request, -// const std::vector>& fleets); - -// //============================================================================== -// void request_loop( -// const rmf_task_msgs::msg::Loop& request, -// const std::vector>& fleets); - } // namespace agv } // namespace rmf_fleet_adapter From af7c3ab41fe34518b627f2ee7a094d435bc264d2 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 23 Nov 2020 11:41:35 +0800 Subject: [PATCH 075/128] Made ChargeBattery ids unique --- .../src/rmf_task/requests/ChargeBattery.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 6eec494fa..8c71c850d 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -16,11 +16,35 @@ */ #include +#include +#include + #include namespace rmf_task { namespace requests { +//============================================================================== +namespace { +std::string generate_uuid(const std::size_t length = 3) +{ + std::stringstream ss; + for (std::size_t i = 0; i < length; ++i) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(0, 255); + const auto random_char = dis(gen); + std::stringstream hexstream; + hexstream << std::hex << random_char; + auto hex = hexstream.str(); + ss << (hex.length() < 2 ? '0' + hex : hex); + } + return ss.str(); +} + +} // anonymous namespace + //============================================================================== class ChargeBattery::Implementation { @@ -53,6 +77,7 @@ rmf_task::ConstRequestPtr ChargeBattery::make( bool drain_battery) { std::shared_ptr charge_battery(new ChargeBattery()); + charge_battery->_pimpl->_id += generate_uuid(); charge_battery->_pimpl->_battery_system = rmf_battery::agv::BatterySystemPtr(new rmf_battery::agv::BatterySystem( battery_system.nominal_voltage(), From 22c649e0907c68fa4197d1b5c4c421c97a22a9a1 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 24 Nov 2020 15:37:03 +0800 Subject: [PATCH 076/128] add fleetname to task summary --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 9e8f9f563..5bb3b5d77 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -230,6 +230,7 @@ void TaskManager::set_queue( msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; msg.robot_name = _context->name(); + msg.fleet_name = _context->description().owner(); msg.task_profile.task_type = task_type_msg; msg.start_time = rmf_traffic_ros2::convert( _queue.back()->deployment_time()); @@ -300,6 +301,7 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.robot_name = _context->name(); + msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( _active_task->deployment_time()); msg.end_time = rmf_traffic_ros2::convert( @@ -321,6 +323,7 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.robot_name = _context->name(); + msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( _active_task->deployment_time()); msg.end_time = rmf_traffic_ros2::convert( @@ -335,6 +338,7 @@ void TaskManager::_begin_next_task() msg.task_profile.task_id = id; msg.state = msg.STATE_COMPLETED; msg.robot_name = _context->name(); + msg.fleet_name = _context->description().owner(); msg.start_time = rmf_traffic_ros2::convert( _active_task->deployment_time()); msg.end_time = rmf_traffic_ros2::convert( From c516bcddc9f62e6192c8e89206a81d28d5bfb8c1 Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 26 Nov 2020 14:35:14 +0800 Subject: [PATCH 077/128] Debug/task planner candidates (#218) * Added test for planning an impossible to complete task * TaskPlanner returns empty assignments if request is impossible to complete --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 101 ++++++++++++++------ rmf_task/test/unit/agv/test_TaskPlanner.cpp | 65 +++++++++++++ 2 files changed, 138 insertions(+), 28 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index eefc24517..404d1eec2 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -193,7 +193,7 @@ class Candidates // Map finish time to Entry using Map = std::multimap; - static Candidates make( + static std::shared_ptr make( const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, @@ -283,7 +283,7 @@ class Candidates } }; -Candidates Candidates::make( +std::shared_ptr Candidates::make( const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, @@ -317,51 +317,82 @@ Candidates Candidates::make( { auto new_finish = request.estimate_finish( battery_estimate.value().finish_state(), state_config, estimate_cache); - assert(new_finish.has_value()); - initial_map.insert( - {new_finish.value().finish_state().finish_time(), - Entry{ - i, - new_finish.value().finish_state(), - new_finish.value().wait_until(), - state, - true}}); - } - else - { - std::cerr << "Unable to create entry for candidate [" << i - << "] and request [" << request.id() << " ]" << std::endl; - assert(false); + if (new_finish.has_value()) + { + initial_map.insert( + {new_finish.value().finish_state().finish_time(), + Entry{ + i, + new_finish.value().finish_state(), + new_finish.value().wait_until(), + state, + true}}); + } } } } - return Candidates(std::move(initial_map)); + if (initial_map.empty()) + { + std::cout << "Unable to create candidates for request [" << request.id() + << "]" << std::endl; + return nullptr; + } + + std::shared_ptr candidates( + new Candidates(std::move(initial_map))); + return candidates; } // ============================================================================ -struct PendingTask +class PendingTask { - PendingTask( +public: + + static std::shared_ptr make( std::vector& initial_states, std::vector& state_configs, rmf_task::ConstRequestPtr request_, rmf_task::ConstRequestPtr charge_battery_request, - std::shared_ptr estimate_cache) + std::shared_ptr estimate_cache); + + rmf_task::ConstRequestPtr request; + Candidates candidates; + rmf_traffic::Time earliest_start_time; + +private: + PendingTask( + rmf_task::ConstRequestPtr request_, + Candidates candidates_) : request(std::move(request_)), - candidates(Candidates::make(initial_states, state_configs, - *request, *charge_battery_request, estimate_cache)), + candidates(candidates_), earliest_start_time(request->earliest_start_time()) { // Do nothing } - - rmf_task::ConstRequestPtr request; - Candidates candidates; - rmf_traffic::Time earliest_start_time; }; +std::shared_ptr PendingTask::make( + std::vector& initial_states, + std::vector& state_configs, + rmf_task::ConstRequestPtr request_, + rmf_task::ConstRequestPtr charge_battery_request, + std::shared_ptr estimate_cache) +{ + const auto candidates = Candidates::make(initial_states, state_configs, + *request_, *charge_battery_request, estimate_cache); + + if (!candidates) + return nullptr; + + std::shared_ptr pending_task( + new PendingTask(request_, *candidates)); + return pending_task; +} + + + // ============================================================================ struct Node { @@ -726,6 +757,8 @@ class TaskPlanner::Implementation assert(initial_states.size() == state_configs.size()); auto node = make_initial_node(initial_states, state_configs, requests, time_now); + if (!node) + return {}; TaskPlanner::Assignments complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); @@ -777,6 +810,8 @@ class TaskPlanner::Implementation } node = make_initial_node(estimates, state_configs, new_tasks, time_now); + if (!node) + return {}; initial_states = estimates; } @@ -871,10 +906,20 @@ class TaskPlanner::Implementation // Generate a unique internal id for the request. Currently, multiple // requests with the same string id will be assigned different internal ids std::size_t internal_id = initial_node->get_available_internal_id(); + const auto pending_task= PendingTask::make( + initial_states, + state_configs, + request, + charge_battery, + estimate_cache); + + if (!pending_task) + return nullptr; + initial_node->unassigned_tasks.insert( { internal_id, - PendingTask(initial_states, state_configs, request, charge_battery, estimate_cache) + *pending_task }); } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 538c5e97d..24bbb65c0 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -766,4 +767,68 @@ SCENARIO("Grid World") REQUIRE(optimal_cost <= greedy_cost); } + WHEN("A loop request is impossible to fulfil") + { + const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; + + std::vector initial_states = + { + rmf_task::agv::State{first_location, 9, 1.0}, + }; + + std::vector state_configs = + { + rmf_task::agv::StateConfig{0.2}, + }; + + std::vector requests = + { + rmf_task::requests::Loop::make( + "Loop1", + 0, + 15, + 1000, + motion_sink, + device_sink, + planner, + now, + true) + }; + + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + rmf_task::agv::TaskPlanner task_planner(task_config); + + + auto start_time = std::chrono::steady_clock::now(); + const auto greedy_assignments = task_planner.greedy_plan( + now, initial_states, state_configs, requests); + const double greedy_cost = task_planner.compute_cost(greedy_assignments); + auto finish_time = std::chrono::steady_clock::now(); + std::cout << "Greedy solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Greedy", greedy_assignments, greedy_cost); + + task_planner = rmf_task::agv::TaskPlanner(task_config); + start_time = std::chrono::steady_clock::now(); + const auto optimal_assignments = task_planner.optimal_plan( + now, initial_states, state_configs, requests, nullptr); + const double optimal_cost = task_planner.compute_cost(optimal_assignments); + finish_time = std::chrono::steady_clock::now(); + std::cout << "Optimal solution found in: " + << (finish_time - start_time).count() / 1e9 << std::endl; + display_solution("Optimal", optimal_assignments, optimal_cost); + + REQUIRE(optimal_assignments.empty()); + REQUIRE(greedy_assignments.empty()); + REQUIRE(optimal_cost <= greedy_cost); + } + } \ No newline at end of file From 0f4849e6d4227039091d44152d9ee0551737c9ca Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 26 Nov 2020 19:44:06 +0800 Subject: [PATCH 078/128] Updated API documentation in FleetUpdateHandle --- .../rmf_fleet_adapter/agv/FleetUpdateHandle.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index a12628e08..a5e137e96 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -96,13 +96,12 @@ class FleetUpdateHandle : public std::enable_shared_from_this const bool drain_battery); - /// Set the threshold state of charge below which the robot should - /// automatically head back to its charging dock. The user is responsible to - /// set this value such that the robot is capable of reaching its nearest - /// charging station from anywhere on the map. Default value is 0.2. + /// Set the threshold for state of charge below which robots in this fleet + /// will cease to operate and require recharging. A value between 0.0 and 1.0 + /// should be specified. Default value is 0.2. /// /// \param[in] threshold - /// The fraction of the total battery capacity + /// Threshold as a fraction of the total battery capacity FleetUpdateHandle& set_recharge_threshold(const double threshold); /// A callback function that evaluates whether a fleet will accept a task @@ -143,7 +142,8 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// Provide a callback that indicates whether this fleet will accept a /// delivery request. By default all delivery requests will be rejected. /// - /// \note The callback function that you give should ideally be non-blocking + /// \note This function will be deprecated in favor of accept_task_requests() + /// The callback function that you give should ideally be non-blocking /// and return quickly. It's meant to check whether this fleet's vehicles are /// compatible with the requested payload, pickup, and dropoff behavior /// settings. The path planning feasibility will be taken care of by the From fc08b9e8a8508464afa891dabcaa97266a972174 Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 2 Dec 2020 13:27:20 +0800 Subject: [PATCH 079/128] TaskPlanner returns an std::variant (#221) * TaskPlanner solve methods return variant * Cleanup * Updated fleet adapter implementation * Fixed typo * Fixed logging * Fixed logging * Changed logging level to Error for task planner failure messages --- .../agv/FleetUpdateHandle.cpp | 50 +++- rmf_task/CMakeLists.txt | 4 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 22 +- .../rmf_task/requests/ChargeBattery.hpp | 3 + rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 71 ++++-- .../src/rmf_task/requests/ChargeBattery.cpp | 5 + rmf_task/test/unit/agv/test_TaskPlanner.cpp | 223 ++++++++++++------ 7 files changed, 278 insertions(+), 100 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 126f29a18..2e36a884b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -444,20 +444,60 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Generate new task assignments while accommodating for the new // request - // Call greedy_plan but run optimal_plan() in a separate thread - const auto assignments = task_planner->optimal_plan( + const auto result = task_planner->optimal_plan( rmf_traffic_ros2::convert(node->now()), states, state_configs, pending_requests, nullptr); + auto assignments_ptr = std::get_if< + rmf_task::agv::TaskPlanner::Assignments>(&result); + + if (!assignments_ptr) + { + auto error = std::get_if< + rmf_task::agv::TaskPlanner::TaskPlannerError>(&result); + + if (*error == rmf_task::agv::TaskPlanner::TaskPlannerError::low_battery) + { + RCLCPP_ERROR( + node->get_logger(), + "[TaskPlanner] Failed to compute assignments for task_id:[%s] due to" + " insufficient initial battery charge for all robots in this fleet.", + id.c_str()); + } + + else if (*error == + rmf_task::agv::TaskPlanner::TaskPlannerError::limited_capacity) + { + RCLCPP_ERROR( + node->get_logger(), + "[TaskPlanner] Failed to compute assignments for task_id:[%s] due to" + " insufficient battery capacity to accommodate one or more requests by" + " any of the robots in this fleet.", id.c_str()); + } + + else + { + RCLCPP_ERROR( + node->get_logger(), + "[TaskPlanner] Failed to compute assignments for task_id:[%s]", + id.c_str()); + } + + return; + } + + const auto assignments = *assignments_ptr; + if (assignments.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), - "Failed to compute assignments for task_id:[%s]", id.c_str()); - + "[TaskPlanner] Failed to compute assignments for task_id:[%s]", + id.c_str()); + return; } diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index f7e1f2138..3f67ee6f8 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -4,9 +4,9 @@ project(rmf_task VERSION 1.0.0) set(CMAKE_EXPORT_COMPILE_COMMANDS on) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 4610426de..9fd19512e 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -33,6 +33,7 @@ #include #include #include +#include namespace rmf_task { namespace agv { @@ -142,8 +143,23 @@ class TaskPlanner rmf_utils::impl_ptr _pimpl; }; + enum class TaskPlannerError + { + /// None of the agents in the initial states have sufficient initial charge + /// to even head back to their charging stations. Manual intervention is + /// needed to recharge one or more agents. + low_battery, + + /// None of the agents in the initial states have sufficient battery + /// capacity to accommodate one or more requests. This may be remedied by + /// increasing the battery capacity or by lowering the threshold_soc in the + /// state configs of the agents or by modifying the original request. + limited_capacity + }; + /// Container for assignments for each agent using Assignments = std::vector>; + using Result = std::variant; /// Constructor /// @@ -153,7 +169,7 @@ class TaskPlanner /// Get the greedy planner based assignments for a set of initial states and /// requests - Assignments greedy_plan( + Result greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, @@ -165,15 +181,17 @@ class TaskPlanner /// segment, this plan may take a while to be generated. Hence, it is /// recommended to call plan() method and use the greedy solution for bidding. /// If a bid is awarded, the optimal solution may be used for assignments. - Assignments optimal_plan( + Result optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, std::vector requests, std::function interrupter); + /// Compute the cost of a set of assignments double compute_cost(const Assignments& assignments); + /// Retrieve the task planner cache const std::shared_ptr estimate_cache() const; class Implementation; diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 1ddb8f1e8..b94bae004 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -61,6 +61,9 @@ class ChargeBattery : public rmf_task::Request const rmf_battery::agv::BatterySystem& battery_system() const; + /// Retrieve the charge soc which the battery will be charged upto + double max_charge_soc() const; + class Implementation; private: ChargeBattery(); diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 404d1eec2..b42652f38 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -197,8 +197,9 @@ class Candidates const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request, - const std::shared_ptr estimate_cache); + const rmf_task::requests::ChargeBattery& charge_battery_request, + const std::shared_ptr estimate_cache, + TaskPlanner::TaskPlannerError& error); Candidates(const Candidates& other) { @@ -287,8 +288,9 @@ std::shared_ptr Candidates::make( const std::vector& initial_states, const std::vector& state_configs, const rmf_task::Request& request, - const rmf_task::Request& charge_battery_request, - const std::shared_ptr estimate_cache) + const rmf_task::requests::ChargeBattery& charge_battery_request, + const std::shared_ptr estimate_cache, + TaskPlanner::TaskPlannerError& error) { Map initial_map; for (std::size_t i = 0; i < initial_states.size(); ++i) @@ -328,15 +330,30 @@ std::shared_ptr Candidates::make( state, true}}); } + else + { + error = TaskPlanner::TaskPlannerError::limited_capacity; + } + } + else + { + // Control reaches here either if ChargeBattery::estimate_finish() was + // called on initial state with full battery or low battery such that + // agent is unable to make it back to the charger + if (abs( + state.battery_soc() - charge_battery_request.max_charge_soc()) < 1e-3) + error = TaskPlanner::TaskPlannerError::limited_capacity; + else + error = TaskPlanner::TaskPlannerError::low_battery; + } + } } if (initial_map.empty()) { - std::cout << "Unable to create candidates for request [" << request.id() - << "]" << std::endl; return nullptr; } @@ -355,7 +372,8 @@ class PendingTask std::vector& state_configs, rmf_task::ConstRequestPtr request_, rmf_task::ConstRequestPtr charge_battery_request, - std::shared_ptr estimate_cache); + std::shared_ptr estimate_cache, + TaskPlanner::TaskPlannerError& error); rmf_task::ConstRequestPtr request; Candidates candidates; @@ -378,11 +396,16 @@ std::shared_ptr PendingTask::make( std::vector& state_configs, rmf_task::ConstRequestPtr request_, rmf_task::ConstRequestPtr charge_battery_request, - std::shared_ptr estimate_cache) + std::shared_ptr estimate_cache, + TaskPlanner::TaskPlannerError& error) { + + auto battery_request = std::dynamic_pointer_cast< + const rmf_task::requests::ChargeBattery>(charge_battery_request); + const auto candidates = Candidates::make(initial_states, state_configs, - *request_, *charge_battery_request, estimate_cache); - + *request_, *battery_request, estimate_cache, error); + if (!candidates) return nullptr; @@ -745,8 +768,8 @@ class TaskPlanner::Implementation return assignments; } - - Assignments complete_solve( + + Result complete_solve( rmf_traffic::Time time_now, std::vector& initial_states, const std::vector& state_configs, @@ -755,10 +778,11 @@ class TaskPlanner::Implementation bool greedy) { assert(initial_states.size() == state_configs.size()); - - auto node = make_initial_node(initial_states, state_configs, requests, time_now); + TaskPlannerError error; + auto node = make_initial_node( + initial_states, state_configs, requests, time_now, error); if (!node) - return {}; + return error; TaskPlanner::Assignments complete_assignments; complete_assignments.resize(node->assigned_tasks.size()); @@ -809,9 +833,10 @@ class TaskPlanner::Implementation estimates[i] = assignments.back().assignment.state(); } - node = make_initial_node(estimates, state_configs, new_tasks, time_now); + node = make_initial_node( + estimates, state_configs, new_tasks, time_now, error); if (!node) - return {}; + return error; initial_states = estimates; } @@ -893,7 +918,8 @@ class TaskPlanner::Implementation std::vector initial_states, std::vector state_configs, std::vector requests, - rmf_traffic::Time time_now) + rmf_traffic::Time time_now, + TaskPlannerError& error) { auto initial_node = std::make_shared(); @@ -911,8 +937,9 @@ class TaskPlanner::Implementation state_configs, request, charge_battery, - estimate_cache); - + estimate_cache, + error); + if (!pending_task) return nullptr; @@ -1359,7 +1386,7 @@ auto TaskPlanner::greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests) -> Assignments + std::vector requests) -> Result { return _pimpl->complete_solve( time_now, @@ -1375,7 +1402,7 @@ auto TaskPlanner::optimal_plan( std::vector initial_states, std::vector state_configs, std::vector requests, - std::function interrupter) -> Assignments + std::function interrupter) -> Result { return _pimpl->complete_solve( time_now, diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 8c71c850d..9e3eb93f9 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -206,5 +206,10 @@ const rmf_battery::agv::BatterySystem& ChargeBattery::battery_system() const return *_pimpl->_battery_system; } +double ChargeBattery::max_charge_soc() const +{ + return _pimpl->_charge_soc; +} + } // namespace requests } // namespace rmf_task diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 24bbb65c0..e85b06375 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -41,10 +41,11 @@ #include +using TaskPlanner = rmf_task::agv::TaskPlanner; //============================================================================== inline bool check_implicit_charging_task_start( - const rmf_task::agv::TaskPlanner::Assignments& assignments, + const TaskPlanner::Assignments& assignments, const double initial_soc) { bool implicit_charging_task_added = false; @@ -76,7 +77,7 @@ inline bool check_implicit_charging_task_start( //============================================================================== inline void display_solution( std::string parent, - const rmf_task::agv::TaskPlanner::Assignments& assignments, + const TaskPlanner::Assignments& assignments, const double cost) { std::cout << parent << " cost: " << cost << std::endl; @@ -225,34 +226,39 @@ SCENARIO("Grid World") drain_battery) }; - std::shared_ptr task_config = - std::make_shared( + std::shared_ptr task_config = + std::make_shared( battery_system, motion_sink, device_sink, planner); - rmf_task::agv::TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_assignments = task_planner.greedy_plan( + const auto greedy_result = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); + const auto greedy_assignments = std::get_if< + TaskPlanner::Assignments>(&greedy_result); + REQUIRE(greedy_assignments); + const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); std::cout << "Greedy solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Greedy", *greedy_assignments, greedy_cost); // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another - task_planner = rmf_task::agv::TaskPlanner(task_config); + task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); - const auto optimal_assignments = task_planner.optimal_plan( + const auto optimal_result = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); + const auto optimal_assignments = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); std::cout << "Optimal solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", optimal_assignments, optimal_cost); + display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } @@ -423,32 +429,39 @@ SCENARIO("Grid World") drain_battery) }; - std::shared_ptr task_config = - std::make_shared( + std::shared_ptr task_config = + std::make_shared( battery_system, motion_sink, device_sink, planner); - rmf_task::agv::TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_assignments = task_planner.greedy_plan( + const auto greedy_result = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); + const auto greedy_assignments = std::get_if< + TaskPlanner::Assignments>(&greedy_result); + REQUIRE(greedy_assignments); + const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); std::cout << "Greedy solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Greedy", *greedy_assignments, greedy_cost); - task_planner = rmf_task::agv::TaskPlanner(task_config); + // Create new TaskPlanner to reset cache so that measured run times + // remain independent of one another + task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); - const auto optimal_assignments = task_planner.optimal_plan( + const auto optimal_result = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); + const auto optimal_assignments = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); std::cout << "Optimal solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", optimal_assignments, optimal_cost); + display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } @@ -529,32 +542,39 @@ SCENARIO("Grid World") drain_battery) }; - std::shared_ptr task_config = - std::make_shared( + std::shared_ptr task_config = + std::make_shared( battery_system, motion_sink, device_sink, planner); - rmf_task::agv::TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_assignments = task_planner.greedy_plan( + const auto greedy_result = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); + const auto greedy_assignments = std::get_if< + TaskPlanner::Assignments>(&greedy_result); + REQUIRE(greedy_assignments); + const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); std::cout << "Greedy solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Greedy", *greedy_assignments, greedy_cost); - task_planner = rmf_task::agv::TaskPlanner(task_config); + // Create new TaskPlanner to reset cache so that measured run times + // remain independent of one another + task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); - const auto optimal_assignments = task_planner.optimal_plan( + const auto optimal_result = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); + const auto optimal_assignments = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); std::cout << "Optimal solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", optimal_assignments, optimal_cost); + display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); @@ -562,11 +582,11 @@ SCENARIO("Grid World") // Checks if Assignments take into account a charging task in the beginning // without explicitly including the task in Assignments. bool implicit_charging_task_added = check_implicit_charging_task_start( - greedy_assignments, initial_soc); + *greedy_assignments, initial_soc); REQUIRE(!implicit_charging_task_added); implicit_charging_task_added = check_implicit_charging_task_start( - optimal_assignments, initial_soc); + *optimal_assignments, initial_soc); REQUIRE(!implicit_charging_task_added); } @@ -736,38 +756,45 @@ SCENARIO("Grid World") drain_battery) }; - std::shared_ptr task_config = - std::make_shared( + std::shared_ptr task_config = + std::make_shared( battery_system, motion_sink, device_sink, planner); - rmf_task::agv::TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config); auto start_time = std::chrono::steady_clock::now(); - const auto greedy_assignments = task_planner.greedy_plan( + const auto greedy_result = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); + const auto greedy_assignments = std::get_if< + TaskPlanner::Assignments>(&greedy_result); + REQUIRE(greedy_assignments); + const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); std::cout << "Greedy solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", greedy_assignments, greedy_cost); + display_solution("Greedy", *greedy_assignments, greedy_cost); - task_planner = rmf_task::agv::TaskPlanner(task_config); + // Create new TaskPlanner to reset cache so that measured run times + // remain independent of one another + task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); - const auto optimal_assignments = task_planner.optimal_plan( + const auto optimal_result = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); + const auto optimal_assignments = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); std::cout << "Optimal solution found in: " << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", optimal_assignments, optimal_cost); + display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } - WHEN("A loop request is impossible to fulfil") + WHEN("A loop request is impossible to fulfil due to battery capacity") { const auto now = std::chrono::steady_clock::now(); const double default_orientation = 0.0; @@ -776,7 +803,7 @@ SCENARIO("Grid World") std::vector initial_states = { - rmf_task::agv::State{first_location, 9, 1.0}, + rmf_task::agv::State{first_location, 13, 1.0}, }; std::vector state_configs = @@ -798,37 +825,95 @@ SCENARIO("Grid World") true) }; - std::shared_ptr task_config = - std::make_shared( + std::shared_ptr task_config = + std::make_shared( battery_system, motion_sink, device_sink, planner); - rmf_task::agv::TaskPlanner task_planner(task_config); + TaskPlanner task_planner(task_config); - - auto start_time = std::chrono::steady_clock::now(); - const auto greedy_assignments = task_planner.greedy_plan( + const auto greedy_result = task_planner.greedy_plan( now, initial_states, state_configs, requests); - const double greedy_cost = task_planner.compute_cost(greedy_assignments); - auto finish_time = std::chrono::steady_clock::now(); - std::cout << "Greedy solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", greedy_assignments, greedy_cost); - - task_planner = rmf_task::agv::TaskPlanner(task_config); - start_time = std::chrono::steady_clock::now(); - const auto optimal_assignments = task_planner.optimal_plan( + const auto greedy_assignments = std::get_if< + TaskPlanner::Assignments>(&greedy_result); + REQUIRE_FALSE(greedy_assignments); + auto error = std::get_if< + TaskPlanner::TaskPlannerError>(&greedy_result); + CHECK(*error == TaskPlanner::TaskPlannerError::limited_capacity); + + // Create new TaskPlanner to reset cache so that measured run times + // remain independent of one another + task_planner = TaskPlanner(task_config); + const auto optimal_result = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); - const double optimal_cost = task_planner.compute_cost(optimal_assignments); - finish_time = std::chrono::steady_clock::now(); - std::cout << "Optimal solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", optimal_assignments, optimal_cost); + const auto optimal_assignments = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + REQUIRE_FALSE(optimal_assignments); + error = std::get_if( + &optimal_result); + CHECK(*error == TaskPlanner::TaskPlannerError::limited_capacity); + } + + WHEN("A loop request is impossible to fulfil due to low initial battery") + { + const auto now = std::chrono::steady_clock::now(); + const double default_orientation = 0.0; + + rmf_traffic::agv::Plan::Start first_location{now, 9, default_orientation}; + + std::vector initial_states = + { + rmf_task::agv::State{first_location, 13, 0.0}, + }; + + std::vector state_configs = + { + rmf_task::agv::StateConfig{0.2}, + }; + + std::vector requests = + { + rmf_task::requests::Loop::make( + "Loop1", + 0, + 15, + 1000, + motion_sink, + device_sink, + planner, + now, + true) + }; - REQUIRE(optimal_assignments.empty()); - REQUIRE(greedy_assignments.empty()); - REQUIRE(optimal_cost <= greedy_cost); + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + TaskPlanner task_planner(task_config); + + const auto greedy_result = task_planner.greedy_plan( + now, initial_states, state_configs, requests); + const auto greedy_assignments = std::get_if< + TaskPlanner::Assignments>(&greedy_result); + REQUIRE_FALSE(greedy_assignments); + auto error = std::get_if< + TaskPlanner::TaskPlannerError>(&greedy_result); + CHECK(*error == TaskPlanner::TaskPlannerError::low_battery); + + // Create new TaskPlanner to reset cache so that measured run times + // remain independent of one another + task_planner = TaskPlanner(task_config); + const auto optimal_result = task_planner.optimal_plan( + now, initial_states, state_configs, requests, nullptr); + const auto optimal_assignments = std::get_if< + TaskPlanner::Assignments>(&optimal_result); + REQUIRE_FALSE(optimal_assignments); + error = std::get_if( + &optimal_result); + CHECK(*error == TaskPlanner::TaskPlannerError::low_battery); } } \ No newline at end of file From 993fa5dd5a08a300c76404a77d73ee608c9db10e Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 2 Dec 2020 13:27:42 +0800 Subject: [PATCH 080/128] Automatic retreat for battery recharging (#222) * TaskManager initiates a ChargeBattery request when robot is idle and battery will fall below threshold * Adjusted retreat threshold --- .../src/rmf_fleet_adapter/TaskManager.cpp | 105 +++++++++++++++++- .../src/rmf_fleet_adapter/TaskManager.hpp | 11 +- .../agv/FleetUpdateHandle.cpp | 3 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 12 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 9 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 3 + rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 13 +++ 7 files changed, 148 insertions(+), 8 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 5bb3b5d77..f10a52d66 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -24,6 +24,8 @@ #include +#include + #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" #include "tasks/Delivery.hpp" @@ -52,7 +54,7 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) } }); - mgr->_timer = mgr->context()->node()->create_wall_timer( + mgr->_task_timer = mgr->context()->node()->create_wall_timer( std::chrono::seconds(1), [w = mgr->weak_from_this()]() { @@ -61,6 +63,16 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) mgr->_begin_next_task(); } }); + + mgr->_retreat_timer = mgr->context()->node()->create_wall_timer( + std::chrono::seconds(10), + [w = mgr->weak_from_this()]() + { + if (auto mgr = w.lock()) + { + mgr->retreat_to_charger(); + } + }); return mgr; } @@ -359,4 +371,95 @@ void TaskManager::clear_queue() _queue.clear(); } +//============================================================================== +void TaskManager::retreat_to_charger() +{ + if (_active_task || !_queue.empty()) + return; + + const auto task_planner = _context->task_planner(); + if (!task_planner) + return; + + const auto current_state = expected_finish_state(); + if (current_state.waypoint() == current_state.charging_waypoint()) + return; + + const double threshold_soc = _context->state_config().threshold_soc(); + const double retreat_threshold = 1.2 * threshold_soc; + const double current_battery_soc = _context->current_battery_soc(); + + const auto task_planner_config = task_planner->config(); + const auto estimate_cache = task_planner->estimate_cache(); + + double retreat_battery_drain = 0.0; + const auto endpoints = std::make_pair(current_state.waypoint(), + current_state.charging_waypoint()); + const auto& cache_result = estimate_cache->get(endpoints); + + if (cache_result) + { + retreat_battery_drain = cache_result->dsoc; + } + else + { + const rmf_traffic::agv::Planner::Goal retreat_goal{current_state.charging_waypoint()}; + const auto result_to_charger = task_planner_config->planner()->plan( + current_state.location(), retreat_goal); + + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - current_state.finish_time(); + + const double dSOC_motion = + task_planner_config->motion_sink()->compute_change_in_charge(trajectory); + const double dSOC_device = + task_planner_config->ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_device; + + // TODO(YV) Protect this call with a mutex + estimate_cache->set(endpoints, retreat_duration, + retreat_battery_drain); + } + + const double battery_soc_after_retreat = + current_battery_soc - retreat_battery_drain; + + if ((battery_soc_after_retreat < retreat_threshold) && + (battery_soc_after_retreat > threshold_soc)) + { + // Add a new charging task to the task queue + auto charging_request = rmf_task::requests::ChargeBattery::make( + task_planner_config->battery_system(), + task_planner_config->motion_sink(), + task_planner_config->ambient_sink(), + task_planner_config->planner(), + current_state.finish_time()); + + const auto finish = charging_request->estimate_finish( + current_state, + _context->state_config(), + estimate_cache); + + if (!finish) + return; + + rmf_task::agv::TaskPlanner::Assignment charging_assignment( + charging_request, + finish.value().finish_state(), + current_state.finish_time()); + + set_queue({charging_assignment}); + + RCLCPP_INFO( + _context->node()->get_logger(), + "Initiating automatic retreat to charger for robot [%s]", + _context->name().c_str()); + } +} + } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index b5c1bd224..7284684c7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -64,12 +64,16 @@ class TaskManager : public std::enable_shared_from_this /// Get the non-charging requests among pending tasks const std::vector requests() const; - // Callback for timer which begins next task if its deployment time has passed + /// Callback for task timer which begins next task if its deployment time has passed void _begin_next_task(); - // The state of the robot. + /// The state of the robot. State expected_finish_state() const; + /// Callback for the retreat timer. Appends a charging task to the task queue + /// when robot is idle and battery level drops below a retreat threshold. + void retreat_to_charger(); + private: TaskManager(agv::RobotContextPtr context); @@ -82,7 +86,8 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _emergency_sub; std::mutex _mutex; - rclcpp::TimerBase::SharedPtr _timer; + rclcpp::TimerBase::SharedPtr _task_timer; + rclcpp::TimerBase::SharedPtr _retreat_timer; void clear_queue(); }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 2e36a884b..bf42b280a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -673,7 +673,8 @@ void FleetUpdateHandle::add_robot( fleet->_pimpl->worker, fleet->_pimpl->default_maximum_delay, state, - state_config + state_config, + fleet->_pimpl->task_planner }); // We schedule the following operations on the worker to make sure we do not diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 4b322710c..beed3ce6f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -253,6 +253,12 @@ const rxcpp::observable& RobotContext::observe_battery_soc() const return _battery_soc_obs; } +//============================================================================== +const std::shared_ptr& +RobotContext::task_planner() const +{ + return _task_planner; +} //============================================================================== void RobotContext::respond( @@ -284,7 +290,8 @@ RobotContext::RobotContext( const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, rmf_task::agv::State state, - rmf_task::agv::StateConfig state_config) + rmf_task::agv::StateConfig state_config, + std::shared_ptr task_planner) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), _itinerary(std::move(itinerary)), @@ -296,7 +303,8 @@ RobotContext::RobotContext( _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), _state(state), - _state_config(state_config) + _state_config(state_config), + _task_planner(std::move(task_planner)) { _profile = std::make_shared( _itinerary.description().profile()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 95d274960..cfb359ac5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -11,6 +11,7 @@ #include #include +#include #include @@ -134,6 +135,10 @@ class RobotContext // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; + /// Get a mutable reference to the task planner for this robot + const std::shared_ptr& task_planner() const; + + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; @@ -148,7 +153,8 @@ class RobotContext const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, rmf_task::agv::State state, - rmf_task::agv::StateConfig state_config); + rmf_task::agv::StateConfig state_config, + std::shared_ptr task_planner); std::weak_ptr _command_handle; std::vector _location; @@ -175,6 +181,7 @@ class RobotContext rxcpp::observable _battery_soc_obs; rmf_task::agv::State _state; rmf_task::agv::StateConfig _state_config; + std::shared_ptr _task_planner; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 9fd19512e..8e454a1d7 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -167,6 +167,9 @@ class TaskPlanner /// The configuration for the planner TaskPlanner(std::shared_ptr config); + /// Get a shared pointer to the configuration of this task planner + const std::shared_ptr config() const; + /// Get the greedy planner based assignments for a set of initial states and /// requests Result greedy_plan( diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index b42652f38..af2dacb85 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -702,9 +702,11 @@ bool Filter::ignore(const Node& node) return !new_node; } +// ============================================================================ const rmf_traffic::Duration segmentation_threshold = rmf_traffic::time::from_seconds(1.0); +// ============================================================================ inline double compute_g_assignment(const TaskPlanner::Assignment& assignment) { if (std::dynamic_pointer_cast( @@ -719,6 +721,7 @@ inline double compute_g_assignment(const TaskPlanner::Assignment& assignment) } // anonymous namespace +// ============================================================================ class TaskPlanner::Implementation { public: @@ -1372,6 +1375,7 @@ class TaskPlanner::Implementation }; +// ============================================================================ TaskPlanner::TaskPlanner(std::shared_ptr config) : _pimpl(rmf_utils::make_impl( Implementation{ @@ -1382,6 +1386,7 @@ TaskPlanner::TaskPlanner(std::shared_ptr config) // Do nothing } +// ============================================================================ auto TaskPlanner::greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, @@ -1397,6 +1402,7 @@ auto TaskPlanner::greedy_plan( true); } +// ============================================================================ auto TaskPlanner::optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, @@ -1413,16 +1419,23 @@ auto TaskPlanner::optimal_plan( false); } +// ============================================================================ auto TaskPlanner::compute_cost(const Assignments& assignments) -> double { return _pimpl->compute_g(assignments); } +// ============================================================================ const std::shared_ptr TaskPlanner::estimate_cache() const { return _pimpl->estimate_cache; } +// ============================================================================ +const std::shared_ptr TaskPlanner::config() const +{ + return _pimpl->config; +} } // namespace agv } // namespace rmf_task From 09d795e8a17d8b397e5ef037521577f100c66748 Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Wed, 2 Dec 2020 15:02:38 +0800 Subject: [PATCH 081/128] Take into account battery drain while idling in TaskPlanner estimates (#223) * Take into account charge depletion due to waiting to begin task * Add charge depletion for Loop and Clean tasks. Rename Delivery::Implementation Also modifies ChargeBattery requests to return a value even if charge is already at 100%, if a robot is not currently at a charger. * Remove leading underscores from ChargeBattery::Implementation --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 1 - .../src/rmf_task/requests/ChargeBattery.cpp | 69 ++++---- rmf_task/src/rmf_task/requests/Clean.cpp | 16 ++ rmf_task/src/rmf_task/requests/Delivery.cpp | 147 ++++++++++-------- rmf_task/src/rmf_task/requests/Loop.cpp | 16 ++ 5 files changed, 152 insertions(+), 97 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index af2dacb85..73c4ed701 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -1367,7 +1367,6 @@ class TaskPlanner::Implementation // Add copies and with a newly assigned task to queue for (const auto&n : new_nodes) priority_queue.push(n); - } return nullptr; diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 9e3eb93f9..ad8beaca9 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -54,17 +54,17 @@ class ChargeBattery::Implementation {} // fixed id for now - std::string _id = "Charge"; - rmf_battery::agv::BatterySystemPtr _battery_system; - std::shared_ptr _motion_sink; - std::shared_ptr _device_sink; - std::shared_ptr _planner; - rmf_traffic::Time _start_time; - bool _drain_battery; + std::string id = "Charge"; + rmf_battery::agv::BatterySystemPtr battery_system; + std::shared_ptr motion_sink; + std::shared_ptr device_sink; + std::shared_ptr planner; + rmf_traffic::Time start_time; + bool drain_battery; // soc to always charge the battery up to - double _charge_soc = 1.0; - rmf_traffic::Duration _invariant_duration; + double charge_soc = 1.0; + rmf_traffic::Duration invariant_duration; }; //============================================================================== @@ -77,20 +77,20 @@ rmf_task::ConstRequestPtr ChargeBattery::make( bool drain_battery) { std::shared_ptr charge_battery(new ChargeBattery()); - charge_battery->_pimpl->_id += generate_uuid(); - charge_battery->_pimpl->_battery_system = + charge_battery->_pimpl->id += generate_uuid(); + charge_battery->_pimpl->battery_system = rmf_battery::agv::BatterySystemPtr(new rmf_battery::agv::BatterySystem( battery_system.nominal_voltage(), battery_system.capacity(), battery_system.charging_current(), battery_system.type(), battery_system.profile())); - charge_battery->_pimpl->_motion_sink = std::move(motion_sink); - charge_battery->_pimpl->_device_sink = std::move(device_sink); - charge_battery->_pimpl->_planner = std::move(planner); - charge_battery->_pimpl->_start_time = start_time; - charge_battery->_pimpl->_drain_battery = drain_battery; - charge_battery->_pimpl->_invariant_duration = + charge_battery->_pimpl->motion_sink = std::move(motion_sink); + charge_battery->_pimpl->device_sink = std::move(device_sink); + charge_battery->_pimpl->planner = std::move(planner); + charge_battery->_pimpl->start_time = start_time; + charge_battery->_pimpl->drain_battery = drain_battery; + charge_battery->_pimpl->invariant_duration = rmf_traffic::time::from_seconds(0.0); return charge_battery; } @@ -103,7 +103,7 @@ ChargeBattery::ChargeBattery() //============================================================================== std::string ChargeBattery::id() const { - return _pimpl->_id; + return _pimpl->id; } //============================================================================== @@ -112,7 +112,14 @@ rmf_utils::optional ChargeBattery::estimate_finish( const agv::StateConfig& state_config, const std::shared_ptr estimate_cache) const { - if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) + // 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. + if ((abs(initial_state.battery_soc() - _pimpl->charge_soc) < 1e-3) + && initial_state.waypoint() == initial_state.charging_waypoint()) return rmf_utils::nullopt; // Compute time taken to reach charging waypoint from current location @@ -147,17 +154,17 @@ rmf_utils::optional ChargeBattery::estimate_finish( { // Compute plan to charging waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result = _pimpl->_planner->plan( + const auto result = _pimpl->planner->plan( initial_state.location(), goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); variant_duration = finish_time - start_time; - if (_pimpl->_drain_battery) + if (_pimpl->drain_battery) { - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge( + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge( trajectory); - dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + dSOC_device = _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_device; } @@ -172,18 +179,18 @@ rmf_utils::optional ChargeBattery::estimate_finish( } // Default _charge_soc = 1.0 - double delta_soc = _pimpl->_charge_soc - battery_soc; + double delta_soc = _pimpl->charge_soc - battery_soc; assert(delta_soc >= 0.0); double time_to_charge = - (3600 * delta_soc * _pimpl->_battery_system->capacity()) / - _pimpl->_battery_system->charging_current(); + (3600 * delta_soc * _pimpl->battery_system->capacity()) / + _pimpl->battery_system->charging_current(); const rmf_traffic::Time wait_until = initial_state.finish_time(); state.finish_time( wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge)); - state.battery_soc(_pimpl->_charge_soc); + state.battery_soc(_pimpl->charge_soc); return Estimate(state, wait_until); } @@ -191,24 +198,24 @@ rmf_utils::optional ChargeBattery::estimate_finish( //============================================================================== rmf_traffic::Duration ChargeBattery::invariant_duration() const { - return _pimpl->_invariant_duration; + return _pimpl->invariant_duration; } //============================================================================== rmf_traffic::Time ChargeBattery::earliest_start_time() const { - return _pimpl->_start_time; + return _pimpl->start_time; } //============================================================================== const rmf_battery::agv::BatterySystem& ChargeBattery::battery_system() const { - return *_pimpl->_battery_system; + return *_pimpl->battery_system; } double ChargeBattery::max_charge_soc() const { - return _pimpl->_charge_soc; + return _pimpl->charge_soc; } } // namespace requests diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index e7519c7b7..f0df52a83 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -182,6 +182,22 @@ rmf_utils::optional Clean::estimate_finish( initial_state.finish_time() > ideal_start ? initial_state.finish_time() : 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 (_pimpl->drain_battery && wait_until > initial_state.finish_time() && + initial_state.waypoint() != initial_state.charging_waypoint()) + { + rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); + dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_ambient; + + if (battery_soc <= state_config.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + // Factor in invariants state.finish_time( wait_until + variant_duration + _pimpl->invariant_duration + end_duration); diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 0f372b6a6..176370313 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -30,20 +30,20 @@ class Delivery::Implementation Implementation() {} - std::string _id; - std::size_t _pickup_waypoint; - std::string _pickup_dispenser; - std::size_t _dropoff_waypoint; - std::string _dropoff_ingestor; - std::vector _items; - std::shared_ptr _motion_sink; - std::shared_ptr _device_sink; - std::shared_ptr _planner; - bool _drain_battery; - rmf_traffic::Time _start_time; - - rmf_traffic::Duration _invariant_duration; - double _invariant_battery_drain; + std::string id; + std::size_t pickup_waypoint; + std::string pickup_dispenser; + std::size_t dropoff_waypoint; + std::string dropoff_ingestor; + std::vector items; + std::shared_ptr motion_sink; + std::shared_ptr device_sink; + std::shared_ptr planner; + bool drain_battery; + rmf_traffic::Time start_time; + + rmf_traffic::Duration invariant_duration; + double invariant_battery_drain; }; //============================================================================== @@ -61,47 +61,47 @@ rmf_task::ConstRequestPtr Delivery::make( bool drain_battery) { std::shared_ptr delivery(new Delivery()); - delivery->_pimpl->_id = id; - delivery->_pimpl->_pickup_waypoint = pickup_waypoint; - delivery->_pimpl->_pickup_dispenser = std::move(pickup_dispenser); - delivery->_pimpl->_dropoff_waypoint = dropoff_waypoint; - delivery->_pimpl->_dropoff_ingestor = std::move(dropoff_ingestor); - delivery->_pimpl->_items = std::move(items); - delivery->_pimpl->_motion_sink = std::move(motion_sink); - delivery->_pimpl->_device_sink = std::move(device_sink); - delivery->_pimpl->_planner = std::move(planner); - delivery->_pimpl->_drain_battery = drain_battery; - delivery->_pimpl->_start_time = start_time; + delivery->_pimpl->id = id; + delivery->_pimpl->pickup_waypoint = pickup_waypoint; + delivery->_pimpl->pickup_dispenser = std::move(pickup_dispenser); + delivery->_pimpl->dropoff_waypoint = dropoff_waypoint; + delivery->_pimpl->dropoff_ingestor = std::move(dropoff_ingestor); + delivery->_pimpl->items = std::move(items); + delivery->_pimpl->motion_sink = std::move(motion_sink); + delivery->_pimpl->device_sink = std::move(device_sink); + delivery->_pimpl->planner = std::move(planner); + delivery->_pimpl->drain_battery = drain_battery; + delivery->_pimpl->start_time = start_time; // Calculate duration of invariant component of task - delivery->_pimpl->_invariant_duration = rmf_traffic::Duration{0}; - delivery->_pimpl->_invariant_battery_drain = 0.0; + delivery->_pimpl->invariant_duration = rmf_traffic::Duration{0}; + delivery->_pimpl->invariant_battery_drain = 0.0; - if (delivery->_pimpl->_pickup_waypoint != delivery->_pimpl->_dropoff_waypoint) + if (delivery->_pimpl->pickup_waypoint != delivery->_pimpl->dropoff_waypoint) { const auto plan_start_time = std::chrono::steady_clock::now(); rmf_traffic::agv::Planner::Start start{ plan_start_time, - delivery->_pimpl->_pickup_waypoint, + delivery->_pimpl->pickup_waypoint, 0.0}; - rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; - const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); + rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->dropoff_waypoint}; + const auto result_to_dropoff = delivery->_pimpl->planner->plan(start, goal); const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + delivery->_pimpl->invariant_duration = finish_time - plan_start_time; - if (delivery->_pimpl->_drain_battery) + if (delivery->_pimpl->drain_battery) { // Compute battery drain const double dSOC_motion = - delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); + delivery->_pimpl->motion_sink->compute_change_in_charge(trajectory); const double dSOC_device = - delivery->_pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); - delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + delivery->_pimpl->device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(delivery->_pimpl->invariant_duration)); + delivery->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_device; } } @@ -116,7 +116,7 @@ Delivery::Delivery() //============================================================================== std::string Delivery::id() const { - return _pimpl->_id; + return _pimpl->id; } //============================================================================== @@ -127,7 +127,7 @@ rmf_utils::optional Delivery::estimate_finish( { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), - _pimpl->_dropoff_waypoint, + _pimpl->dropoff_waypoint, initial_state.location().orientation()}; agv::State state{ std::move(final_plan_start), @@ -141,10 +141,11 @@ rmf_utils::optional Delivery::estimate_finish( double dSOC_motion = 0.0; double dSOC_device = 0.0; - if (initial_state.waypoint() != _pimpl->_pickup_waypoint) + // Factor in battery drain while moving to start waypoint of task + if (initial_state.waypoint() != _pimpl->pickup_waypoint) { const auto endpoints = std::make_pair(initial_state.waypoint(), - _pimpl->_pickup_waypoint); + _pimpl->pickup_waypoint); const auto& cache_result = estimate_cache->get(endpoints); // Use previously memoized values if possible if (cache_result) @@ -156,7 +157,7 @@ rmf_utils::optional Delivery::estimate_finish( { // Compute plan to pickup waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result_to_pickup = _pimpl->_planner->plan( + const auto result_to_pickup = _pimpl->planner->plan( initial_state.location(), goal); // We assume we can always compute a plan const auto& trajectory = @@ -164,12 +165,12 @@ rmf_utils::optional Delivery::estimate_finish( const auto& finish_time = *trajectory.finish_time(); variant_duration = finish_time - start_time; - if (_pimpl->_drain_battery) + if (_pimpl->drain_battery) { // Compute battery drain - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); dSOC_device = - _pimpl->_device_sink->compute_change_in_charge( + _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_device; } @@ -181,26 +182,42 @@ rmf_utils::optional Delivery::estimate_finish( return rmf_utils::nullopt; } - const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; const rmf_traffic::Time wait_until = initial_state.finish_time() > ideal_start ? initial_state.finish_time() : 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 (_pimpl->drain_battery && wait_until > initial_state.finish_time() && + initial_state.waypoint() != initial_state.charging_waypoint()) + { + rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); + dSOC_device = _pimpl->device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_device; + + if (battery_soc <= state_config.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + // Factor in invariants state.finish_time( - wait_until + variant_duration + _pimpl->_invariant_duration); + wait_until + variant_duration + _pimpl->invariant_duration); - if (_pimpl->_drain_battery) + if (_pimpl->drain_battery) { - battery_soc -= _pimpl->_invariant_battery_drain; + battery_soc -= _pimpl->invariant_battery_drain; if (battery_soc <= state_config.threshold_soc()) return rmf_utils::nullopt; // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; - if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) + if ( _pimpl->dropoff_waypoint != state.charging_waypoint()) { - const auto endpoints = std::make_pair(_pimpl->_dropoff_waypoint, + const auto endpoints = std::make_pair(_pimpl->dropoff_waypoint, state.charging_waypoint()); const auto& cache_result = estimate_cache->get(endpoints); if (cache_result) @@ -216,7 +233,7 @@ rmf_utils::optional Delivery::estimate_finish( rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result_to_charger = _pimpl->_planner->plan(start, goal); + const auto result_to_charger = _pimpl->planner->plan(start, goal); // We assume we can always compute a plan const auto& trajectory = result_to_charger->get_itinerary().back().trajectory(); @@ -224,8 +241,8 @@ rmf_utils::optional Delivery::estimate_finish( const rmf_traffic::Duration retreat_duration = finish_time - state.finish_time(); - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); - dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(retreat_duration)); retreat_battery_drain = dSOC_motion + dSOC_device; @@ -246,54 +263,54 @@ rmf_utils::optional Delivery::estimate_finish( //============================================================================== rmf_traffic::Duration Delivery::invariant_duration() const { - return _pimpl->_invariant_duration; + return _pimpl->invariant_duration; } //============================================================================== rmf_traffic::Time Delivery::earliest_start_time() const { - return _pimpl->_start_time; + return _pimpl->start_time; } //============================================================================== std::size_t Delivery::pickup_waypoint() const { - return _pimpl->_pickup_waypoint; + return _pimpl->pickup_waypoint; } //============================================================================== const std::string& Delivery::pickup_dispenser() const { - return _pimpl->_pickup_dispenser; + return _pimpl->pickup_dispenser; } //============================================================================== const std::string& Delivery::dropoff_ingestor() const { - return _pimpl->_dropoff_ingestor; + return _pimpl->dropoff_ingestor; } //============================================================================== std::size_t Delivery::dropoff_waypoint() const { - return _pimpl->_dropoff_waypoint; + return _pimpl->dropoff_waypoint; } //============================================================================== const std::vector& Delivery::items() const { - return _pimpl->_items; + return _pimpl->items; } //============================================================================== Delivery::Start Delivery::dropoff_start(const Delivery::Start& start) const { - if (start.waypoint() == _pimpl->_pickup_waypoint) + if (start.waypoint() == _pimpl->pickup_waypoint) return start; - rmf_traffic::agv::Planner::Goal goal{_pimpl->_pickup_waypoint}; + rmf_traffic::agv::Planner::Goal goal{_pimpl->pickup_waypoint}; - const auto result = _pimpl->_planner->plan(start, goal); + const auto result = _pimpl->planner->plan(start, goal); // We assume we can always compute a plan const auto& trajectory = result->get_itinerary().back().trajectory(); @@ -302,7 +319,7 @@ Delivery::Start Delivery::dropoff_start(const Delivery::Start& start) const rmf_traffic::agv::Planner::Start dropoff_start{ finish_time, - _pimpl->_pickup_waypoint, + _pimpl->pickup_waypoint, orientation}; return dropoff_start; diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 0fd83ea8a..0a89b8137 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -178,6 +178,22 @@ rmf_utils::optional Loop::estimate_finish( initial_state.finish_time() > ideal_start ? initial_state.finish_time() : 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 (_pimpl->drain_battery && wait_until > initial_state.finish_time() && + initial_state.waypoint() != initial_state.charging_waypoint()) + { + rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); + dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_device; + + if (battery_soc <= state_config.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + // Compute finish time const rmf_traffic::Time state_finish_time = wait_until + variant_duration + _pimpl->invariant_duration; From 081306d3481411a3152c3f7581abc54b68318e58 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 9 Dec 2020 15:17:38 +0800 Subject: [PATCH 082/128] Removed commented code from Adapter.cpp --- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 60 +------------------ 1 file changed, 1 insertion(+), 59 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index 7b17041b1..4c064ca13 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -26,9 +26,6 @@ #include #include -// #include -// #include - #include "internal_TrafficLight.hpp" #include "internal_EasyTrafficLight.hpp" @@ -69,23 +66,11 @@ class Adapter::Implementation std::shared_ptr blockade_writer; rmf_traffic_ros2::schedule::MirrorManager mirror_manager; - // using Delivery = rmf_task_msgs::msg::Delivery; - // using DeliverySub = rclcpp::Subscription::SharedPtr; - // DeliverySub delivery_sub; - - // using Loop = rmf_task_msgs::msg::Loop; - // using LoopSub = rclcpp::Subscription::SharedPtr; - // LoopSub loop_sub; - std::vector> fleets = {}; // TODO(MXG): This mutex probably isn't needed std::mutex mutex; - // std::unordered_set received_tasks; - // std::map task_times; - // rclcpp::TimerBase::SharedPtr task_purge_timer; - // This mutex protects the initialization of traffic lights std::mutex traffic_light_init_mutex; @@ -102,50 +87,7 @@ class Adapter::Implementation blockade_writer{rmf_traffic_ros2::blockade::Writer::make(*node)}, mirror_manager{std::move(mirror_manager_)} { - // const auto default_qos = rclcpp::SystemDefaultsQoS(); - // delivery_sub = node->create_subscription( - // DeliveryTopicName, default_qos, - // [this](Delivery::SharedPtr msg) - // { - // std::lock_guard lock(mutex); - // if (!received_tasks.insert(msg->task_id).second) - // return; - - // task_times.insert( - // task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); - - // rmf_fleet_adapter::agv::request_delivery(*msg, fleets); - // }); - - // loop_sub = node->create_subscription( - // LoopRequestTopicName, default_qos, - // [this](Loop::SharedPtr msg) - // { - // std::lock_guard lock(mutex); - // if (!received_tasks.insert(msg->task_id).second) - // return; - - // task_times.insert( - // task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); - - // rmf_fleet_adapter::agv::request_loop(*msg, fleets); - // }); - - // task_purge_timer = node->create_wall_timer( - // std::chrono::minutes(60), [this]() - // { - // // This purge of task ids is to prevent the log of tasks from growing - // // infinitely. - // const auto purge_end = - // std::chrono::steady_clock::now() - std::chrono::minutes(60); - - // auto it = task_times.begin(); - // for (; it != task_times.end() && it->first < purge_end; ++it) - // received_tasks.erase(it->second); - - // if (it != task_times.begin()) - // task_times.erase(task_times.begin(), it); - // }); + // Do nothing } static rmf_utils::unique_impl_ptr make( From bbd626e91bce80eb6d068bb24dec32ca5cee38bf Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 11 Dec 2020 15:33:53 +0800 Subject: [PATCH 083/128] Powersystem does not require string name --- .../include/rmf_battery/agv/PowerSystem.hpp | 13 ++--------- .../src/rmf_battery/agv/PowerSystem.cpp | 22 +++--------------- .../test/unit/agv/test_PowerSystem.cpp | 23 ++++++------------- .../test/unit/agv/test_battery_drain.cpp | 18 +++++++-------- rmf_fleet_adapter/src/full_control/main.cpp | 6 ++--- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 6 ++--- 6 files changed, 26 insertions(+), 62 deletions(-) diff --git a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp index 20ad8f6aa..a1f587375 100644 --- a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp @@ -30,23 +30,14 @@ class PowerSystem /// Constructor /// - /// \param[in] name - /// A string representing the name of this power system - /// /// \param[in] nominal_power /// The rated nominal power consumption in Watts for this power system - /// - PowerSystem( - std::string name, - double nominal_power); - - PowerSystem& name(std::string name); - const std::string& name() const; + PowerSystem(double nominal_power); PowerSystem& nominal_power(double nom_power); double nominal_power() const; - /// Returns true if the values are valid, i.e. greater than zero. + /// Returns true if the nominal power is valid, i.e. greater than zero. bool valid() const; class Implementation; diff --git a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp index 6026a51f2..c1238c64c 100644 --- a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -23,33 +23,17 @@ namespace agv { class PowerSystem::Implementation { public: - std::string name; double nominal_power; }; //============================================================================== -PowerSystem::PowerSystem( - const std::string name, - const double nominal_power) +PowerSystem::PowerSystem(const double nominal_power) : _pimpl(rmf_utils::make_impl( - Implementation{name, nominal_power})) + Implementation{nominal_power})) { // Do nothing } -//============================================================================== -auto PowerSystem::name(std::string name) -> PowerSystem& -{ - _pimpl->name = name; - return *this; -} - -//============================================================================== -const std::string& PowerSystem::name() const -{ - return _pimpl->name; -} - //============================================================================== auto PowerSystem::nominal_power(double nom_power) ->PowerSystem& { @@ -66,7 +50,7 @@ double PowerSystem::nominal_power() const //============================================================================== bool PowerSystem::valid() const { - return !_pimpl->name.empty() && _pimpl->nominal_power >= 0.0; + return _pimpl->nominal_power >= 0.0; } diff --git a/rmf_battery/test/unit/agv/test_PowerSystem.cpp b/rmf_battery/test/unit/agv/test_PowerSystem.cpp index 7b2421aac..e7fc21739 100644 --- a/rmf_battery/test/unit/agv/test_PowerSystem.cpp +++ b/rmf_battery/test/unit/agv/test_PowerSystem.cpp @@ -21,29 +21,20 @@ SCENARIO("Test PowerSystem") { - rmf_battery::agv::PowerSystem power_system( - "cleaning_system", 60); - REQUIRE(power_system.name() == "cleaning_system"); - REQUIRE(power_system.nominal_power() - 60 == Approx(0.0)); + rmf_battery::agv::PowerSystem power_system(60.0); + REQUIRE(power_system.nominal_power() - 60.0 == Approx(0.0)); REQUIRE(power_system.valid()); - WHEN("Name is set") + WHEN("A valid nomial power is set") { - power_system.name("vacuuming_system"); - CHECK(power_system.name() == "vacuuming_system"); - CHECK(power_system.nominal_power() - 60 == Approx(0.0)); - CHECK(power_system.valid()); - } - WHEN("Nominal power is set") - { - power_system.nominal_power(80); - CHECK(power_system.nominal_power() - 80 == Approx(0.0)); + power_system.nominal_power(80.0); + CHECK(power_system.nominal_power() - 80.0 == Approx(0.0)); CHECK(power_system.valid()); } - WHEN("A property is negative") + WHEN("An invalid nomimal power is set") { - power_system.nominal_power(-12); + power_system.nominal_power(-12.0); CHECK_FALSE(power_system.valid()); } } \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 6c2fbdce2..bae33f142 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -45,10 +45,10 @@ SCENARIO("Test battery drain with RobotA") REQUIRE(battery_system.valid()); MechanicalSystem mechanical_system{20, 10, 0.3}; REQUIRE(mechanical_system.valid()); - PowerSystem power_system_1{"processor", 10}; - REQUIRE(power_system_1.valid()); + PowerSystem power_system_processor{10.0}; + REQUIRE(power_system_processor.valid()); SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; - SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; // Initializing vehicle traits @@ -116,10 +116,10 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") REQUIRE(battery_system.valid()); MechanicalSystem mechanical_system{70, 40, 0.22}; REQUIRE(mechanical_system.valid()); - PowerSystem power_system_1{"processor", 20}; - REQUIRE(power_system_1.valid()); + PowerSystem power_system_processor{20.0}; + REQUIRE(power_system_processor.valid()); SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; - SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( @@ -256,10 +256,10 @@ SCENARIO("Testing Cleaning Request") REQUIRE(battery_system.valid()); MechanicalSystem mechanical_system{70, 40, 0.22}; REQUIRE(mechanical_system.valid()); - PowerSystem power_system_1{"processor", 20}; - REQUIRE(power_system_1.valid()); + PowerSystem power_system_processor{20.0}; + REQUIRE(power_system_processor.valid()); SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; - SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 5596792a0..574836ef1 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -523,8 +523,7 @@ std::shared_ptr make_fleet( const double ambient_power_drain = rmf_fleet_adapter::get_parameter_or_default( *node, "ambient_power_drain", 20.0); - rmf_battery::agv::PowerSystem ambient_power_system{ - "ambient", ambient_power_drain}; + rmf_battery::agv::PowerSystem ambient_power_system{ambient_power_drain}; if (!ambient_power_system.valid()) { RCLCPP_ERROR( @@ -540,8 +539,7 @@ std::shared_ptr make_fleet( // Tool power system const double tool_power_drain = rmf_fleet_adapter::get_parameter_or_default( *node, "tool_power_drain", 10.0); - rmf_battery::agv::PowerSystem tool_power_system{ - "ambient", tool_power_drain}; + rmf_battery::agv::PowerSystem tool_power_system{tool_power_drain}; if (!tool_power_system.valid()) { RCLCPP_ERROR( diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index e85b06375..bcd81833e 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -156,13 +156,13 @@ SCENARIO("Grid World") REQUIRE(battery_system.valid()); rmf_battery::agv::MechanicalSystem mechanical_system{70.0, 40.0, 0.22}; REQUIRE(mechanical_system.valid()); - rmf_battery::agv::PowerSystem power_system{"processor", 20.0}; - REQUIRE(power_system.valid()); + rmf_battery::agv::PowerSystem power_system_processor{20.0}; + REQUIRE(power_system_processor.valid()); std::shared_ptr motion_sink = std::make_shared(battery_system, mechanical_system); std::shared_ptr device_sink = - std::make_shared(battery_system, power_system); + std::make_shared(battery_system, power_system_processor); WHEN("Planning for 3 requests and 2 agents") { From d416f83a321ccf04a08fd1711248194babee5fbc Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 11 Dec 2020 15:54:48 +0800 Subject: [PATCH 084/128] Added documentation for MechanicalSystem --- .../include/rmf_battery/agv/MechanicalSystem.hpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp index e209e2293..0ebc2e23b 100644 --- a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp @@ -27,7 +27,18 @@ namespace agv { class MechanicalSystem { public: - + /// Constructor + /// + /// \param[in] mass + /// The mass of the robot in kilograms + /// + /// \param[in] inertia + /// The moment of inertia of the robot along its yaw axis + /// + /// \param[in] friction_coefficient + /// The coefficient of kinetic friction measured at the wheels of the robot. + /// This value is used to compute the energy loss due to rotation of the + /// vehicle's wheels during locomotion. MechanicalSystem( double mass, double inertia, From eb4ac7312e01824cf9381783455af4f128b065dd Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 14 Dec 2020 15:31:39 +0800 Subject: [PATCH 085/128] Removed BatteryProfile. Switched to make() pattern in rmf_battery --- rmf_battery/CMakeLists.txt | 4 +- .../include/rmf_battery/agv/BatterySystem.hpp | 114 +------- .../rmf_battery/agv/MechanicalSystem.hpp | 25 +- .../include/rmf_battery/agv/PowerSystem.hpp | 15 +- .../src/rmf_battery/agv/BatterySystem.cpp | 251 ++---------------- .../src/rmf_battery/agv/MechanicalSystem.cpp | 54 ++-- .../src/rmf_battery/agv/PowerSystem.cpp | 26 +- .../test/unit/agv/test_BatterySystem.cpp | 19 +- .../test/unit/agv/test_MechanicalSystem.cpp | 51 ++-- .../test/unit/agv/test_PowerSystem.cpp | 20 +- .../test/unit/agv/test_battery_drain.cpp | 68 ++--- rmf_fleet_adapter/src/full_control/main.cpp | 29 +- .../src/rmf_fleet_adapter/load_param.cpp | 13 +- .../src/rmf_fleet_adapter/load_param.hpp | 5 +- .../src/rmf_task/requests/ChargeBattery.cpp | 13 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 18 +- 16 files changed, 206 insertions(+), 519 deletions(-) diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 09f650ade..d6e3b3c46 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(rmf_battery) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp b/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp index c0d16adf3..c837feb30 100644 --- a/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/BatterySystem.hpp @@ -20,7 +20,8 @@ #define RMF_BATTERY__AGV__BATTERYSYSTEM_HPP #include -#include + +#include namespace rmf_battery { namespace agv { @@ -28,121 +29,34 @@ namespace agv { class BatterySystem { public: - enum class BatteryType : uint16_t - { - /// The vehicle is powered by a Lead-Acid battery - LeadAcid, - /// The vehicle is powered by a Lithium-Ion battery - LiIon, - /// The vehicle is powered by a Nickel-Metal-Hydride battery - NiMH - }; - - class BatteryProfile - { - public: - /// The parameters required to construct a generic battery model to map - /// state of charge of the battery to its voltage. - /// - /// \param[in] resistance - /// The internal resistance of the battery in ohms - /// - /// \param[in] max_voltage - /// The maximum voltage in Volts of the battery - /// - /// \param[in] exp_voltage - /// The voltage of the battery in Volts at the end of the exponential - /// zone in its discharge profile - /// - /// \param[in] exp_capacity - /// The capacity of the battery in Ah at the end of the exponential zone - /// in its discharge profile - /// - /// \param[in] nominal_capacity - /// The capacity of the battery in Ah at the end of the exponential zone - /// in its discharge profile - BatteryProfile( - double resistance, - double max_voltage, - double exp_voltage, - double exp_capacity, - double nominal_capacity, - double discharge_current); - - BatteryProfile& resistance(double resistance); - double resistance() const; - - BatteryProfile& max_voltage(double max_voltage); - double max_voltage() const; - - BatteryProfile& exp_voltage(double exp_voltage); - double exp_voltage() const; - - BatteryProfile& exp_capacity(double exp_capacity); - double exp_capacity() const; - - BatteryProfile& nominal_capacity(double nominal_capacity); - double nominal_capacity() const; - - BatteryProfile& discharge_current(double discharge_current); - double discharge_current() const; - - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - /// Constructor - /// + /// Returns a BatterySystem object if valid values were supplied for the + /// various fields else returns std::nullopt. Here valid implies that the + /// values are greater than zero. /// \param[in] nominal_voltage - /// The nominal voltage of the battery in volts + /// The nominal voltage of the battery in Volts /// /// \param[in] capacity - /// The nominal capacity of the battery in Ah + /// The nominal capacity of the battery in Ampere-hours /// /// \param[in] charging_current - /// The rated current in A for charging the battery - /// - /// \param[in] type - /// The chemisty type of the battery - /// - /// \param[in] profile - /// The battery profile for this battery. This is only required for calling - /// the get_voltage() method - BatterySystem( + /// The rated current in Amperes for charging the battery + static std::optional make( double nominal_voltage, double capacity, - double charging_current, - BatteryType type = BatteryType::LeadAcid, - rmf_utils::optional profile = rmf_utils::nullopt); + double charging_current); - /// Estimate the voltage of the battery given its state of charge. This function - /// will return a nullopt if the battery profile is not defined - rmf_utils::optional estimate_voltage(const double soc) const; - - BatterySystem& nominal_voltage(double nominal_voltage); + /// Get the nominal voltage of this battery system double nominal_voltage() const; - BatterySystem& capacity(double nominal_capacity); + /// Get the capacity of this battery system double capacity() const; - BatterySystem& charging_current(double charging_current); + /// Get the charging current of this battery system double charging_current() const; - BatterySystem& type(BatteryType type); - BatteryType type() const; - - BatterySystem& profile(rmf_utils::optional profile); - rmf_utils::optional profile() const; - - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - class Implementation; private: + BatterySystem(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp index 0ebc2e23b..889b9c097 100644 --- a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp @@ -20,44 +20,45 @@ #include +#include + namespace rmf_battery { namespace agv { - class MechanicalSystem { public: - /// Constructor - /// + + /// Returns a MechanicalSystem object if valid values were supplied for the + /// various fields else returns std::nullopt. Here valid implies that the + /// values are greater than zero. /// \param[in] mass - /// The mass of the robot in kilograms + /// The mass of the robot in Kilograms(kg) /// /// \param[in] inertia - /// The moment of inertia of the robot along its yaw axis + /// The moment of inertia of the robot along its yaw axis in kg.m^2 /// /// \param[in] friction_coefficient /// The coefficient of kinetic friction measured at the wheels of the robot. /// This value is used to compute the energy loss due to rotation of the /// vehicle's wheels during locomotion. - MechanicalSystem( + static std::optional make( double mass, double inertia, double friction_coefficient); - MechanicalSystem& mass(double mass); + /// Get the mass of this mechanical system double mass() const; - MechanicalSystem& inertia(double inertia); + /// Get the moment of inertia of this mechanical system double inertia() const; - MechanicalSystem& friction_coefficient(double friction_coeff); + /// Get the friction coefficient of this mechanical system double friction_coefficient() const; - /// Returns true if the values are valid, i.e. greater than zero. - bool valid() const; - class Implementation; private: + MechanicalSystem(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp index a1f587375..994db4cf5 100644 --- a/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/PowerSystem.hpp @@ -20,6 +20,8 @@ #include +#include + namespace rmf_battery { namespace agv { @@ -28,20 +30,19 @@ class PowerSystem { public: - /// Constructor - /// + /// Returns a PowerSystem object if valid values were supplied for the + /// various fields else returns std::nullopt. Here valid implies that the + /// values are greater than zero. /// \param[in] nominal_power /// The rated nominal power consumption in Watts for this power system - PowerSystem(double nominal_power); + static std::optional make(double nominal_power); - PowerSystem& nominal_power(double nom_power); + /// Get the nominal power of this power system double nominal_power() const; - /// Returns true if the nominal power is valid, i.e. greater than zero. - bool valid() const; - class Implementation; private: + PowerSystem(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp index ba1523abf..da4a5ed15 100644 --- a/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp @@ -17,133 +17,9 @@ #include -#include - namespace rmf_battery { namespace agv { -using BatteryProfile = BatterySystem::BatteryProfile; -using BatteryType = BatterySystem::BatteryType; - -class BatterySystem::BatteryProfile::Implementation -{ -public: - double resistance; - double max_voltage; - double exp_voltage; - double exp_capacity; - double nominal_capacity; - double discharge_current; -}; - -//============================================================================== -BatterySystem::BatteryProfile::BatteryProfile( - const double resistance, - const double max_voltage, - const double exp_voltage, - const double exp_capacity, - const double nominal_capacity, - const double discharge_current) -: _pimpl(rmf_utils::make_impl( - Implementation - { - resistance, - max_voltage, - exp_voltage, - exp_capacity, - nominal_capacity, - discharge_current - })) -{ - // Do nothing -} - -//============================================================================== -auto BatteryProfile::resistance(double resistance) -> BatteryProfile& -{ - _pimpl->resistance = resistance; - return *this; -} - -//============================================================================== -double BatteryProfile::resistance() const -{ - return _pimpl->resistance; -} - -//============================================================================== -auto BatteryProfile::max_voltage(double max_voltage) -> BatteryProfile& -{ - _pimpl->max_voltage = max_voltage; - return *this; -} - -//============================================================================== -double BatteryProfile::max_voltage() const -{ - return _pimpl->max_voltage; -} - -//============================================================================== -auto BatteryProfile::exp_voltage(double exp_voltage) -> BatteryProfile& -{ - _pimpl->exp_voltage = exp_voltage; - return *this; -} - -//============================================================================== -double BatteryProfile::exp_voltage() const -{ - return _pimpl->exp_voltage; -} - -//============================================================================== -auto BatteryProfile::exp_capacity(double exp_capacity) -> BatteryProfile& -{ - _pimpl->exp_capacity = exp_capacity; - return *this; -} - -//============================================================================== -double BatteryProfile::nominal_capacity() const -{ - return _pimpl->nominal_capacity; -} - -//============================================================================== -auto BatteryProfile::nominal_capacity(double nominal_capacity) -> BatteryProfile& -{ - _pimpl->nominal_capacity = nominal_capacity; - return *this; -} - -//============================================================================== -double BatteryProfile::exp_capacity() const -{ - return _pimpl->exp_capacity; -} - -//============================================================================== -auto BatteryProfile::discharge_current(double discharge_current) -> BatteryProfile& -{ - _pimpl->discharge_current = discharge_current; - return *this; -} - -//============================================================================== -double BatteryProfile::discharge_current() const -{ - return _pimpl->discharge_current; -} - -//============================================================================== -bool BatteryProfile::valid() const -{ - return _pimpl->resistance > 0.0 && _pimpl->max_voltage > 0.0 && - _pimpl->exp_voltage > 0.0 && _pimpl->exp_capacity > 0.0 && - _pimpl->nominal_capacity > 0.0 && _pimpl->discharge_current > 0.0; -} - //============================================================================== class BatterySystem::Implementation { @@ -151,73 +27,34 @@ class BatterySystem::Implementation double nominal_voltage; double capacity; double charging_current; - BatteryType type; - rmf_utils::optional profile; - - // Battery parameters used in get_voltage() to derive voltage of the battery - // given its state of charge. - // Ref: O. Tremblay, L. Dessaint and A. Dekkiche, "A Generic Battery Model for - // the Dynamic Simulation of Hybrid Electric Vehicles," 2007 IEEE Vehicle - // Power and Propulsion Conference, Arlington, TX, 2007, pp. 284-289, - // doi: 10.1109/VPPC.2007.4544139. - struct ProfileParams - { - double a; // V - double b; // (Ah)^-1 - double k; // V - double e0;// V - }; - - ProfileParams params = {0.0, 0.0, 0.0, 0.0}; }; //============================================================================== -BatterySystem::BatterySystem( - const double nominal_voltage, - const double capacity, - const double charging_current, - BatteryType type, - rmf_utils::optional profile) -: _pimpl(rmf_utils::make_impl( - Implementation{ - nominal_voltage, - capacity, - charging_current, - type, - std::move(profile) - })) +std::optional BatterySystem::make( + double nominal_voltage, + double capacity, + double charging_current) { - if (_pimpl->profile.has_value()) + if (nominal_voltage <= 0.0 || + capacity <= 0.0 || + charging_current <= 0.0) { - assert(profile->valid()); - _pimpl->params.a = profile->max_voltage() - profile->exp_voltage(); - _pimpl->params.b = 3.0 / profile->exp_capacity(); - _pimpl->params.k = (profile->max_voltage() - _pimpl->nominal_voltage + - _pimpl->params.a*( - exp(-_pimpl->params.b*_pimpl->profile->nominal_capacity()) - 1)*( - _pimpl->capacity - _pimpl->profile->nominal_capacity())) / - _pimpl->profile->nominal_capacity(); - _pimpl->params.e0 = _pimpl->profile->max_voltage() + _pimpl->params.k + - _pimpl->profile->resistance() * _pimpl->profile->discharge_current() - - _pimpl->params.a; + return std::nullopt; } -} -rmf_utils::optional BatterySystem::estimate_voltage(const double soc) const -{ - assert(soc > 0.0 && soc <= 1.0); - if (!_pimpl->profile.has_value() || !_pimpl->profile->valid()) - return rmf_utils::nullopt; + BatterySystem battery_system; + battery_system._pimpl->nominal_voltage = nominal_voltage; + battery_system._pimpl->capacity = capacity; + battery_system._pimpl->charging_current = charging_current; - return _pimpl->params.e0 - _pimpl->params.k*(1/soc) + _pimpl->params.a * - exp(-_pimpl->params.b * _pimpl->capacity*(1 - soc)); + return battery_system; } //============================================================================== -auto BatterySystem::nominal_voltage(double nom_voltage) -> BatterySystem& +BatterySystem::BatterySystem() +: _pimpl(rmf_utils::make_impl(Implementation())) { - _pimpl->nominal_voltage = nom_voltage; - return *this; + // Do nothing } //============================================================================== @@ -226,73 +63,17 @@ double BatterySystem::nominal_voltage() const return _pimpl->nominal_voltage; } -//============================================================================== -auto BatterySystem::capacity(double nom_capacity) -> BatterySystem& -{ - _pimpl->capacity = nom_capacity; - return *this; -} - //============================================================================== double BatterySystem::capacity() const { return _pimpl->capacity; } -//============================================================================== -auto BatterySystem::charging_current(double charging_current) -> BatterySystem& -{ - _pimpl->charging_current = charging_current; - return *this; -} - //============================================================================== double BatterySystem::charging_current() const { return _pimpl->charging_current; } -//============================================================================== -auto BatterySystem::type(BatteryType type) --> BatterySystem& -{ - _pimpl->type = type; - return *this; -} - -//============================================================================== -BatteryType BatterySystem::type() const -{ - return _pimpl->type; -} - -//============================================================================== -auto BatterySystem::profile( - rmf_utils::optional profile) -> BatterySystem& -{ - _pimpl->profile = std::move(profile); - return *this; -} - -//============================================================================== -rmf_utils::optional BatterySystem::profile() const -{ - return _pimpl->profile; -} - -//============================================================================== -bool BatterySystem::valid() const -{ - bool valid = _pimpl->nominal_voltage > 0.0 && - _pimpl->capacity > 0.0 && _pimpl->charging_current > 0.0 && - (_pimpl->type == BatteryType::LeadAcid - || _pimpl->type == BatteryType::LiIon); - if (_pimpl->profile) - return valid && _pimpl->profile->valid(); - - return valid; - -} - } // namespace agv } // namespace rmf_battery \ No newline at end of file diff --git a/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp index 872a718ac..8f60a2730 100644 --- a/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp @@ -30,25 +30,26 @@ class MechanicalSystem::Implementation }; //============================================================================== -MechanicalSystem::MechanicalSystem( - const double mass, - const double inertia, - const double friction_coefficient) -: _pimpl(rmf_utils::make_impl( - Implementation{ - mass, - inertia, - friction_coefficient, - })) +std::optional MechanicalSystem::make( + double mass, + double inertia, + double friction_coefficient) { - // Do nothing -} + if (mass <= 0.0 || inertia <= 0.0 || friction_coefficient <= 0.0) + return std::nullopt; + + MechanicalSystem mechanical_system; + mechanical_system._pimpl->mass = mass; + mechanical_system._pimpl->inertia = inertia; + mechanical_system._pimpl->friction_coefficient = friction_coefficient; + return mechanical_system; +} //============================================================================== -auto MechanicalSystem::mass(double mass) -> MechanicalSystem& +MechanicalSystem::MechanicalSystem() +: _pimpl(rmf_utils::make_impl(Implementation())) { - _pimpl->mass = mass; - return *this; + // Do nothing } //============================================================================== @@ -57,40 +58,17 @@ double MechanicalSystem::mass() const return _pimpl->mass; } -//============================================================================== -auto MechanicalSystem::friction_coefficient(double friction_coeff) --> MechanicalSystem& -{ - _pimpl->friction_coefficient = friction_coeff; - return *this; -} - //============================================================================== double MechanicalSystem::friction_coefficient() const { return _pimpl->friction_coefficient; } -//============================================================================== -auto MechanicalSystem::inertia(double inertia) --> MechanicalSystem& -{ - _pimpl->inertia = inertia; - return *this; -} - //============================================================================== double MechanicalSystem::inertia() const { return _pimpl->inertia; } -//============================================================================== -bool MechanicalSystem::valid() const -{ - return _pimpl->mass > 0.0 && _pimpl->friction_coefficient > 0.0 && - _pimpl->inertia > 0.0; -} - } // namespace agv } // namespace rmf_battery diff --git a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp index c1238c64c..a3eb81139 100644 --- a/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -20,6 +20,7 @@ namespace rmf_battery { namespace agv { +//============================================================================== class PowerSystem::Implementation { public: @@ -27,18 +28,22 @@ class PowerSystem::Implementation }; //============================================================================== -PowerSystem::PowerSystem(const double nominal_power) -: _pimpl(rmf_utils::make_impl( - Implementation{nominal_power})) +std::optional PowerSystem::make(double nominal_power) { - // Do nothing + if (nominal_power < 0.0) + return std::nullopt; + + PowerSystem power_system; + power_system._pimpl->nominal_power = nominal_power; + + return power_system; } //============================================================================== -auto PowerSystem::nominal_power(double nom_power) ->PowerSystem& +PowerSystem::PowerSystem() +: _pimpl(rmf_utils::make_impl(Implementation())) { - _pimpl->nominal_power = nom_power; - return *this; + // Do nothing } //============================================================================== @@ -47,12 +52,5 @@ double PowerSystem::nominal_power() const return _pimpl->nominal_power; } -//============================================================================== -bool PowerSystem::valid() const -{ - return _pimpl->nominal_power >= 0.0; -} - - } // namespace agv } // namespace rmf_battery diff --git a/rmf_battery/test/unit/agv/test_BatterySystem.cpp b/rmf_battery/test/unit/agv/test_BatterySystem.cpp index edaa6c2f9..fdef8eba0 100644 --- a/rmf_battery/test/unit/agv/test_BatterySystem.cpp +++ b/rmf_battery/test/unit/agv/test_BatterySystem.cpp @@ -19,9 +19,22 @@ #include - - SCENARIO("Test BatterySystem") { - // TODO + using BatterySystem = rmf_battery::agv::BatterySystem; + + WHEN("Valid values are supplied to make()") + { + auto battery_system = BatterySystem::make(12.0, 20.0, 6.0); + REQUIRE(battery_system); + CHECK(battery_system->nominal_voltage() == 12.0); + CHECK(battery_system->capacity() == 20.0); + CHECK(battery_system->charging_current() == 6.0); + } + + WHEN("In-valid values are supplied to make()") + { + auto battery_system = BatterySystem::make(-12.0, 20.0, 6.0); + CHECK_FALSE(battery_system); + } } diff --git a/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp b/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp index 12719aef5..ef7be26d0 100644 --- a/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp +++ b/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp @@ -19,47 +19,40 @@ #include - - SCENARIO("Test MechanicalSystem") { using MechanicalSystem = rmf_battery::agv::MechanicalSystem; - MechanicalSystem mechanical_system{70.0, 30.0, 0.3}; - REQUIRE(mechanical_system.valid()); - CHECK(mechanical_system.mass() - 70.0 == Approx(0.0)); - CHECK(mechanical_system.inertia() - 30.0 == Approx(0.0)); - CHECK(mechanical_system.friction_coefficient() - 0.3 == Approx(0.0)); + + WHEN("Valid values are supplied to make()") + { + auto mechanical_system = MechanicalSystem::make(10.0, 20.0, 0.3); + REQUIRE(mechanical_system); + CHECK(mechanical_system->mass() == 10.0); + CHECK(mechanical_system->inertia() == 20.0); + CHECK(mechanical_system->friction_coefficient() == 0.3); + } - WHEN("Mass is set") + WHEN("In-valid mass is supplied to make()") { - mechanical_system.mass(75.0); - CHECK(mechanical_system.valid()); - CHECK(mechanical_system.mass() - 75.0 == Approx(0.0)); - CHECK(mechanical_system.inertia() - 30.0 == Approx(0.0)); - CHECK(mechanical_system.friction_coefficient() - 0.3 == Approx(0.0)); + auto mechanical_system = MechanicalSystem::make(-10.0, 20.0, 0.3); + CHECK_FALSE(mechanical_system); } - WHEN("Inertia is set") + WHEN("In-valid inertia is supplied to make()") { - mechanical_system.inertia(35.0); - CHECK(mechanical_system.valid()); - CHECK(mechanical_system.mass() - 70.0 == Approx(0.0)); - CHECK(mechanical_system.inertia() - 35.0 == Approx(0.0)); - CHECK(mechanical_system.friction_coefficient() - 0.3 == Approx(0.0)); + auto mechanical_system = MechanicalSystem::make(10.0, -20.0, 0.3); + CHECK_FALSE(mechanical_system); } - - WHEN("Friction coefficient is set") + + WHEN("In-valid friction coefficient is supplied to make()") { - mechanical_system.friction_coefficient(0.4); - CHECK(mechanical_system.valid()); - CHECK(mechanical_system.mass() - 70.0 == Approx(0.0)); - CHECK(mechanical_system.inertia() - 30.0 == Approx(0.0)); - CHECK(mechanical_system.friction_coefficient() - 0.4 == Approx(0.0)); + auto mechanical_system = MechanicalSystem::make(10.0, 20.0, -0.3); + CHECK_FALSE(mechanical_system); } - WHEN("A negative value is set") + WHEN("In-valid values are supplied to make()") { - mechanical_system.mass(-10.0); - CHECK_FALSE(mechanical_system.valid()); + auto mechanical_system = MechanicalSystem::make(-10.0, -20.0, -0.3); + CHECK_FALSE(mechanical_system); } } diff --git a/rmf_battery/test/unit/agv/test_PowerSystem.cpp b/rmf_battery/test/unit/agv/test_PowerSystem.cpp index e7fc21739..e8f1e8ad2 100644 --- a/rmf_battery/test/unit/agv/test_PowerSystem.cpp +++ b/rmf_battery/test/unit/agv/test_PowerSystem.cpp @@ -21,20 +21,18 @@ SCENARIO("Test PowerSystem") { - rmf_battery::agv::PowerSystem power_system(60.0); - REQUIRE(power_system.nominal_power() - 60.0 == Approx(0.0)); - REQUIRE(power_system.valid()); - - WHEN("A valid nomial power is set") + using PowerSystem = rmf_battery::agv::PowerSystem; + + WHEN("Valid nominal power is supplied to make()") { - power_system.nominal_power(80.0); - CHECK(power_system.nominal_power() - 80.0 == Approx(0.0)); - CHECK(power_system.valid()); + auto power_system = PowerSystem::make(60.0); + REQUIRE(power_system); + CHECK(power_system->nominal_power() == 60.0); } - WHEN("An invalid nomimal power is set") + WHEN("In-valid nominal power is supplied to make()") { - power_system.nominal_power(-12.0); - CHECK_FALSE(power_system.valid()); + auto power_system = PowerSystem::make(0.0); + CHECK_FALSE(power_system); } } \ No newline at end of file diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index bae33f142..2a5ca8fd4 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -29,8 +29,6 @@ #include -#include - SCENARIO("Test battery drain with RobotA") { using BatterySystem = rmf_battery::agv::BatterySystem; @@ -41,12 +39,18 @@ SCENARIO("Test battery drain with RobotA") using namespace std::chrono_literals; // Initializing system traits - BatterySystem battery_system{12, 24, 2}; - REQUIRE(battery_system.valid()); - MechanicalSystem mechanical_system{20, 10, 0.3}; - REQUIRE(mechanical_system.valid()); - PowerSystem power_system_processor{10.0}; - REQUIRE(power_system_processor.valid()); + auto battery_system_optional = BatterySystem::make(12.0, 24.0, 2.0); + REQUIRE(battery_system_optional); + BatterySystem& battery_system = *battery_system_optional; + + auto mechanical_system_optional = MechanicalSystem::make(20.0, 10.0, 0.3); + REQUIRE(mechanical_system_optional); + MechanicalSystem& mechanical_system = *mechanical_system_optional; + + auto power_system_optional = PowerSystem::make(10.0); + REQUIRE(power_system_optional); + PowerSystem& power_system_processor = *power_system_optional; + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; @@ -73,8 +77,6 @@ SCENARIO("Test battery drain with RobotA") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } @@ -95,8 +97,6 @@ SCENARIO("Test battery drain with RobotA") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1 && remaining_soc < 0.05; CHECK(ok); } @@ -112,12 +112,18 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") using namespace std::chrono_literals; // Initializing system traits - BatterySystem battery_system{24, 40, 2}; - REQUIRE(battery_system.valid()); - MechanicalSystem mechanical_system{70, 40, 0.22}; - REQUIRE(mechanical_system.valid()); - PowerSystem power_system_processor{20.0}; - REQUIRE(power_system_processor.valid()); + 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; + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; @@ -143,8 +149,6 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; CHECK(ok); } @@ -165,8 +169,6 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; CHECK(ok); } @@ -235,8 +237,6 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - // std::cout << "Remaining soc: " << remaining_soc << std::endl; const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } @@ -252,12 +252,18 @@ SCENARIO("Testing Cleaning Request") using namespace std::chrono_literals; // Initializing system traits - BatterySystem battery_system{24, 40, 8.8}; - REQUIRE(battery_system.valid()); - MechanicalSystem mechanical_system{70, 40, 0.22}; - REQUIRE(mechanical_system.valid()); - PowerSystem power_system_processor{20.0}; - REQUIRE(power_system_processor.valid()); + 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; + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; @@ -288,8 +294,6 @@ SCENARIO("Testing Cleaning Request") const double dSOC_device = device_power_sink.compute_change_in_charge( rmf_traffic::time::to_seconds(trajectory.duration())); - // std::cout << "Motion: " << dSOC_motion << "Device: " << dSOC_device << std::endl; - const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; REQUIRE(remaining_soc <= 1.0); } diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 574836ef1..ef871cc53 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -493,9 +493,9 @@ std::shared_ptr make_fleet( // Parameters required for task planner // Battery system - auto battery_system = std::make_shared( - rmf_fleet_adapter::get_battery_system(*node, 24.0, 40.0, 8.8)); - if (!battery_system->valid()) + auto battery_system_optional = rmf_fleet_adapter::get_battery_system( + *node, 24.0, 40.0, 8.8); + if (!battery_system_optional) { RCLCPP_ERROR( node->get_logger(), @@ -503,11 +503,13 @@ std::shared_ptr make_fleet( return nullptr; } + auto battery_system = std::make_shared( + *battery_system_optional); // Mechanical system and motion_sink - auto mechanical_system = rmf_fleet_adapter::get_mechanical_system( + auto mechanical_system_optional = rmf_fleet_adapter::get_mechanical_system( *node, 70.0, 40.0, 0.22); - if (!mechanical_system.valid()) + if (!mechanical_system_optional) { RCLCPP_ERROR( node->get_logger(), @@ -515,6 +517,9 @@ std::shared_ptr make_fleet( return nullptr; } + rmf_battery::agv::MechanicalSystem& mechanical_system = + *mechanical_system_optional; + std::shared_ptr motion_sink = std::make_shared( *battery_system, mechanical_system); @@ -523,8 +528,9 @@ std::shared_ptr make_fleet( const double ambient_power_drain = rmf_fleet_adapter::get_parameter_or_default( *node, "ambient_power_drain", 20.0); - rmf_battery::agv::PowerSystem ambient_power_system{ambient_power_drain}; - if (!ambient_power_system.valid()) + auto ambient_power_system = rmf_battery::agv::PowerSystem::make( + ambient_power_drain); + if (!ambient_power_system) { RCLCPP_ERROR( node->get_logger(), @@ -534,13 +540,14 @@ std::shared_ptr make_fleet( } std::shared_ptr ambient_sink = std::make_shared( - *battery_system, ambient_power_system); + *battery_system, *ambient_power_system); // Tool power system const double tool_power_drain = rmf_fleet_adapter::get_parameter_or_default( *node, "tool_power_drain", 10.0); - rmf_battery::agv::PowerSystem tool_power_system{tool_power_drain}; - if (!tool_power_system.valid()) + auto tool_power_system = rmf_battery::agv::PowerSystem::make( + tool_power_drain); + if (!tool_power_system) { RCLCPP_ERROR( node->get_logger(), @@ -550,7 +557,7 @@ std::shared_ptr make_fleet( } std::shared_ptr tool_sink = std::make_shared( - *battery_system, tool_power_system); + *battery_system, *tool_power_system); // Drain battery const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp index 16caa24aa..de90d26fe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp @@ -90,7 +90,7 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default(rclcpp::Node& node, //============================================================================== -rmf_battery::agv::BatterySystem get_battery_system( +std::optional get_battery_system( rclcpp::Node& node, const double default_voltage, const double default_capacity, @@ -104,13 +104,14 @@ rmf_battery::agv::BatterySystem get_battery_system( get_parameter_or_default( node, "battery_charging_current", default_charging_current); - rmf_battery::agv::BatterySystem battery_system{ - voltage, capacity, charging_current}; + auto battery_system = rmf_battery::agv::BatterySystem::make( + voltage, capacity, charging_current); return battery_system; } -rmf_battery::agv::MechanicalSystem get_mechanical_system( +//============================================================================== +std::optional get_mechanical_system( rclcpp::Node& node, const double default_mass, const double default_inertia, @@ -123,8 +124,8 @@ rmf_battery::agv::MechanicalSystem get_mechanical_system( const double friction = get_parameter_or_default(node, "friction_coefficient", default_friction); - rmf_battery::agv::MechanicalSystem mechanical_system{ - mass, inertia, friction}; + auto mechanical_system = rmf_battery::agv::MechanicalSystem::make( + mass, inertia, friction); return mechanical_system; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp index dafda60ff..f02b9b0f9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp @@ -27,6 +27,7 @@ #include #include +#include namespace rmf_fleet_adapter { @@ -64,13 +65,13 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default( const double default_r_f, const double default_r_v); //============================================================================== -rmf_battery::agv::BatterySystem get_battery_system( +std::optional get_battery_system( rclcpp::Node& node, const double default_voltage, const double default_capacity, const double default_charging_current); -rmf_battery::agv::MechanicalSystem get_mechanical_system( +std::optional get_mechanical_system( rclcpp::Node& node, const double default_mass, const double default_inertia, diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ad8beaca9..984767830 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -50,10 +50,6 @@ class ChargeBattery::Implementation { public: - Implementation() - {} - - // fixed id for now std::string id = "Charge"; rmf_battery::agv::BatterySystemPtr battery_system; std::shared_ptr motion_sink; @@ -78,13 +74,8 @@ rmf_task::ConstRequestPtr ChargeBattery::make( { std::shared_ptr charge_battery(new ChargeBattery()); charge_battery->_pimpl->id += generate_uuid(); - charge_battery->_pimpl->battery_system = - rmf_battery::agv::BatterySystemPtr(new rmf_battery::agv::BatterySystem( - battery_system.nominal_voltage(), - battery_system.capacity(), - battery_system.charging_current(), - battery_system.type(), - battery_system.profile())); + charge_battery->_pimpl->battery_system = std::make_shared< + rmf_battery::agv::BatterySystem>(battery_system); charge_battery->_pimpl->motion_sink = std::move(motion_sink); charge_battery->_pimpl->device_sink = std::move(device_sink); charge_battery->_pimpl->planner = std::move(planner); diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index bcd81833e..4a329ba1a 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -108,6 +108,9 @@ SCENARIO("Grid World") const double edge_length = 1000; const 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; @@ -152,12 +155,15 @@ SCENARIO("Grid World") rmf_traffic::agv::Planner::Configuration{graph, traits}, default_options); - rmf_battery::agv::BatterySystem battery_system{24.0, 40.0, 8.8}; - REQUIRE(battery_system.valid()); - rmf_battery::agv::MechanicalSystem mechanical_system{70.0, 40.0, 0.22}; - REQUIRE(mechanical_system.valid()); - rmf_battery::agv::PowerSystem power_system_processor{20.0}; - REQUIRE(power_system_processor.valid()); + 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); From 1b9af999e334db8fc9f67d4dd27cbd28c42276da Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 14 Dec 2020 15:40:53 +0800 Subject: [PATCH 086/128] Addressed suggestions for rmf_battery --- .../src/rmf_battery/agv/SimpleDevicePowerSink.cpp | 5 ----- .../src/rmf_battery/agv/SimpleMotionPowerSink.cpp | 9 +++------ rmf_battery/test/unit/agv/test_PowerSystem.cpp | 2 +- 3 files changed, 4 insertions(+), 12 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp index 846f1a59b..1928d8c33 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp @@ -19,8 +19,6 @@ #include -#include - namespace rmf_battery { namespace agv { @@ -58,9 +56,6 @@ const PowerSystem& SimpleDevicePowerSink::power_system() const double SimpleDevicePowerSink::compute_change_in_charge( const double run_time) const { - assert(_pimpl->battery_system.valid()); - assert(_pimpl->power_system.valid()); - const double capacity = _pimpl->battery_system.capacity(); const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); const double nominal_power = _pimpl->power_system.nominal_power(); diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 1aa8d239c..4caf9bdee 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -23,8 +23,6 @@ #include #include -#include - namespace rmf_battery { namespace agv { @@ -76,8 +74,8 @@ double compute_friction_energy( double SimpleMotionPowerSink::compute_change_in_charge( const rmf_traffic::Trajectory& trajectory) const { - assert(_pimpl->battery_system.valid()); - assert(_pimpl->mechanical_system.valid()); + if (trajectory.size() < 2) + return 0.0; const double capacity = _pimpl->battery_system.capacity(); const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); @@ -86,10 +84,9 @@ double SimpleMotionPowerSink::compute_change_in_charge( const double friction = _pimpl->mechanical_system.friction_coefficient(); auto begin_it = trajectory.begin(); - auto end_it = --trajectory.end(); auto start_time = begin_it->time(); - const auto end_time = end_it->time(); + const auto end_time = *trajectory.finish_time(); const auto motion = rmf_traffic::Motion::compute_cubic_splines( begin_it, trajectory.end()); diff --git a/rmf_battery/test/unit/agv/test_PowerSystem.cpp b/rmf_battery/test/unit/agv/test_PowerSystem.cpp index e8f1e8ad2..b75adcaf7 100644 --- a/rmf_battery/test/unit/agv/test_PowerSystem.cpp +++ b/rmf_battery/test/unit/agv/test_PowerSystem.cpp @@ -32,7 +32,7 @@ SCENARIO("Test PowerSystem") WHEN("In-valid nominal power is supplied to make()") { - auto power_system = PowerSystem::make(0.0); + auto power_system = PowerSystem::make(-10.0); CHECK_FALSE(power_system); } } \ No newline at end of file From 91d662409c24fbbe1aabe9c65a351df1bda21ec1 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 14 Dec 2020 16:24:04 +0800 Subject: [PATCH 087/128] Added account_for_batery_drain() --- .../agv/FleetUpdateHandle.hpp | 22 +++++++++---------- rmf_fleet_adapter/src/full_control/main.cpp | 3 ++- .../agv/FleetUpdateHandle.cpp | 10 +++++---- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index a5e137e96..36710112d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -82,19 +82,21 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// \param[in] tool_sink /// Specify the device sink for special tools used by the vehicles in this fleet. /// - /// \param[in] drain_battery - /// If false, battery drain will not be considered when planning for tasks. - /// As a consequence, charging tasks will not be automatically assigned to - /// vehicles in this fleet when battery levels fall below their thresholds. - /// /// \return true if task planner parameters were successfully updated. bool set_task_planner_params( std::shared_ptr battery_system, std::shared_ptr motion_sink, std::shared_ptr ambient_sink, - std::shared_ptr tool_sink, - const bool drain_battery); + std::shared_ptr tool_sink); + /// Specify whether battery drain is to be considered while allocating tasks. + /// By default battery drain is considered. + /// + /// \param[in] value + /// If false, battery drain will not be considered when planning for tasks. + /// As a consequence, charging tasks will not be automatically assigned to + /// vehicles in this fleet when battery levels fall below their thresholds. + bool account_for_battery_drain(bool value = true); /// Set the threshold for state of charge below which robots in this fleet /// will cease to operate and require recharging. A value between 0.0 and 1.0 @@ -134,20 +136,18 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// \return true to indicate that this fleet should accept the request, false /// to reject the request. /// - /// \note This interface will be deprecated. Use the more general - /// AcceptTaskRequest callback using AcceptDeliveryRequest = std::function; /// Provide a callback that indicates whether this fleet will accept a /// delivery request. By default all delivery requests will be rejected. /// - /// \note This function will be deprecated in favor of accept_task_requests() - /// The callback function that you give should ideally be non-blocking + /// \note The callback function that you give should ideally be non-blocking /// and return quickly. It's meant to check whether this fleet's vehicles are /// compatible with the requested payload, pickup, and dropoff behavior /// settings. The path planning feasibility will be taken care of by the /// adapter internally. + [[deprecated("Use accept_task_requests() instead")]] FleetUpdateHandle& accept_delivery_requests(AcceptDeliveryRequest check); /// Specify the default value for how high the delay of the current itinerary diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index ef871cc53..6c7eb416f 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -562,6 +562,7 @@ std::shared_ptr make_fleet( // Drain battery const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default( *node, "drain_battery", false); + connections->fleet->account_for_battery_drain(drain_battery); // Recharge threshold const double recharge_threshold = rmf_fleet_adapter::get_parameter_or_default( @@ -570,7 +571,7 @@ std::shared_ptr make_fleet( connections->fleet->set_recharge_threshold(recharge_threshold); if (!connections->fleet->set_task_planner_params( - battery_system, motion_sink, ambient_sink, tool_sink, drain_battery)) + battery_system, motion_sink, ambient_sink, tool_sink)) { RCLCPP_ERROR( node->get_logger(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index bf42b280a..c5a97a67f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -754,8 +754,7 @@ bool FleetUpdateHandle::set_task_planner_params( std::shared_ptr battery_system, std::shared_ptr motion_sink, std::shared_ptr ambient_sink, - std::shared_ptr tool_sink, - const bool drain_battery) + std::shared_ptr tool_sink) { if (battery_system && motion_sink && ambient_sink && tool_sink) { @@ -774,8 +773,6 @@ bool FleetUpdateHandle::set_task_planner_params( _pimpl->task_planner = std::make_shared( task_config); - - _pimpl->drain_battery = drain_battery; _pimpl->initialized_task_planner = true; @@ -785,6 +782,11 @@ bool FleetUpdateHandle::set_task_planner_params( return false; } +bool FleetUpdateHandle::account_for_battery_drain(bool value) +{ + _pimpl->drain_battery = value; + return _pimpl->drain_battery; +} //============================================================================== FleetUpdateHandle& FleetUpdateHandle::set_recharge_threshold( const double threshold) From 0688fa577df6ff13d2514c5c5acc6d7a99f4df3a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 10:19:19 +0800 Subject: [PATCH 088/128] Added unstable API extension to RobotUpdateHandle --- .../agv/RobotUpdateHandle.hpp | 23 +++++++++++--- rmf_fleet_adapter/src/full_control/main.cpp | 5 +-- .../agv/RobotUpdateHandle.cpp | 31 +++++++++++++------ .../agv/internal_RobotUpdateHandle.hpp | 3 ++ 4 files changed, 47 insertions(+), 15 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 9008ebd3f..7e2411ad6 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -105,11 +105,26 @@ class RobotUpdateHandle /// value that was given to the setter. rmf_utils::optional maximum_delay() const; - /// Get the schedule participant of this robot - rmf_utils::optional> - get_participant(); - class Implementation; + + /// This API is experimental and will not be supported in the future. Users + /// are to avoid relying on these feature for any integration. + class Unstable + { + public: + /// Get the schedule participant of this robot + rmf_traffic::schedule::Participant* get_participant(); + + private: + friend Implementation; + Implementation* _pimpl; + }; + + /// Get a mutable reference to the unstable API extension + Unstable& unstable(); + /// Get a const reference to the unstable API extension + const Unstable& unstable() const; + private: RobotUpdateHandle(); rmf_utils::unique_impl_ptr _pimpl; diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 6c7eb416f..6dd04bfbe 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -319,9 +319,10 @@ class FleetDriverRobotCommandHandle if (trajectory.size() < 2) return; - if (auto participant = _travel_info.updater->get_participant()) + if (auto participant = + _travel_info.updater->unstable().get_participant()) { - participant.value().get().set( + participant->set( {rmf_traffic::Route{state.location.level_name, trajectory}}); _dock_schedule_time = now; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 866992d87..97c97ac57 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -205,22 +205,35 @@ RobotUpdateHandle::maximum_delay() const } //============================================================================== -rmf_utils::optional> - RobotUpdateHandle::get_participant() +RobotUpdateHandle::RobotUpdateHandle() +{ + // Do nothing +} + +//============================================================================== +RobotUpdateHandle::Unstable& RobotUpdateHandle::unstable() +{ + return _pimpl->unstable; +} + +//============================================================================== +const RobotUpdateHandle::Unstable& RobotUpdateHandle::unstable() const +{ + return _pimpl->unstable; +} + +//============================================================================== +rmf_traffic::schedule::Participant* +RobotUpdateHandle::Unstable::get_participant() { if (const auto context = _pimpl->get_context()) { auto& itinerary = context->itinerary(); - return itinerary; + return &itinerary; } - return rmf_utils::nullopt; + return nullptr; } -//============================================================================== -RobotUpdateHandle::RobotUpdateHandle() -{ - // Do nothing -} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index c39ce46d2..28ff376a0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -31,6 +31,7 @@ class RobotUpdateHandle::Implementation std::weak_ptr context; std::string name; + RobotUpdateHandle::Unstable unstable; bool reported_loss = false; static std::shared_ptr make(RobotContextPtr context) @@ -40,6 +41,8 @@ class RobotUpdateHandle::Implementation RobotUpdateHandle handle; handle._pimpl = rmf_utils::make_impl( Implementation{std::move(context), std::move(name)}); + handle._pimpl->unstable._pimpl = + &RobotUpdateHandle::Implementation::get(handle); return std::make_shared(std::move(handle)); } From be64e8c4af5856af8f9c45c4e67a5a15698da165 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 11:00:49 +0800 Subject: [PATCH 089/128] Log error message when battery percentage in RobotState is out of range --- rmf_fleet_adapter/src/full_control/main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 6dd04bfbe..14895fb3f 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -339,6 +339,13 @@ class FleetDriverRobotCommandHandle const double battery_soc = state.battery_percent / 100.0; if (battery_soc >= 0.0 && battery_soc <= 1.0) _travel_info.updater->update_battery_soc(battery_soc); + else + RCLCPP_ERROR( + _node->get_logger(), + "Battery percentage reported by the robot is outside of the valid " + "range [0,100] and hence the battery soc will not be updated. It is " + "critical to update the battery soc with a valid battery percentage " + "for task allocation planning."); } void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) From e78dd7938de085b4230390ee1c23ef79927bf309 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 11:20:10 +0800 Subject: [PATCH 090/128] Removed TODO in Task and iostream header --- rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp | 2 -- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 4 ---- 2 files changed, 6 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index 36a34be6d..d0850e042 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -97,8 +97,6 @@ class Task : public std::enable_shared_from_this using PendingPhases = std::vector>; // Make a new task - // TODO(YV) Remove default nullptr for request after refactoring Loop and - // Delivery static std::shared_ptr make( std::string id, PendingPhases phases, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index f10a52d66..8c60030ac 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -31,8 +31,6 @@ #include "tasks/Delivery.hpp" #include "tasks/Loop.hpp" -#include - namespace rmf_fleet_adapter { //============================================================================== @@ -89,8 +87,6 @@ void TaskManager::queue_task(std::shared_ptr task, Start expected_finish) _queue.push_back(std::move(task)); _expected_finish_location = std::move(expected_finish); - std::cout << "Queuing new task for [" << _context->requester_id() - << "]. New queue size: " << _queue.size() << std::endl; RCLCPP_INFO( _context->node()->get_logger(), "Queuing new task [%s] for [%s]. New queue size: %d", From b8d61d65da5b67781eed5483e11ab80d650e70eb Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 11:39:13 +0800 Subject: [PATCH 091/128] Renamed state() to current_task_end_state() in Robotcontext --- .../src/rmf_fleet_adapter/TaskManager.cpp | 8 ++++---- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 10 +++++----- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 11 ++++++----- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 8c60030ac..c3368f6a4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -123,10 +123,10 @@ auto TaskManager::expected_finish_state() const -> State // If an active task exists, return the estimated finish state of that task /// else update the current time and battery level for the state and return if (_active_task) - return _context->state(); + return _context->current_task_end_state(); // Update battery soc and finish time in the current state - auto& finish_state = _context->state(); + auto& finish_state = _context->current_task_end_state(); auto location = finish_state.location(); location.time(rmf_traffic_ros2::convert(_context->node()->now())); finish_state.location(location); @@ -159,7 +159,7 @@ void TaskManager::set_queue( for (std::size_t i = 0; i < assignments.size(); ++i) { const auto& a = assignments[i]; - auto start = _context->state().location(); + auto start = _context->current_task_end_state().location(); if (i != 0) start = assignments[i-1].state().location(); start.time(a.deployment_time()); @@ -291,7 +291,7 @@ void TaskManager::_begin_next_task() if (now > deployment_time) { // Update state in RobotContext and Assign active task - _context->state(_queue.front()->finish_state()); + _context->current_task_end_state(_queue.front()->finish_state()); _active_task = std::move(_queue.front()); _queue.erase(_queue.begin()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index beed3ce6f..49e9d5f79 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -214,16 +214,16 @@ RobotContext& RobotContext::maximum_delay( } //============================================================================== -rmf_task::agv::State& RobotContext::state() +rmf_task::agv::State& RobotContext::current_task_end_state() { - return _state; + return _current_task_end_state; } //============================================================================== -RobotContext& RobotContext::state( +RobotContext& RobotContext::current_task_end_state( const rmf_task::agv::State& state) { - _state = state; + _current_task_end_state = state; return *this; } @@ -302,7 +302,7 @@ RobotContext::RobotContext( _maximum_delay(maximum_delay), _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), - _state(state), + _current_task_end_state(state), _state_config(state_config), _task_planner(std::move(task_planner)) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index cfb359ac5..be03bf753 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -116,11 +116,12 @@ class RobotContext const TableViewerPtr& table_viewer, const ResponderPtr& responder) final; - /// Set the state of this robot - RobotContext& state(const rmf_task::agv::State& state); + /// Set the state of this robot at the end of its current task + RobotContext& current_task_end_state(const rmf_task::agv::State& state); - /// Get a mutable reference to the state of this robot - rmf_task::agv::State& state(); + /// Get a mutable reference to the state of this robot at the end of its + // current task + rmf_task::agv::State& current_task_end_state(); /// Get the state config of this robot const rmf_task::agv::StateConfig state_config() const; @@ -179,7 +180,7 @@ class RobotContext double _current_battery_soc; rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; - rmf_task::agv::State _state; + rmf_task::agv::State _current_task_end_state; rmf_task::agv::StateConfig _state_config; std::shared_ptr _task_planner; }; From ece42e9badc59f55bbffda1e081657f9dce7d714 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 13:29:12 +0800 Subject: [PATCH 092/128] TaskManager queue.clear() is protected by mutex --- .../src/rmf_fleet_adapter/TaskManager.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index c3368f6a4..f63a5d6c1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -126,7 +126,7 @@ auto TaskManager::expected_finish_state() const -> State return _context->current_task_end_state(); // Update battery soc and finish time in the current state - auto& finish_state = _context->current_task_end_state(); + auto finish_state = _context->current_task_end_state(); auto location = finish_state.location(); location.time(rmf_traffic_ros2::convert(_context->node()->now())); finish_state.location(location); @@ -153,7 +153,10 @@ agv::ConstRobotContextPtr TaskManager::context() const void TaskManager::set_queue( const std::vector& assignments) { - _queue.clear(); + { + std::lock_guard guard(_mutex); + _queue.clear(); + } // We use dynamic cast to determine the type of request and then call the // appropriate make(~) function to convert the request into a task for (std::size_t i = 0; i < assignments.size(); ++i) @@ -177,7 +180,6 @@ void TaskManager::set_queue( a.state()); std::lock_guard guard(_mutex); - _queue.push_back(task); } @@ -192,9 +194,8 @@ void TaskManager::set_queue( start, a.deployment_time(), a.state()); - - std::lock_guard guard(_mutex); + std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -210,6 +211,7 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); + std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -224,6 +226,7 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); + std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -288,7 +291,7 @@ void TaskManager::_begin_next_task() const auto next_task = _queue.front(); const auto deployment_time = next_task->deployment_time(); - if (now > deployment_time) + if (now >= deployment_time) { // Update state in RobotContext and Assign active task _context->current_task_end_state(_queue.front()->finish_state()); From 81ee72b3599c5dc2dfd2a6e8b0171956babc555e Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 13:50:56 +0800 Subject: [PATCH 093/128] Protected read instances of queue with mutex --- .../src/rmf_fleet_adapter/TaskManager.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index f63a5d6c1..0a3a4d56b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -84,6 +84,7 @@ TaskManager::TaskManager(agv::RobotContextPtr context) //============================================================================== void TaskManager::queue_task(std::shared_ptr task, Start expected_finish) { + std::lock_guard guard(_mutex); _queue.push_back(std::move(task)); _expected_finish_location = std::move(expected_finish); @@ -153,10 +154,9 @@ agv::ConstRobotContextPtr TaskManager::context() const void TaskManager::set_queue( const std::vector& assignments) { - { - std::lock_guard guard(_mutex); - _queue.clear(); - } + std::lock_guard guard(_mutex); + _queue.clear(); + // We use dynamic cast to determine the type of request and then call the // appropriate make(~) function to convert the request into a task for (std::size_t i = 0; i < assignments.size(); ++i) @@ -179,7 +179,6 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); - std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -195,7 +194,6 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); - std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -211,7 +209,6 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); - std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -226,7 +223,6 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); - std::lock_guard guard(_mutex); _queue.push_back(task); } @@ -272,6 +268,8 @@ void TaskManager::_begin_next_task() if (_active_task) return; + std::lock_guard guard(_mutex); + if (_queue.empty()) { // _task_sub.unsubscribe(); @@ -285,7 +283,6 @@ void TaskManager::_begin_next_task() return; } - std::lock_guard guard(_mutex); const rmf_traffic::Time now = rmf_traffic_ros2::convert( _context->node()->now()); const auto next_task = _queue.front(); @@ -373,8 +370,11 @@ void TaskManager::clear_queue() //============================================================================== void TaskManager::retreat_to_charger() { - if (_active_task || !_queue.empty()) - return; + { + std::lock_guard guard(_mutex); + if (_active_task || !_queue.empty()) + return; + } const auto task_planner = _context->task_planner(); if (!task_planner) From d0fcd46ce67b1739e2579b152caa584da836eb1f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 14:49:37 +0800 Subject: [PATCH 094/128] Added warning when robot has insufficient charge to automatically retreat to charger --- .../src/rmf_fleet_adapter/TaskManager.cpp | 11 ++++++++++- rmf_task/src/rmf_task/Estimate.cpp | 7 ++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 0a3a4d56b..3005c998b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -420,7 +420,6 @@ void TaskManager::retreat_to_charger() rmf_traffic::time::to_seconds(retreat_duration)); retreat_battery_drain = dSOC_motion + dSOC_device; - // TODO(YV) Protect this call with a mutex estimate_cache->set(endpoints, retreat_duration, retreat_battery_drain); } @@ -459,6 +458,16 @@ void TaskManager::retreat_to_charger() "Initiating automatic retreat to charger for robot [%s]", _context->name().c_str()); } + + if ((battery_soc_after_retreat < retreat_threshold) && + (battery_soc_after_retreat < threshold_soc)) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Robot [%s] needs to be charged but has insufficient battery remaining " + "to retreat to its designated charger.", + _context->name().c_str()); + } } } // namespace rmf_fleet_adapter diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 12945d413..56eaa0281 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -14,7 +14,9 @@ * limitations under the License. * */ + #include +#include #include @@ -81,17 +83,19 @@ class EstimateCache::Implementation using Cache = std::unordered_map, CacheElement, PairHash>; Cache _cache; + std::shared_ptr _mutex = std::make_shared(); }; //============================================================================== EstimateCache::EstimateCache() - : _pimpl(rmf_utils::make_impl()) + : _pimpl(rmf_utils::make_impl(Implementation())) {} //============================================================================== std::optional EstimateCache::get( std::pair waypoints) const { + std::lock_guard guard(*_pimpl->_mutex); auto it = _pimpl->_cache.find(waypoints); if (it != _pimpl->_cache.end()) { @@ -104,6 +108,7 @@ std::optional EstimateCache::get( void EstimateCache::set(std::pair waypoints, rmf_traffic::Duration duration, double dsoc) { + std::lock_guard guard(*_pimpl->_mutex); _pimpl->_cache[waypoints] = CacheElement{duration, dsoc}; } From d907d545c5f9f2b67dca882af25ffe71a6fddda2 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 15:01:17 +0800 Subject: [PATCH 095/128] Removed clear_queue() and moved begin_next_task into private --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 8 -------- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp | 6 ++---- 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3005c998b..b35e16479 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -337,7 +337,6 @@ void TaskManager::_begin_next_task() msg.end_time = rmf_traffic_ros2::convert( _active_task->finish_state().finish_time()); _context->node()->task_summary()->publish(msg); - // _begin_next_task(); }, [this, id = _active_task->id()]() { @@ -360,13 +359,6 @@ void TaskManager::_begin_next_task() } } -//============================================================================== -void TaskManager::clear_queue() -{ - std::lock_guard guard(_mutex); - _queue.clear(); -} - //============================================================================== void TaskManager::retreat_to_charger() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 7284684c7..c1a11956b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -64,9 +64,6 @@ class TaskManager : public std::enable_shared_from_this /// Get the non-charging requests among pending tasks const std::vector requests() const; - /// Callback for task timer which begins next task if its deployment time has passed - void _begin_next_task(); - /// The state of the robot. State expected_finish_state() const; @@ -89,7 +86,8 @@ class TaskManager : public std::enable_shared_from_this rclcpp::TimerBase::SharedPtr _task_timer; rclcpp::TimerBase::SharedPtr _retreat_timer; - void clear_queue(); + /// Callback for task timer which begins next task if its deployment time has passed + void _begin_next_task(); }; using TaskManagerPtr = std::shared_ptr; From 12e5b75b8be1459f9b598ae14ab0459a9d880520 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 15:27:15 +0800 Subject: [PATCH 096/128] Added TODO to redesign TaskManager --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index c1a11956b..a38281834 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -82,6 +82,9 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _task_sub; rxcpp::subscription _emergency_sub; + // TODO: Eliminate the need for a mutex by redesigning the use of the task + // manager so that modifications of shared data only happen on designated + // rxcpp worker std::mutex _mutex; rclcpp::TimerBase::SharedPtr _task_timer; rclcpp::TimerBase::SharedPtr _retreat_timer; From f73f6896aedf9ba33247fe43c9034138912a848a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 16:23:04 +0800 Subject: [PATCH 097/128] Bumped printouts for missing fields to ERROR and other corrections to FleetUpdateHandle --- .../agv/FleetUpdateHandle.cpp | 39 ++++++++++--------- .../agv/internal_FleetUpdateHandle.hpp | 2 +- 2 files changed, 21 insertions(+), 20 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index c5a97a67f..e0d3e4d58 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -73,7 +73,7 @@ class LiaisonNegotiator : public rmf_traffic::schedule::Negotiator //============================================================================== void FleetUpdateHandle::Implementation::dock_summary_cb( - const DockSummary::SharedPtr msg) + const DockSummary::SharedPtr& msg) { for (const auto& dock : msg->docks) { @@ -151,7 +151,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( { if (task_profile.clean.start_waypoint.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [clean.start_waypoint] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -184,10 +184,10 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( return; } - const auto clean_param = clean_param_it->second; + const auto& clean_param = clean_param_it->second; // Check for valid finish waypoint - const std::string finish_wp_name = clean_param.finish; + const std::string& finish_wp_name = clean_param.finish; const auto finish_wp = graph.find_waypoint(finish_wp_name); if (!finish_wp) { @@ -210,7 +210,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( start_time, positions); - if (!(cleaning_trajectory.size() > 0)) + if (cleaning_trajectory.size() == 0) { RCLCPP_INFO( node->get_logger(), @@ -234,7 +234,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), - "Generated Clean request"); + "Generated Clean request for task_id:[%s]", id.c_str()); } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) @@ -242,7 +242,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const auto& delivery = task_profile.delivery; if (delivery.pickup_place_name.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [delivery.pickup_place_name] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -252,7 +252,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (delivery.pickup_dispenser.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [delivery.pickup_dispenser] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -262,7 +262,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (delivery.dropoff_place_name.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [delivery.dropoff_place_name] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -272,7 +272,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (delivery.dropoff_place_name.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [delivery.dropoff_place_name] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -282,7 +282,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (delivery.dropoff_ingestor.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [delivery.dropoff_ingestor] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -329,7 +329,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), - "Generated Delivery request"); + "Generated Delivery request for task_id:[%s]", id.c_str()); } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) @@ -337,7 +337,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const auto& loop = task_profile.loop; if (loop.start_name.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [loop.start_name] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -347,7 +347,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (loop.finish_name.empty()) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [loop.finish_name] missing in TaskProfile." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -357,7 +357,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (loop.num_loops < 1) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), "Required param [loop.num_loops] in TaskProfile is invalid." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); @@ -402,14 +402,15 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), - "Generated Loop request"); + "Generated Loop request for task_id:[%s]", id.c_str()); } else { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), - "Invalid TaskType in TaskProfile. Rejecting BidNotice with task_id:[%s]", - id.c_str()); + "Invalid TaskType [%d] in TaskProfile. Rejecting BidNotice with " + "task_id:[%s]", + task_type.type, id.c_str()); return; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 2272e2310..229e984a4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -245,7 +245,7 @@ class FleetUpdateHandle::Implementation return std::make_shared(std::move(handle)); } - void dock_summary_cb(const DockSummary::SharedPtr msg); + void dock_summary_cb(const DockSummary::SharedPtr& msg); void bid_notice_cb(const BidNotice::SharedPtr msg); From b0402b3294d5b9e0f247ebf6435b23e9accf0016 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 15 Dec 2020 17:17:07 +0800 Subject: [PATCH 098/128] retreat_to_charger() estimates battery drain over all trajectories in the plan --- .../src/rmf_fleet_adapter/TaskManager.cpp | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index b35e16479..5f1d475ed 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -394,24 +394,34 @@ void TaskManager::retreat_to_charger() } else { - const rmf_traffic::agv::Planner::Goal retreat_goal{current_state.charging_waypoint()}; + const rmf_traffic::agv::Planner::Goal retreat_goal{ + current_state.charging_waypoint()}; const auto result_to_charger = task_planner_config->planner()->plan( current_state.location(), retreat_goal); // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - current_state.finish_time(); - - const double dSOC_motion = - task_planner_config->motion_sink()->compute_change_in_charge(trajectory); - const double dSOC_device = - task_planner_config->ambient_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - retreat_battery_drain = dSOC_motion + dSOC_device; + double dSOC_motion = 0.0; + double dSOC_device = 0.0; + rmf_traffic::Duration retreat_duration = rmf_traffic::Duration{0}; + rmf_traffic::Time itinerary_start_time = current_state.finish_time(); + 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 += + task_planner_config->motion_sink()->compute_change_in_charge( + trajectory); + dSOC_device += + task_planner_config->ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(itinerary_duration)); + retreat_battery_drain += dSOC_motion + dSOC_device; + retreat_duration +=itinerary_duration; + itinerary_start_time = finish_time; + } estimate_cache->set(endpoints, retreat_duration, retreat_battery_drain); } From 3e616c9e3e88da1bb33a95a151fc990ac00330dd Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 16 Dec 2020 13:12:46 +0800 Subject: [PATCH 099/128] Battery drain is computed over all trajectories in a generated plan --- .../src/rmf_fleet_adapter/TaskManager.cpp | 4 +- .../src/rmf_task/requests/ChargeBattery.cpp | 32 +++++---- rmf_task/src/rmf_task/requests/Clean.cpp | 66 +++++++++++------- rmf_task/src/rmf_task/requests/Delivery.cpp | 68 ++++++++++++------- rmf_task/src/rmf_task/requests/Loop.cpp | 65 +++++++++++------- 5 files changed, 147 insertions(+), 88 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 5f1d475ed..fa6ac0a4d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -412,10 +412,10 @@ void TaskManager::retreat_to_charger() const rmf_traffic::Duration itinerary_duration = finish_time - itinerary_start_time; - dSOC_motion += + dSOC_motion = task_planner_config->motion_sink()->compute_change_in_charge( trajectory); - dSOC_device += + dSOC_device = task_planner_config->ambient_sink()->compute_change_in_charge( rmf_traffic::time::to_seconds(itinerary_duration)); retreat_battery_drain += dSOC_motion + dSOC_device; diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 984767830..c4062b94e 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -128,6 +128,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( double battery_soc = initial_state.battery_soc(); double dSOC_motion = 0.0; double dSOC_device = 0.0; + double variant_battery_drain = 0.0; rmf_traffic::Duration variant_duration(0); if (initial_state.waypoint() != initial_state.charging_waypoint()) @@ -147,21 +148,28 @@ rmf_utils::optional ChargeBattery::estimate_finish( rmf_traffic::agv::Planner::Goal goal{endpoints.second}; const auto result = _pimpl->planner->plan( initial_state.location(), goal); - const auto& trajectory = result->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if (_pimpl->drain_battery) + auto itinerary_start_time = start_time; + for (const auto& itinerary : result->get_itinerary()) { - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge( - trajectory); - dSOC_device = _pimpl->device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if (_pimpl->drain_battery) + { + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge( + trajectory); + dSOC_device = _pimpl->device_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, - dSOC_motion + dSOC_device); + variant_battery_drain); } // If a robot cannot reach its charging dock given its initial battery soc diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index f0df52a83..ece55075f 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -138,7 +138,8 @@ rmf_utils::optional Clean::estimate_finish( if (cache_result) { variant_duration = cache_result->duration; - battery_soc = battery_soc - cache_result->dsoc; + if (_pimpl->drain_battery) + battery_soc = battery_soc - cache_result->dsoc; } else { @@ -147,23 +148,32 @@ rmf_utils::optional Clean::estimate_finish( const auto result_to_start = _pimpl->planner->plan( initial_state.location(), goal); // We assume we can always compute a plan - const auto& trajectory = - result_to_start->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if(_pimpl->drain_battery) + auto itinerary_start_time = start_time; + double variant_battery_drain = 0.0; + for (const auto& itinerary : result_to_start->get_itinerary()) { - // Compute battery drain - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_ambient = - _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_ambient; + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if(_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge( + trajectory); + dSOC_ambient = + _pimpl->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, - dSOC_motion + dSOC_ambient); + variant_battery_drain); } if (battery_soc <= state_config.threshold_soc()) @@ -230,16 +240,24 @@ rmf_utils::optional Clean::estimate_finish( const auto result_to_charger = _pimpl->planner->plan(start, goal); // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - state.finish_time(); - - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - retreat_battery_drain = dSOC_motion + dSOC_ambient; + 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 = _pimpl->motion_sink->compute_change_in_charge( + trajectory); + dSOC_ambient = _pimpl->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); } } diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 176370313..dc7c59ebc 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -151,7 +151,8 @@ rmf_utils::optional Delivery::estimate_finish( if (cache_result) { variant_duration = cache_result->duration; - battery_soc = battery_soc - cache_result->dsoc; + if (_pimpl->drain_battery) + battery_soc = battery_soc - cache_result->dsoc; } else { @@ -160,22 +161,31 @@ rmf_utils::optional Delivery::estimate_finish( const auto result_to_pickup = _pimpl->planner->plan( initial_state.location(), goal); // We assume we can always compute a plan - const auto& trajectory = - result_to_pickup->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if (_pimpl->drain_battery) - { - // Compute battery drain - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_device = - _pimpl->device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; + 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 (_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge( + trajectory); + dSOC_device = + _pimpl->device_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, - dSOC_motion + dSOC_device); + variant_battery_drain); } if (battery_soc <= state_config.threshold_soc()) @@ -235,17 +245,23 @@ rmf_utils::optional Delivery::estimate_finish( const auto result_to_charger = _pimpl->planner->plan(start, goal); // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - state.finish_time(); - - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_device = _pimpl->device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - retreat_battery_drain = dSOC_motion + dSOC_device; - + 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 = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->device_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); } diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 0a89b8137..68e2d64c9 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -141,7 +141,8 @@ rmf_utils::optional Loop::estimate_finish( if (cache_result) { variant_duration = cache_result->duration; - battery_soc = battery_soc - cache_result->dsoc; + if (_pimpl->drain_battery) + battery_soc = battery_soc - cache_result->dsoc; } else { @@ -150,22 +151,30 @@ rmf_utils::optional Loop::estimate_finish( const auto plan_to_start = _pimpl->planner->plan( initial_state.location(), loop_start_goal); // We assume we can always compute a plan - const auto& trajectory = plan_to_start->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - variant_duration = finish_time - start_time; - - if (_pimpl->drain_battery) + auto itinerary_start_time = start_time; + double variant_battery_drain = 0.0; + for (const auto& itinerary : plan_to_start->get_itinerary()) { - // Compute battery drain - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_device = - _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(variant_duration)); - battery_soc = battery_soc - dSOC_motion - dSOC_device; + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if (_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = + _pimpl->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, - dSOC_motion + dSOC_device); + variant_battery_drain); } if (battery_soc <= state_config.threshold_soc()) @@ -228,16 +237,24 @@ rmf_utils::optional Loop::estimate_finish( const auto result_to_charger = _pimpl->planner->plan( retreat_start, charger_goal); // We assume we can always compute a plan - const auto& trajectory = - result_to_charger->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration retreat_duration = - finish_time - state_finish_time; - - dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); - dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(retreat_duration)); - retreat_battery_drain = dSOC_motion + dSOC_device; + 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 = _pimpl->motion_sink->compute_change_in_charge( + trajectory); + dSOC_device = _pimpl->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); } From 3f20bb963bd69a985af6662734b8fd75083ffb16 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 16 Dec 2020 14:48:51 +0800 Subject: [PATCH 100/128] Iterate over all trajectories in plan when computing invariant battery drain --- rmf_task/src/rmf_task/requests/Delivery.cpp | 35 ++++++++++-------- rmf_task/src/rmf_task/requests/Loop.cpp | 41 +++++++++++++-------- 2 files changed, 45 insertions(+), 31 deletions(-) diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index dc7c59ebc..5cc1377f5 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -79,29 +79,34 @@ rmf_task::ConstRequestPtr Delivery::make( if (delivery->_pimpl->pickup_waypoint != delivery->_pimpl->dropoff_waypoint) { - const auto plan_start_time = std::chrono::steady_clock::now(); rmf_traffic::agv::Planner::Start start{ - plan_start_time, + start_time, delivery->_pimpl->pickup_waypoint, 0.0}; rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->dropoff_waypoint}; const auto result_to_dropoff = delivery->_pimpl->planner->plan(start, goal); - const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - - delivery->_pimpl->invariant_duration = finish_time - plan_start_time; - - if (delivery->_pimpl->drain_battery) + auto itinerary_start_time = start_time; + for (const auto& itinerary : result_to_dropoff->get_itinerary()) { - // Compute battery drain - const double dSOC_motion = - delivery->_pimpl->motion_sink->compute_change_in_charge(trajectory); - const double dSOC_device = - delivery->_pimpl->device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(delivery->_pimpl->invariant_duration)); - delivery->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_device; + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const auto itinerary_duration = finish_time - itinerary_start_time; + + if (delivery->_pimpl->drain_battery) + { + // Compute battery drain + const double dSOC_motion = + delivery->_pimpl->motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + delivery->_pimpl->device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(itinerary_duration)); + delivery->_pimpl->invariant_battery_drain += dSOC_motion + dSOC_device; + } + + delivery->_pimpl->invariant_duration += itinerary_duration; + itinerary_start_time = finish_time; } } diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 68e2d64c9..dc63d0589 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -80,24 +80,33 @@ ConstRequestPtr Loop::make( const auto forward_loop_plan = loop->_pimpl->planner->plan( loop_start, loop_end_goal); - const auto trajectory = - forward_loop_plan->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const auto forward_duration = finish_time - start_time; - loop->_pimpl->invariant_duration = (2 * num_loops - 1) * forward_duration; - - if (loop->_pimpl->drain_battery) + auto itinerary_start_time = start_time; + double forward_battery_drain = 0.0; + rmf_traffic::Duration forward_duration(0); + for (const auto& itinerary : forward_loop_plan->get_itinerary()) { - // Compute battery drain - const double dSOC_motion = - loop->_pimpl->motion_sink->compute_change_in_charge(trajectory); - const double dSOC_device = - loop->_pimpl->ambient_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(forward_duration)); - const double forward_battery_drain = dSOC_motion + dSOC_device; - loop->_pimpl->invariant_battery_drain = - (2 * num_loops - 1) * forward_battery_drain; + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const auto itinerary_duration = finish_time - itinerary_start_time; + + if (loop->_pimpl->drain_battery) + { + // Compute battery drain + const double dSOC_motion = + loop->_pimpl->motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + loop->_pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(itinerary_duration)); + forward_battery_drain += dSOC_motion + dSOC_device; + } + + forward_duration += itinerary_duration; + itinerary_start_time = finish_time; } + loop->_pimpl->invariant_duration = + (2 * num_loops - 1) * forward_duration; + loop->_pimpl->invariant_battery_drain = + (2 * num_loops - 1) * forward_battery_drain; } return loop; From 6b661b051545cd2752059222760b1564da93004b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 17 Dec 2020 18:19:26 +0800 Subject: [PATCH 101/128] Rename inertia to moment_of_inertia --- .../include/rmf_battery/agv/MechanicalSystem.hpp | 6 +++--- rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp | 12 ++++++------ .../src/rmf_battery/agv/SimpleMotionPowerSink.cpp | 6 ++++-- rmf_battery/test/unit/agv/test_MechanicalSystem.cpp | 2 +- .../src/rmf_fleet_adapter/load_param.cpp | 8 ++++---- 5 files changed, 18 insertions(+), 16 deletions(-) diff --git a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp index 889b9c097..23a7f66ff 100644 --- a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp @@ -35,7 +35,7 @@ class MechanicalSystem /// \param[in] mass /// The mass of the robot in Kilograms(kg) /// - /// \param[in] inertia + /// \param[in] moment_of_inertia /// The moment of inertia of the robot along its yaw axis in kg.m^2 /// /// \param[in] friction_coefficient @@ -44,14 +44,14 @@ class MechanicalSystem /// vehicle's wheels during locomotion. static std::optional make( double mass, - double inertia, + double moment_of_inertia, double friction_coefficient); /// Get the mass of this mechanical system double mass() const; /// Get the moment of inertia of this mechanical system - double inertia() const; + double moment_of_inertia() const; /// Get the friction coefficient of this mechanical system double friction_coefficient() const; diff --git a/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp index 8f60a2730..a07fe7ec0 100644 --- a/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp +++ b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp @@ -25,22 +25,22 @@ class MechanicalSystem::Implementation { public: double mass; - double inertia; + double moment_of_inertia; double friction_coefficient; }; //============================================================================== std::optional MechanicalSystem::make( double mass, - double inertia, + double moment_of_inertia, double friction_coefficient) { - if (mass <= 0.0 || inertia <= 0.0 || friction_coefficient <= 0.0) + if (mass <= 0.0 || moment_of_inertia <= 0.0 || friction_coefficient <= 0.0) return std::nullopt; MechanicalSystem mechanical_system; mechanical_system._pimpl->mass = mass; - mechanical_system._pimpl->inertia = inertia; + mechanical_system._pimpl->moment_of_inertia = moment_of_inertia; mechanical_system._pimpl->friction_coefficient = friction_coefficient; return mechanical_system; @@ -65,9 +65,9 @@ double MechanicalSystem::friction_coefficient() const } //============================================================================== -double MechanicalSystem::inertia() const +double MechanicalSystem::moment_of_inertia() const { - return _pimpl->inertia; + return _pimpl->moment_of_inertia; } } // namespace agv diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 4caf9bdee..867a42a52 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -80,7 +80,8 @@ double SimpleMotionPowerSink::compute_change_in_charge( const double capacity = _pimpl->battery_system.capacity(); const double nominal_voltage = _pimpl->battery_system.nominal_voltage(); const double mass = _pimpl->mechanical_system.mass(); - const double inertia = _pimpl->mechanical_system.inertia(); + const double moment_of_inertia = + _pimpl->mechanical_system.moment_of_inertia(); const double friction = _pimpl->mechanical_system.friction_coefficient(); auto begin_it = trajectory.begin(); @@ -109,7 +110,8 @@ double SimpleMotionPowerSink::compute_change_in_charge( const double alpha = std::abs(acceleration[2]); // Loss through acceleration - const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; + const double EA = ((mass * a * v) + + (moment_of_inertia * alpha * w)) * sim_step; // Loss through friction const double EF = compute_friction_energy(friction, mass, v, sim_step); diff --git a/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp b/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp index ef7be26d0..62337b04a 100644 --- a/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp +++ b/rmf_battery/test/unit/agv/test_MechanicalSystem.cpp @@ -28,7 +28,7 @@ SCENARIO("Test MechanicalSystem") auto mechanical_system = MechanicalSystem::make(10.0, 20.0, 0.3); REQUIRE(mechanical_system); CHECK(mechanical_system->mass() == 10.0); - CHECK(mechanical_system->inertia() == 20.0); + CHECK(mechanical_system->moment_of_inertia() == 20.0); CHECK(mechanical_system->friction_coefficient() == 0.3); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp index de90d26fe..32ae019b6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp @@ -114,18 +114,18 @@ std::optional get_battery_system( std::optional get_mechanical_system( rclcpp::Node& node, const double default_mass, - const double default_inertia, + const double default_moment, const double default_friction) { const double mass = get_parameter_or_default(node, "mass", default_mass); - const double inertia = - get_parameter_or_default(node, "inertia", default_inertia); + const double moment_of_inertia = + get_parameter_or_default(node, "inertia", default_moment); const double friction = get_parameter_or_default(node, "friction_coefficient", default_friction); auto mechanical_system = rmf_battery::agv::MechanicalSystem::make( - mass, inertia, friction); + mass, moment_of_inertia, friction); return mechanical_system; } From ce7c2abcbbd9b296ab7e328cd225b9039640ab96 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 17 Dec 2020 18:49:42 +0800 Subject: [PATCH 102/128] Added const type specifier and comments for test expectations in rmf_battery --- .../rmf_battery/agv/SimpleDevicePowerSink.hpp | 4 +- .../rmf_battery/agv/SimpleMotionPowerSink.hpp | 4 +- .../rmf_battery/agv/SimpleDevicePowerSink.cpp | 6 ++- .../rmf_battery/agv/SimpleMotionPowerSink.cpp | 10 ++-- .../test/unit/agv/test_battery_drain.cpp | 54 ++++++++++++------- 5 files changed, 48 insertions(+), 30 deletions(-) diff --git a/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp b/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp index 7dacc600f..46964a5af 100644 --- a/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp +++ b/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp @@ -40,8 +40,8 @@ class SimpleDevicePowerSink : public DevicePowerSink /// \param[in] power_system /// The PowerSystem for this device SimpleDevicePowerSink( - BatterySystem& battery_system, - PowerSystem& power_system); + const BatterySystem& battery_system, + const PowerSystem& power_system); /// Get a constant reference to the battery system const BatterySystem& battery_system() const; diff --git a/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp b/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp index 56ac2246b..4aea243ef 100644 --- a/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp +++ b/rmf_battery/include/rmf_battery/agv/SimpleMotionPowerSink.hpp @@ -40,8 +40,8 @@ class SimpleMotionPowerSink : public MotionPowerSink /// \param[in] mechanical_system /// The MechanicalSystem of the robot SimpleMotionPowerSink( - BatterySystem& battery_system, - MechanicalSystem& mechanical_system); + const BatterySystem& battery_system, + const MechanicalSystem& mechanical_system); /// Get a constant reference to the battery system const BatterySystem& battery_system() const; diff --git a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp index 1928d8c33..b3ee31062 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp @@ -31,8 +31,8 @@ class SimpleDevicePowerSink::Implementation //============================================================================== SimpleDevicePowerSink::SimpleDevicePowerSink( - BatterySystem& battery_system, - PowerSystem& power_system) + const BatterySystem& battery_system, + const PowerSystem& power_system) : _pimpl(rmf_utils::make_impl( Implementation{battery_system, power_system})) { @@ -62,6 +62,8 @@ double SimpleDevicePowerSink::compute_change_in_charge( const double dE = nominal_power * run_time; const double dQ = dE / nominal_voltage; + // We multiply the capacity by 3600 to convert from units of Ampere-hours to + // Ampere-seconds const double dSOC = dQ / (capacity * 3600.0); return dSOC; diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 867a42a52..6ebf5fe26 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -35,8 +35,8 @@ class SimpleMotionPowerSink::Implementation //============================================================================== SimpleMotionPowerSink::SimpleMotionPowerSink( - BatterySystem& battery_system, - MechanicalSystem& mechanical_system) + const BatterySystem& battery_system, + const MechanicalSystem& mechanical_system) : _pimpl(rmf_utils::make_impl( Implementation{battery_system, mechanical_system})) { @@ -118,9 +118,11 @@ double SimpleMotionPowerSink::compute_change_in_charge( dE += EA + EF; } - // The charge consumed + // Compute the charge consumed const double dQ = dE / nominal_voltage; - // The depleted state of charge + // Compute the change in state of charge + // We multiply the capacity by 3600 to convert from units of Ampere-hours to + // Ampere-seconds const double dSOC = dQ / (capacity * 3600.0); return dSOC; } diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 2a5ca8fd4..5bdfb1074 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -41,18 +41,18 @@ SCENARIO("Test battery drain with RobotA") // Initializing system traits auto battery_system_optional = BatterySystem::make(12.0, 24.0, 2.0); REQUIRE(battery_system_optional); - BatterySystem& battery_system = *battery_system_optional; + const BatterySystem& battery_system = *battery_system_optional; auto mechanical_system_optional = MechanicalSystem::make(20.0, 10.0, 0.3); REQUIRE(mechanical_system_optional); - MechanicalSystem& mechanical_system = *mechanical_system_optional; + const MechanicalSystem& mechanical_system = *mechanical_system_optional; auto power_system_optional = PowerSystem::make(10.0); REQUIRE(power_system_optional); - PowerSystem& power_system_processor = *power_system_optional; + const PowerSystem& power_system_processor = *power_system_optional; - SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; - SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; + const SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + const SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; // Initializing vehicle traits @@ -77,6 +77,8 @@ SCENARIO("Test battery drain with RobotA") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // We expect less than 1% depletion of the battery level after this robot + // travels a distance of 100m. const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } @@ -97,7 +99,9 @@ SCENARIO("Test battery drain with RobotA") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - const bool ok = remaining_soc > -1 && remaining_soc < 0.05; + // We expect the robot's battery to be completely drained as the mileage + // for this robot is around 15km. + const bool ok = std::abs(remaining_soc) < 0.06; CHECK(ok); } } @@ -114,18 +118,18 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") // Initializing system traits auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); REQUIRE(battery_system_optional); - BatterySystem& battery_system = *battery_system_optional; + const 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; + const 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; + const PowerSystem& power_system_processor = *power_system_optional; - SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; - SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; + const SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + const SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( @@ -149,6 +153,8 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // We expect less than 2% depletion of the battery level after this robot + // travels a distance of 100m. const bool ok = remaining_soc > 0.98 && remaining_soc < 1.0; CHECK(ok); } @@ -169,7 +175,9 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; + // We expect the robot's battery to be completely drained as the mileage + // for this robot is around 20km. + const bool ok = std::abs(remaining_soc) < 0.06; CHECK(ok); } @@ -189,8 +197,9 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - - const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; + // We expect the robot's battery to be completely drained as the mileage + // for this robot is around 20km. + const bool ok = std::abs(remaining_soc) < 0.06; CHECK(ok); } @@ -213,7 +222,9 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; - const bool ok = remaining_soc > -1.0 && remaining_soc < 0.10; + // We expect the robot's battery to be completely drained as the mileage + // for this robot is around 20km. + const bool ok = std::abs(remaining_soc) < 0.06; CHECK(ok); } @@ -237,6 +248,8 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // We expect less than 1% battery drain for this robot when it turns on the + // spot for 5s. const bool ok = remaining_soc > 0.99 && remaining_soc < 1.0; CHECK(ok); } @@ -254,18 +267,18 @@ SCENARIO("Testing Cleaning Request") // Initializing system traits auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); REQUIRE(battery_system_optional); - BatterySystem& battery_system = *battery_system_optional; + const 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; + const 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; + const PowerSystem& power_system_processor = *power_system_optional; - SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; - SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; + const SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + const SimpleDevicePowerSink device_power_sink{battery_system, power_system_processor}; // Initializing vehicle traits const rmf_traffic::agv::VehicleTraits traits( @@ -295,6 +308,7 @@ SCENARIO("Testing Cleaning Request") rmf_traffic::time::to_seconds(trajectory.duration())); const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // TODO(YV): Determine the theoretical drain for this trajectory. REQUIRE(remaining_soc <= 1.0); } } From 51031ae5fcf45f5074c35aecdbb8df5ccc6f9976 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 13:32:28 +0800 Subject: [PATCH 103/128] Use RCLCPP_DEBUG for printing assignments and fix const correctness of current_task_end_state() --- .../rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 15 +++++++++------ .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 2 +- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 2 +- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index e0d3e4d58..6bf5b7759 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include namespace rmf_fleet_adapter { @@ -504,11 +504,12 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const double cost = task_planner->compute_cost(assignments); - // Display assignments for debugging - std::cout << "Cost: " << cost << std::endl; + // Display computed assignments for debugging + std::stringstream debug_stream; + debug_stream << "Cost: " << cost << std::endl; for (std::size_t i = 0; i < assignments.size(); ++i) { - std:: cout << "--Agent: " << i << std::endl; + debug_stream << "--Agent: " << i << std::endl; for (const auto& a : assignments[i]) { const auto& s = a.state(); @@ -516,13 +517,15 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const double start_seconds = a.deployment_time().time_since_epoch().count()/1e9; const rmf_traffic::Time finish_time = s.finish_time(); const double finish_seconds = finish_time.time_since_epoch().count()/1e9; - std::cout << " <" << a.request()->id() << ": " << request_seconds + debug_stream << " <" << a.request()->id() << ": " << request_seconds << ", " << start_seconds << ", "<< finish_seconds << ", " << 100* s.battery_soc() << "%>" << std::endl; } } - std::cout << " ----------------------" << std::endl; + debug_stream << " ----------------------" << std::endl; + + RCLCPP_DEBUG(node->get_logger(), "%s", debug_stream.str().c_str()); // Publish BidProposal rmf_task_msgs::msg::BidProposal bid_proposal; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 49e9d5f79..76ddb70c5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -214,7 +214,7 @@ RobotContext& RobotContext::maximum_delay( } //============================================================================== -rmf_task::agv::State& RobotContext::current_task_end_state() +const rmf_task::agv::State& RobotContext::current_task_end_state() const { return _current_task_end_state; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index be03bf753..409e44cf5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -121,7 +121,7 @@ class RobotContext /// Get a mutable reference to the state of this robot at the end of its // current task - rmf_task::agv::State& current_task_end_state(); + const rmf_task::agv::State& current_task_end_state() const; /// Get the state config of this robot const rmf_task::agv::StateConfig state_config() const; From 3beae9802bb5d8baa5559ef0bcef98fcc3cda408 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 14:01:50 +0800 Subject: [PATCH 104/128] Added TODO to fix request_delivery and request_loop functions in MockAdapter --- .../src/rmf_fleet_adapter/agv/test/MockAdapter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index 4ab99c92f..609b6cc8b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -141,12 +141,14 @@ void MockAdapter::stop() //============================================================================== void MockAdapter::request_delivery(const rmf_task_msgs::msg::Delivery& request) { + // TODO fix this function which is used in the test script test_Delivery // rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); } //============================================================================== void MockAdapter::request_loop(const rmf_task_msgs::msg::Loop& request) { + // TODO fix this function which is used in the test script test_Loop. // rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets); } From 17ea62b5c4929d32057fdad53b2508d13c7eedb6 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 18 Dec 2020 14:05:01 +0800 Subject: [PATCH 105/128] Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 6bf5b7759..20e91ccd9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -359,7 +359,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( { RCLCPP_ERROR( node->get_logger(), - "Required param [loop.num_loops] in TaskProfile is invalid." + "Required param [loop.num_loops: %d] in TaskProfile is invalid." "Rejecting BidNotice with task_id:[%s]" , id.c_str()); return; From 1bab0602152986ba9b50c689bf91f63685b67060 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 18 Dec 2020 14:05:39 +0800 Subject: [PATCH 106/128] Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 20e91ccd9..06c8684ad 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -360,7 +360,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_ERROR( node->get_logger(), "Required param [loop.num_loops: %d] in TaskProfile is invalid." - "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + "Rejecting BidNotice with task_id:[%s]" , loop.num_loops, id.c_str()); return; } From aaccae96d290575b77ec51b7cfa8a949c3f3067d Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 18 Dec 2020 14:07:42 +0800 Subject: [PATCH 107/128] Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 06c8684ad..35092dcab 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -625,6 +625,8 @@ std::size_t FleetUpdateHandle::Implementation::get_nearest_charger( for (const auto& wp : charging_waypoints) { const auto loc = graph.get_waypoint(wp).get_location(); + // TODO: Replace this with a planner call + // when the performance improvements are finished const double dist = (loc - p).norm(); if (dist < min_dist) { From 697775d18b0b4919c1deef5359ad572bf7213bb0 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 18 Dec 2020 15:12:43 +0800 Subject: [PATCH 108/128] Update rmf_task/include/rmf_task/agv/TaskPlanner.hpp Co-authored-by: Grey --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 8e454a1d7..90998d98e 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -130,7 +130,7 @@ class TaskPlanner // Get the request of this task rmf_task::ConstRequestPtr request() const; - // Get a const reference to the state + // Get a const reference to the predicted state at the end of the assignment const State& state() const; // Get the time when the robot begins executing From 96bab557d25e98250979322b63b7104ed00c43a3 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 18 Dec 2020 15:12:59 +0800 Subject: [PATCH 109/128] Update rmf_task_msgs/srv/GetTask.srv Co-authored-by: Grey --- rmf_task_msgs/srv/GetTask.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task_msgs/srv/GetTask.srv b/rmf_task_msgs/srv/GetTask.srv index ce6e1fbbf..fc28470a1 100644 --- a/rmf_task_msgs/srv/GetTask.srv +++ b/rmf_task_msgs/srv/GetTask.srv @@ -1,6 +1,6 @@ # Query list of submitted tasks | Get service call -# Whom that call this srv +# Identifier for who is requesting the service string requester # Input the generated task ID during submission From 13e61250644663289b0c90e44f2ae27822710d40 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 18 Dec 2020 15:13:11 +0800 Subject: [PATCH 110/128] Update rmf_task_msgs/srv/CancelTask.srv Co-authored-by: Grey --- rmf_task_msgs/srv/CancelTask.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task_msgs/srv/CancelTask.srv b/rmf_task_msgs/srv/CancelTask.srv index 9b3dd88aa..b96ef301e 100644 --- a/rmf_task_msgs/srv/CancelTask.srv +++ b/rmf_task_msgs/srv/CancelTask.srv @@ -1,6 +1,6 @@ # Submit Task | "POST" service call -# Whom that call this srv +# Identifier for who is requesting the service string requester # generated task ID by dispatcher node From db40f2abab10ae46a16ba96a9a706b0a0bb32546 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 15:41:04 +0800 Subject: [PATCH 111/128] Fixed CMakeLists.txt and header guards for rmf_task --- rmf_task/CMakeLists.txt | 6 ------ rmf_task/include/rmf_task/Estimate.hpp | 6 +++--- rmf_task/include/rmf_task/Request.hpp | 6 +++--- rmf_task/include/rmf_task/agv/State.hpp | 6 +++--- rmf_task/include/rmf_task/agv/StateConfig.hpp | 6 +++--- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 2 -- rmf_task/include/rmf_task/requests/ChargeBattery.hpp | 6 +++--- rmf_task/include/rmf_task/requests/Clean.hpp | 6 +++--- rmf_task/include/rmf_task/requests/Delivery.hpp | 6 +++--- rmf_task/include/rmf_task/requests/Loop.hpp | 6 +++--- 10 files changed, 24 insertions(+), 32 deletions(-) diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 3f67ee6f8..d5d40cb5c 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -18,10 +18,6 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") -# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") - - include(GNUInstallDirs) find_package(rmf_utils REQUIRED) @@ -47,7 +43,6 @@ add_library(rmf_task SHARED target_link_libraries(rmf_task PUBLIC rmf_battery::rmf_battery - PRIVATE ${rmf_dispenser_msgs_LIBRARIES} ) @@ -56,7 +51,6 @@ target_include_directories(rmf_task $ $ ${EIGEN3_INCLUDE_DIRS} - PRIVATE ${rmf_dispenser_msgs_INCLUDE_DIRS} ) diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index 20d720a5f..c27d0f5bb 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__ESTIMATE_HPP -#define INCLUDE__RMF_TASK__ESTIMATE_HPP +#ifndef RMF_TASK__ESTIMATE_HPP +#define RMF_TASK__ESTIMATE_HPP #include #include @@ -88,4 +88,4 @@ class EstimateCache } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__ESTIMATE_HPP +#endif // RMF_TASK__ESTIMATE_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 03eb533cb..edc7fde1c 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__TASK_HPP -#define INCLUDE__RMF_TASK__TASK_HPP +#ifndef RMF_TASK__TASK_HPP +#define RMF_TASK__TASK_HPP #include @@ -60,4 +60,4 @@ using ConstRequestPtr = std::shared_ptr; } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__TASK_HPP +#endif // RMF_TASK__REQUEST_HPP diff --git a/rmf_task/include/rmf_task/agv/State.hpp b/rmf_task/include/rmf_task/agv/State.hpp index fbfabf060..20c99bcfa 100644 --- a/rmf_task/include/rmf_task/agv/State.hpp +++ b/rmf_task/include/rmf_task/agv/State.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__AGV__STATE_HPP -#define INCLUDE__RMF_TASK__AGV__STATE_HPP +#ifndef RMF_TASK__AGV__STATE_HPP +#define RMF_TASK__AGV__STATE_HPP #include @@ -93,4 +93,4 @@ class State } // namespace agv } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__AGV__STATE_HPP +#endif // RMF_TASK__AGV__STATE_HPP diff --git a/rmf_task/include/rmf_task/agv/StateConfig.hpp b/rmf_task/include/rmf_task/agv/StateConfig.hpp index 94c7b2352..9f5f7c0c8 100644 --- a/rmf_task/include/rmf_task/agv/StateConfig.hpp +++ b/rmf_task/include/rmf_task/agv/StateConfig.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__AGV__STATECONFIG_HPP -#define INCLUDE__RMF_TASK__AGV__STATECONFIG_HPP +#ifndef RMF_TASK__AGV__STATECONFIG_HPP +#define RMF_TASK__AGV__STATECONFIG_HPP #include @@ -49,4 +49,4 @@ class StateConfig } // namespace agv } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__AGV__STATE_HPP +#endif // RMF_TASK__AGV__STATECONFIG_HPP diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 90998d98e..dcd0ca677 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -38,8 +38,6 @@ namespace rmf_task { namespace agv { - - //============================================================================== class TaskPlanner { diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index b94bae004..c664d5fa8 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP -#define INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP +#ifndef RMF_TASK__REQUESTS__CHARGEBATTERY_HPP +#define RMF_TASK__REQUESTS__CHARGEBATTERY_HPP #include @@ -76,4 +76,4 @@ using ConstChargeBatteryRequestPtr = std::shared_ptr; } // namespace requests } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__REQUESTS__CHARGEBATTERY_HPP +#endif // RMF_TASK__REQUESTS__CHARGEBATTERY_HPP diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index bfc35510d..bcfac431a 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP -#define INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP +#ifndef RMF_TASK__REQUESTS__CLEAN_HPP +#define RMF_TASK__REQUESTS__CLEAN_HPP #include #include @@ -83,4 +83,4 @@ using ConstCleanRequestPtr = std::shared_ptr; } // namespace requests } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP +#endif // RMF_TASK__REQUESTS__CLEAN_HPP diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index e5534a544..4d0b354d7 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP -#define INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP +#ifndef RMF_TASK__REQUESTS__DELIVERY_HPP +#define RMF_TASK__REQUESTS__DELIVERY_HPP #include #include @@ -93,4 +93,4 @@ using ConstDeliveryRequestPtr = std::shared_ptr; } // namespace requests } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__REQUESTS__DELIVERY_HPP +#endif // RMF_TASK__REQUESTS__DELIVERY_HPP diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index e9f4cf2c3..d67fcf2f4 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -15,8 +15,8 @@ * */ -#ifndef INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP -#define INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP +#ifndef RMF_TASK__REQUESTS__LOOP_HPP +#define RMF_TASK__REQUESTS__LOOP_HPP #include #include @@ -87,4 +87,4 @@ using ConstLoopRequestPtr = std::shared_ptr; } // namespace tasks } // namespace rmf_task -#endif // INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP +#endif // RMF_TASK__REQUESTS__LOOP_HPP From 4d28934176c28986170fa247488df67fbea3033f Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 15:59:08 +0800 Subject: [PATCH 112/128] Added setters for other parameters in TaskPlanner Configuration --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 16 +++++++- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 38 +++++++++++++++++-- 2 files changed, 48 insertions(+), 6 deletions(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index dcd0ca677..5d45702dd 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -83,16 +83,28 @@ class TaskPlanner rmf_battery::agv::BatterySystem& battery_system(); /// Set the battery_system - Configuration& battery_system(rmf_battery::agv::BatterySystem battery_system); + Configuration& battery_system( + rmf_battery::agv::BatterySystem battery_system); /// Get the motion sink std::shared_ptr motion_sink() const; + /// Set the motion_sink + Configuration& motion_sink( + std::shared_ptr motion_sink); + /// Get the ambient device sink std::shared_ptr ambient_sink() const; + /// Set the ambient device sink + Configuration& ambient_sink( + std::shared_ptr ambient_sink); + /// Get the planner - std::shared_ptr planner() const; + std::shared_ptr planner() const; + + /// Set the planner + Configuration& planner(std::shared_ptr); /// Get the filter type FilterType filter_type() const; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 73c4ed701..d924588a2 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -81,22 +81,52 @@ auto TaskPlanner::Configuration::battery_system( } //============================================================================== -std::shared_ptr TaskPlanner::Configuration::motion_sink() const +std::shared_ptr +TaskPlanner::Configuration::motion_sink() const { return _pimpl->motion_sink; } //============================================================================== -std::shared_ptr TaskPlanner::Configuration::ambient_sink() const +auto TaskPlanner::Configuration::motion_sink( + std::shared_ptr motion_sink) -> Configuration& +{ + if (motion_sink) + _pimpl->motion_sink = motion_sink; + return *this; +} + +//============================================================================== +std::shared_ptr +TaskPlanner::Configuration::ambient_sink() const { return _pimpl->ambient_sink; } //============================================================================== -std::shared_ptr TaskPlanner::Configuration::planner() const +auto TaskPlanner::Configuration::ambient_sink( + std::shared_ptr ambient_sink) -> Configuration& +{ + if (ambient_sink) + _pimpl->ambient_sink = ambient_sink; + return *this; +} + +//============================================================================== +std::shared_ptr +TaskPlanner::Configuration::planner() const { return _pimpl->planner; -} +} + +//============================================================================== +auto TaskPlanner::Configuration::planner( + std::shared_ptr planner) -> Configuration& +{ + if (planner) + _pimpl->planner = planner; + return *this; +} //============================================================================== TaskPlanner::FilterType TaskPlanner::Configuration::filter_type() const From 2ca073bd8071c65c5d55bb5fec5adc426ae69399 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 16:15:35 +0800 Subject: [PATCH 113/128] Removed FilterType from public API. Planner uses Hash type always. --- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 19 +-------- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 42 +++++++------------ 2 files changed, 18 insertions(+), 43 deletions(-) diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 5d45702dd..6b2282a32 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -42,15 +42,7 @@ namespace agv { class TaskPlanner { public: - - // The type of filter used for solving the task assignment problem - enum class FilterType - { - Passthrough, - Trie, - Hash - }; - + /// The Configuration class contains planning parameters that are immutable /// for each TaskPlanner instance and should not change in between plans. class Configuration @@ -76,8 +68,7 @@ class TaskPlanner rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr ambient_sink, - std::shared_ptr planner, - FilterType filter_type= FilterType::Hash); + std::shared_ptr planner); /// Get the battery system rmf_battery::agv::BatterySystem& battery_system(); @@ -106,12 +97,6 @@ class TaskPlanner /// Set the planner Configuration& planner(std::shared_ptr); - /// Get the filter type - FilterType filter_type() const; - - /// Set the filter type - Configuration& filter_type(FilterType filter_type); - class Implementation; private: diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d924588a2..8096b8f2f 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -34,7 +34,6 @@ namespace rmf_task { namespace agv { - //============================================================================== class TaskPlanner::Configuration::Implementation { @@ -44,23 +43,19 @@ class TaskPlanner::Configuration::Implementation std::shared_ptr motion_sink; std::shared_ptr ambient_sink; std::shared_ptr planner; - FilterType filter_type; - }; TaskPlanner::Configuration::Configuration( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr ambient_sink, - std::shared_ptr planner, - const FilterType filter_type) + std::shared_ptr planner) : _pimpl(rmf_utils::make_impl( Implementation{ battery_system, std::move(motion_sink), std::move(ambient_sink), - std::move(planner), - filter_type + std::move(planner) })) { // Do nothing @@ -128,20 +123,6 @@ auto TaskPlanner::Configuration::planner( return *this; } -//============================================================================== -TaskPlanner::FilterType TaskPlanner::Configuration::filter_type() const -{ - return _pimpl->filter_type; -} - -//============================================================================== -auto TaskPlanner::Configuration::filter_type( - TaskPlanner::FilterType filter_type) -> Configuration& -{ - _pimpl->filter_type = filter_type; - return *this; -} - //============================================================================== class TaskPlanner::Assignment::Implementation { @@ -591,12 +572,21 @@ class InvariantHeuristicQueue std::vector> _stacks; }; +// ============================================================================ +// The type of filter used for solving the task assignment problem +enum class FilterType +{ + Passthrough, + Trie, + Hash +}; + // ============================================================================ class Filter { public: - Filter(TaskPlanner::FilterType type, const std::size_t N_tasks) + Filter(FilterType type, const std::size_t N_tasks) : _type(type), _set(N_tasks, AssignmentHash(N_tasks)) { @@ -679,17 +669,17 @@ class Filter using Set = std::unordered_set; - TaskPlanner::FilterType _type; + FilterType _type; AgentTable _root; Set _set; }; bool Filter::ignore(const Node& node) { - if (_type == TaskPlanner::FilterType::Passthrough) + if (_type == FilterType::Passthrough) return false; - if (_type == TaskPlanner::FilterType::Hash) + if (_type == FilterType::Hash) return !_set.insert(node.assigned_tasks).second; bool new_node = false; @@ -1374,7 +1364,7 @@ class TaskPlanner::Implementation PriorityQueue priority_queue; priority_queue.push(std::move(initial_node)); - Filter filter{config->filter_type(), num_tasks}; + Filter filter{FilterType::Hash, num_tasks}; ConstNodePtr top = nullptr; while (!priority_queue.empty() && !(interrupter && interrupter())) From 49fdf4469866a1ec13eedc2f295e0cb0fbcfd760 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 16:59:51 +0800 Subject: [PATCH 114/128] Updated note on friction_coefficient --- rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp index 23a7f66ff..c433a2c9b 100644 --- a/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp +++ b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp @@ -39,9 +39,12 @@ class MechanicalSystem /// The moment of inertia of the robot along its yaw axis in kg.m^2 /// /// \param[in] friction_coefficient - /// The coefficient of kinetic friction measured at the wheels of the robot. - /// This value is used to compute the energy loss due to rotation of the - /// vehicle's wheels during locomotion. + /// The coefficient of kinetic friction or rolling resistance coefficient + /// measured at the wheels of the robot. This value is used to compute the + /// energy loss due to rotation of the vehicle's wheels during locomotion. + /// This value is the dimensionless constant Crr as described in the + /// reference below. + /// Ref: https://en.wikipedia.org/wiki/Rolling_resistance#Rolling_resistance_coefficient static std::optional make( double mass, double moment_of_inertia, From d3880be4738600ed8a62f9fd023732fcb6b670cc Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 18:37:30 +0800 Subject: [PATCH 115/128] Added documentation for requests and fixed comments --- rmf_task/include/rmf_task/requests/ChargeBattery.hpp | 1 + rmf_task/include/rmf_task/requests/Clean.hpp | 3 +++ rmf_task/include/rmf_task/requests/Delivery.hpp | 7 +++++++ rmf_task/include/rmf_task/requests/Loop.hpp | 7 +++++++ rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 2 -- rmf_task_msgs/srv/CancelTask.srv | 2 +- rmf_task_msgs/srv/SubmitTask.srv | 2 +- 7 files changed, 20 insertions(+), 4 deletions(-) diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index c664d5fa8..2618a16fb 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -59,6 +59,7 @@ class ChargeBattery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + /// Get the battery system in this request const rmf_battery::agv::BatterySystem& battery_system() const; /// Retrieve the charge soc which the battery will be charged upto diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index bcfac431a..d7e12b525 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -64,10 +64,13 @@ class Clean : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + /// Get the start waypoint in this request std::size_t start_waypoint() const; + /// Get the end waypoint in this request std::size_t end_waypoint() const; + /// Get the Start at the end of the cleaning trajectory from an initial Start rmf_traffic::agv::Planner::Start location_after_clean( rmf_traffic::agv::Planner::Start start) const; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 4d0b354d7..58b543460 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -69,16 +69,23 @@ class Delivery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + /// Get the pickup waypoint in this request std::size_t pickup_waypoint() const; + /// Get the name of the dispenser at the pickup waypoint const std::string& pickup_dispenser() const; + /// Get the dropoff waypoint in this request std::size_t dropoff_waypoint() const; + /// Get the name of the ingestor at the dropoff waypoint const std::string& dropoff_ingestor() const; + /// Get the list of dispenser request items in this request const std::vector& items() const; + /// Get the Start when the robot reaches the pickup_waypoint from an initial + /// start Start dropoff_start(const Start& start) const; class Implementation; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index d67fcf2f4..752740692 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -65,14 +65,21 @@ class Loop : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + /// Get the start waypoint of the loop in this request std::size_t start_waypoint() const; + /// Get the finish waypoint of the loop in this request std::size_t finish_waypoint() const; + /// Get the numbert of loops in this request std::size_t num_loops() const; + /// Get the Start when the robot reaches the start_waypoint from an initial + /// start Start loop_start(const Start& start) const; + /// Get the Start when the robot reaches the finish_waypoint from an initial + /// start Start loop_end(const Start& start) const; class Implementation; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 8096b8f2f..ea9dea7c8 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -684,8 +684,6 @@ bool Filter::ignore(const Node& node) bool new_node = false; - // TODO(MXG): Consider replacing this tree structure with a hash set - AgentTable* agent_table = &_root; std::size_t a = 0; std::size_t t = 0; diff --git a/rmf_task_msgs/srv/CancelTask.srv b/rmf_task_msgs/srv/CancelTask.srv index b96ef301e..a8cb16e9e 100644 --- a/rmf_task_msgs/srv/CancelTask.srv +++ b/rmf_task_msgs/srv/CancelTask.srv @@ -1,4 +1,4 @@ -# Submit Task | "POST" service call +# Cancel Task | "Delete" service call # Identifier for who is requesting the service string requester diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv index fc39faf92..7115aff51 100644 --- a/rmf_task_msgs/srv/SubmitTask.srv +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -1,6 +1,6 @@ # Submit Task | POST service call -# Whom that call this srv +# Identifier for who is requesting the service string requester # Desired start time of a task From 91c9c30ac820ddba65808da84ee3692496c1b239 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 18:41:15 +0800 Subject: [PATCH 116/128] Renamed GetTask to GetTaskList --- rmf_task_msgs/CMakeLists.txt | 3 +-- rmf_task_msgs/srv/GetTask.srv | 15 --------------- 2 files changed, 1 insertion(+), 17 deletions(-) delete mode 100644 rmf_task_msgs/srv/GetTask.srv diff --git a/rmf_task_msgs/CMakeLists.txt b/rmf_task_msgs/CMakeLists.txt index 0cc17c5b1..ed5a750cf 100644 --- a/rmf_task_msgs/CMakeLists.txt +++ b/rmf_task_msgs/CMakeLists.txt @@ -26,7 +26,6 @@ set(msg_files "msg/Tasks.msg" "msg/Loop.msg" "msg/Tow.msg" - # New task Msgs "msg/TaskType.msg" "msg/Clean.msg" "msg/TaskProfile.msg" @@ -38,7 +37,7 @@ set(msg_files set(srv_files "srv/SubmitTask.srv" "srv/CancelTask.srv" - "srv/GetTask.srv" + "srv/GetTaskList.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/rmf_task_msgs/srv/GetTask.srv b/rmf_task_msgs/srv/GetTask.srv deleted file mode 100644 index fc28470a1..000000000 --- a/rmf_task_msgs/srv/GetTask.srv +++ /dev/null @@ -1,15 +0,0 @@ -# Query list of submitted tasks | Get service call - -# Identifier for who is requesting the service -string requester - -# Input the generated task ID during submission -# if empty, provide all Submitted Tasks -string[] task_id - ---- - -bool success - -TaskSummary[] active_tasks -TaskSummary[] terminated_tasks From 0f57a1049b07dbd99c304bfc0cec0eb87d5e2588 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 18 Dec 2020 18:41:39 +0800 Subject: [PATCH 117/128] Renamed GetTask to GetTaskList --- rmf_task_msgs/srv/GetTaskList.srv | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 rmf_task_msgs/srv/GetTaskList.srv diff --git a/rmf_task_msgs/srv/GetTaskList.srv b/rmf_task_msgs/srv/GetTaskList.srv new file mode 100644 index 000000000..fc28470a1 --- /dev/null +++ b/rmf_task_msgs/srv/GetTaskList.srv @@ -0,0 +1,15 @@ +# Query list of submitted tasks | Get service call + +# Identifier for who is requesting the service +string requester + +# Input the generated task ID during submission +# if empty, provide all Submitted Tasks +string[] task_id + +--- + +bool success + +TaskSummary[] active_tasks +TaskSummary[] terminated_tasks From 1488362e0921ee88a79e042369c311777419de04 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 21 Dec 2020 11:16:19 +0800 Subject: [PATCH 118/128] Added set_charger_waypoint() to RobotUpdateHandle --- .../agv/RobotUpdateHandle.hpp | 5 +++++ .../agv/RobotUpdateHandle.cpp | 18 ++++++++++++++++++ .../agv/internal_RobotUpdateHandle.hpp | 2 +- 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 7e2411ad6..5ad2b7a29 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -88,6 +88,11 @@ class RobotUpdateHandle const double max_merge_lane_distance = 1.0, const double min_lane_length = 1e-8); + /// Set the waypoint where the charger for this robot is located. + /// If not specified, the nearest waypoint in the graph with the is_charger() + /// property will be assumed as the charger for this robot. + RobotUpdateHandle& set_charger_waypoint(const std::size_t charger_wp); + /// Update the current battery level of the robot by specifying its state of /// charge as a fraction of its total charge capacity void update_battery_soc(const double battery_soc); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 97c97ac57..52998896b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -162,6 +162,24 @@ void RobotUpdateHandle::update_position( } } +//============================================================================== +RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( + const std::size_t charger_wp) +{ + if (const auto context = _pimpl->get_context()) + { + auto end_state = context->current_task_end_state(); + end_state.charging_waypoint(charger_wp); + context->current_task_end_state(end_state); + RCLCPP_INFO( + context->node()->get_logger(), + "Charger waypoint for robot [%s] set to index [%d]", + context->name().c_str(), charger_wp); + } + + return *this; +} + //============================================================================== void RobotUpdateHandle::update_battery_soc(const double battery_soc) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index 28ff376a0..38716d6e8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -31,7 +31,7 @@ class RobotUpdateHandle::Implementation std::weak_ptr context; std::string name; - RobotUpdateHandle::Unstable unstable; + RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::Unstable(); bool reported_loss = false; static std::shared_ptr make(RobotContextPtr context) From 940b66faa98665e14fa12d59e88a9aa9fbd8c405 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 21 Dec 2020 13:34:51 +0800 Subject: [PATCH 119/128] Hashing in EstimateCache uses sum not xor --- rmf_task/src/rmf_task/Estimate.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 56eaa0281..025afd4da 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -76,7 +76,9 @@ class EstimateCache::Implementation { size_t operator()(const std::pair& p) const { - return std::hash()(p.first) ^ std::hash()(p.second); + const auto hash_first = std::hash()(p.first); + const auto hash_second = std::hash()(p.second); + return hash_first + hash_second + (hash_second << 10) ; } }; From 050be4f2f006a5b419d775b87007c1783062b78e Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 22 Dec 2020 11:47:17 +0800 Subject: [PATCH 120/128] Renamed StateConfig to Constraints --- .../src/rmf_fleet_adapter/TaskManager.cpp | 5 +- .../agv/FleetUpdateHandle.cpp | 18 +-- .../rmf_fleet_adapter/agv/RobotContext.cpp | 8 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 10 +- rmf_task/include/rmf_task/Request.hpp | 4 +- .../agv/{StateConfig.hpp => Constraints.hpp} | 12 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 6 +- .../rmf_task/requests/ChargeBattery.hpp | 2 +- rmf_task/include/rmf_task/requests/Clean.hpp | 2 +- .../include/rmf_task/requests/Delivery.hpp | 2 +- rmf_task/include/rmf_task/requests/Loop.hpp | 2 +- .../agv/{StateConfig.cpp => Constraints.cpp} | 10 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 80 ++++++------- .../src/rmf_task/requests/ChargeBattery.cpp | 4 +- rmf_task/src/rmf_task/requests/Clean.cpp | 10 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 10 +- rmf_task/src/rmf_task/requests/Loop.cpp | 10 +- ...t_StateConfig.cpp => test_Constraints.cpp} | 16 +-- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 106 +++++++++--------- 19 files changed, 159 insertions(+), 158 deletions(-) rename rmf_task/include/rmf_task/agv/{StateConfig.hpp => Constraints.hpp} (84%) rename rmf_task/src/rmf_task/agv/{StateConfig.cpp => Constraints.cpp} (84%) rename rmf_task/test/unit/agv/{test_StateConfig.cpp => test_Constraints.cpp} (67%) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 13854da46..e1928c6cd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -382,7 +382,8 @@ void TaskManager::retreat_to_charger() if (current_state.waypoint() == current_state.charging_waypoint()) return; - const double threshold_soc = _context->state_config().threshold_soc(); + const double threshold_soc = + _context->task_planning_constraints().threshold_soc(); const double retreat_threshold = 1.2 * threshold_soc; const double current_battery_soc = _context->current_battery_soc(); @@ -448,7 +449,7 @@ void TaskManager::retreat_to_charger() const auto finish = charging_request->estimate_finish( current_state, - _context->state_config(), + _context->task_planning_constraints(), estimate_cache); if (!finish) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 3ea54d3df..f95a6cef9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -427,7 +427,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Collate robot states and combine new requestptr with requestptr of // non-charging tasks in task manager queues std::vector states; - std::vector state_configs; + std::vector constraints_set; std::vector pending_requests; pending_requests.push_back(new_request); // Map robot index to name for BidProposal @@ -436,7 +436,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( for (const auto& t : task_managers) { states.push_back(t.second->expected_finish_state()); - state_configs.push_back(t.first->state_config()); + constraints_set.push_back(t.first->task_planning_constraints()); const auto requests = t.second->requests(); pending_requests.insert(pending_requests.end(), requests.begin(), requests.end()); @@ -454,7 +454,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const auto result = task_planner->optimal_plan( rmf_traffic_ros2::convert(node->now()), states, - state_configs, + constraints_set, pending_requests, nullptr); @@ -760,12 +760,12 @@ void FleetUpdateHandle::add_robot( fleet = shared_from_this()]( rmf_traffic::schedule::Participant participant) { - // TODO(YV) Get the battery % of this robot - const std::size_t charger = fleet->_pimpl->get_nearest_charger( + const std::size_t charger_wp = fleet->_pimpl->get_nearest_charger( start[0], fleet->_pimpl->charging_waypoints); - rmf_task::agv::State state = rmf_task::agv::State{start[0], charger, 1.0}; - rmf_task::agv::StateConfig state_config = rmf_task::agv::StateConfig{ - fleet->_pimpl->recharge_threshold}; + rmf_task::agv::State state = rmf_task::agv::State{ + start[0], charger_wp, 1.0}; + rmf_task::agv::Constraints task_planning_constraints = + rmf_task::agv::Constraints{fleet->_pimpl->recharge_threshold}; auto context = std::make_shared( RobotContext{ std::move(command), @@ -777,7 +777,7 @@ void FleetUpdateHandle::add_robot( fleet->_pimpl->worker, fleet->_pimpl->default_maximum_delay, state, - state_config, + task_planning_constraints, fleet->_pimpl->task_planner }); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 76ddb70c5..79cdcf0de 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -228,9 +228,9 @@ RobotContext& RobotContext::current_task_end_state( } //============================================================================== -const rmf_task::agv::StateConfig RobotContext::state_config() const +const rmf_task::agv::Constraints RobotContext::task_planning_constraints() const { - return _state_config; + return _task_planning_constraints; } //============================================================================== @@ -290,7 +290,7 @@ RobotContext::RobotContext( const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, rmf_task::agv::State state, - rmf_task::agv::StateConfig state_config, + rmf_task::agv::Constraints task_planning_constraints, std::shared_ptr task_planner) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), @@ -303,7 +303,7 @@ RobotContext::RobotContext( _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), _current_task_end_state(state), - _state_config(state_config), + _task_planning_constraints(task_planning_constraints), _task_planner(std::move(task_planner)) { _profile = std::make_shared( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 409e44cf5..e467742b8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include #include @@ -123,8 +123,8 @@ class RobotContext // current task const rmf_task::agv::State& current_task_end_state() const; - /// Get the state config of this robot - const rmf_task::agv::StateConfig state_config() const; + /// Get the task planning constraints of this robot + const rmf_task::agv::Constraints task_planning_constraints() const; /// Get the current battery state of charge double current_battery_soc() const; @@ -154,7 +154,7 @@ class RobotContext const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, rmf_task::agv::State state, - rmf_task::agv::StateConfig state_config, + rmf_task::agv::Constraints task_planning_constraints, std::shared_ptr task_planner); std::weak_ptr _command_handle; @@ -181,7 +181,7 @@ class RobotContext rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; rmf_task::agv::State _current_task_end_state; - rmf_task::agv::StateConfig _state_config; + rmf_task::agv::Constraints _task_planning_constraints; std::shared_ptr _task_planner; }; diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index edc7fde1c..a26b2412a 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -43,7 +43,7 @@ class Request /// time the robot has to wait before commencing the task virtual rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const = 0; /// Estimate the invariant component of the task's duration diff --git a/rmf_task/include/rmf_task/agv/StateConfig.hpp b/rmf_task/include/rmf_task/agv/Constraints.hpp similarity index 84% rename from rmf_task/include/rmf_task/agv/StateConfig.hpp rename to rmf_task/include/rmf_task/agv/Constraints.hpp index 9f5f7c0c8..11440946e 100644 --- a/rmf_task/include/rmf_task/agv/StateConfig.hpp +++ b/rmf_task/include/rmf_task/agv/Constraints.hpp @@ -15,15 +15,15 @@ * */ -#ifndef RMF_TASK__AGV__STATECONFIG_HPP -#define RMF_TASK__AGV__STATECONFIG_HPP +#ifndef RMF_TASK__AGV__CONSTRAINTS_HPP +#define RMF_TASK__AGV__CONSTRAINTS_HPP #include namespace rmf_task { namespace agv { -class StateConfig +class Constraints { public: @@ -32,14 +32,14 @@ class StateConfig /// \param[in] threshold_soc /// Minimum charge level the battery is allowed to deplete to. This /// value needs to be between 0.0 and 1.0. - StateConfig(double threshold_soc); + Constraints(double threshold_soc); /// Gets the battery state of charge threshold value. double threshold_soc() const; /// Sets a new battery state of charge threshold value. This value needs to be /// between 0.0 and 1.0. - StateConfig& threshold_soc(double threshold_soc); + Constraints& threshold_soc(double threshold_soc); class Implementation; private: @@ -49,4 +49,4 @@ class StateConfig } // namespace agv } // namespace rmf_task -#endif // RMF_TASK__AGV__STATECONFIG_HPP +#endif // RMF_TASK__AGV__CONSTRAINTS_HPP diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 6b2282a32..a0d9e6ac9 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -170,7 +170,7 @@ class TaskPlanner Result greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, - std::vector state_configs, + std::vector constraints_set, std::vector requests); /// Get the optimal planner based assignments for a set of initial states and @@ -182,7 +182,7 @@ class TaskPlanner Result optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, - std::vector state_configs, + std::vector constraints_set, std::vector requests, std::function interrupter); diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 2618a16fb..1d029fec8 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -52,7 +52,7 @@ class ChargeBattery : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index d7e12b525..f8c962e00 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -57,7 +57,7 @@ class Clean : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 58b543460..173e7f980 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -62,7 +62,7 @@ class Delivery : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 752740692..2c30e5d13 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -58,7 +58,7 @@ class Loop : public rmf_task::Request rmf_utils::optional estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const final; rmf_traffic::Duration invariant_duration() const final; diff --git a/rmf_task/src/rmf_task/agv/StateConfig.cpp b/rmf_task/src/rmf_task/agv/Constraints.cpp similarity index 84% rename from rmf_task/src/rmf_task/agv/StateConfig.cpp rename to rmf_task/src/rmf_task/agv/Constraints.cpp index e1ead152e..92222a126 100644 --- a/rmf_task/src/rmf_task/agv/StateConfig.cpp +++ b/rmf_task/src/rmf_task/agv/Constraints.cpp @@ -17,19 +17,19 @@ #include -#include +#include namespace rmf_task { namespace agv { -class StateConfig::Implementation +class Constraints::Implementation { public: double threshold_soc; }; -StateConfig::StateConfig(double threshold_soc) +Constraints::Constraints(double threshold_soc) : _pimpl(rmf_utils::make_impl( Implementation { @@ -41,12 +41,12 @@ StateConfig::StateConfig(double threshold_soc) "Battery State of Charge threshold needs be between 0.0 and 1.0."); } -double StateConfig::threshold_soc() const +double Constraints::threshold_soc() const { return _pimpl->threshold_soc; } -auto StateConfig::threshold_soc(double threshold_soc) -> StateConfig& +auto Constraints::threshold_soc(double threshold_soc) -> Constraints& { if (threshold_soc < 0.0 || threshold_soc > 1.0) throw std::invalid_argument( diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index ea9dea7c8..d1927ee3c 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -206,7 +206,7 @@ class Candidates static std::shared_ptr make( const std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, const rmf_task::Request& request, const rmf_task::requests::ChargeBattery& charge_battery_request, const std::shared_ptr estimate_cache, @@ -297,7 +297,7 @@ class Candidates std::shared_ptr Candidates::make( const std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, const rmf_task::Request& request, const rmf_task::requests::ChargeBattery& charge_battery_request, const std::shared_ptr estimate_cache, @@ -307,9 +307,9 @@ std::shared_ptr Candidates::make( for (std::size_t i = 0; i < initial_states.size(); ++i) { const auto& state = initial_states[i]; - const auto& state_config = state_configs[i]; + const auto& constraints = constraints_set[i]; const auto finish = request.estimate_finish( - state, state_config, estimate_cache); + state, constraints, estimate_cache); if (finish.has_value()) { initial_map.insert({ @@ -325,11 +325,11 @@ std::shared_ptr Candidates::make( { auto battery_estimate = charge_battery_request.estimate_finish( - state, state_config, estimate_cache); + state, constraints, estimate_cache); if (battery_estimate.has_value()) { auto new_finish = request.estimate_finish( - battery_estimate.value().finish_state(), state_config, estimate_cache); + battery_estimate.value().finish_state(), constraints, estimate_cache); if (new_finish.has_value()) { initial_map.insert( @@ -380,7 +380,7 @@ class PendingTask static std::shared_ptr make( std::vector& initial_states, - std::vector& state_configs, + std::vector& constraints_set, rmf_task::ConstRequestPtr request_, rmf_task::ConstRequestPtr charge_battery_request, std::shared_ptr estimate_cache, @@ -404,7 +404,7 @@ class PendingTask std::shared_ptr PendingTask::make( std::vector& initial_states, - std::vector& state_configs, + std::vector& constraints_set, rmf_task::ConstRequestPtr request_, rmf_task::ConstRequestPtr charge_battery_request, std::shared_ptr estimate_cache, @@ -414,7 +414,7 @@ std::shared_ptr PendingTask::make( auto battery_request = std::dynamic_pointer_cast< const rmf_task::requests::ChargeBattery>(charge_battery_request); - const auto candidates = Candidates::make(initial_states, state_configs, + const auto candidates = Candidates::make(initial_states, constraints_set, *request_, *battery_request, estimate_cache, error); if (!candidates) @@ -793,15 +793,15 @@ class TaskPlanner::Implementation Result complete_solve( rmf_traffic::Time time_now, std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, const std::vector& requests, const std::function interrupter, bool greedy) { - assert(initial_states.size() == state_configs.size()); + assert(initial_states.size() == constraints_set.size()); TaskPlannerError error; auto node = make_initial_node( - initial_states, state_configs, requests, time_now, error); + initial_states, constraints_set, requests, time_now, error); if (!node) return error; @@ -811,9 +811,9 @@ class TaskPlanner::Implementation while (node) { if (greedy) - node = greedy_solve(node, initial_states, state_configs, time_now); + node = greedy_solve(node, initial_states, constraints_set, time_now); else - node = solve(node, initial_states, state_configs, requests.size(), time_now, interrupter); + node = solve(node, initial_states, constraints_set, requests.size(), time_now, interrupter); if (!node) return {}; @@ -855,7 +855,7 @@ class TaskPlanner::Implementation } node = make_initial_node( - estimates, state_configs, new_tasks, time_now, error); + estimates, constraints_set, new_tasks, time_now, error); if (!node) return error; initial_states = estimates; @@ -937,7 +937,7 @@ class TaskPlanner::Implementation ConstNodePtr make_initial_node( std::vector initial_states, - std::vector state_configs, + std::vector constraints_set, std::vector requests, rmf_traffic::Time time_now, TaskPlannerError& error) @@ -955,7 +955,7 @@ class TaskPlanner::Implementation std::size_t internal_id = initial_node->get_available_internal_id(); const auto pending_task= PendingTask::make( initial_states, - state_configs, + constraints_set, request, charge_battery, estimate_cache, @@ -1028,11 +1028,11 @@ class TaskPlanner::Implementation const ConstNodePtr& parent, Filter* filter, rmf_traffic::Time time_now, - const std::vector& state_configs) + const std::vector& constraints_set) { const auto& entry = it->second; - const auto& state_config = state_configs[entry.candidate]; + const auto& constraints = constraints_set[entry.candidate]; if (parent->latest_time + segmentation_threshold < entry.wait_until) { @@ -1053,7 +1053,7 @@ class TaskPlanner::Implementation { auto charge_battery = make_charging_request(entry.previous_state.finish_time()); auto battery_estimate = charge_battery->estimate_finish( - entry.previous_state, state_config, estimate_cache); + entry.previous_state, constraints, estimate_cache); if (battery_estimate.has_value()) { assignments.push_back( @@ -1083,7 +1083,7 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish( - entry.state, state_config, estimate_cache); + entry.state, constraints, estimate_cache); if (finish.has_value()) { @@ -1098,13 +1098,13 @@ class TaskPlanner::Implementation { // TODO(YV): Revisit this strategy // auto battery_estimate = - // config->charge_battery_request()->estimate_finish(entry.state, state_config); + // config->charge_battery_request()->estimate_finish(entry.state, constraints); // if (battery_estimate.has_value()) // { // auto new_finish = // new_u.second.request->estimate_finish( // battery_estimate.value().finish_state(), - // state_config); + // constraints); // assert(new_finish.has_value()); // new_u.second.candidates.update_candidate( // entry.candidate, @@ -1126,7 +1126,7 @@ class TaskPlanner::Implementation { auto charge_battery = make_charging_request(entry.state.finish_time()); auto battery_estimate = charge_battery->estimate_finish( - entry.state, state_config, estimate_cache); + entry.state, constraints, estimate_cache); if (battery_estimate.has_value()) { new_node->assigned_tasks[entry.candidate].push_back( @@ -1141,7 +1141,7 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), - state_config, estimate_cache); + constraints, estimate_cache); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -1180,7 +1180,7 @@ class TaskPlanner::Implementation ConstNodePtr parent, const std::size_t agent, const std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, rmf_traffic::Time time_now) { auto new_node = std::make_shared(*parent); @@ -1198,7 +1198,7 @@ class TaskPlanner::Implementation auto charge_battery = make_charging_request(state.finish_time()); auto estimate = charge_battery->estimate_finish( - state, state_configs[agent], estimate_cache); + state, constraints_set[agent], estimate_cache); if (estimate.has_value()) { new_node->assigned_tasks[agent].push_back( @@ -1216,7 +1216,7 @@ class TaskPlanner::Implementation { const auto finish = new_u.second.request->estimate_finish(estimate.value().finish_state(), - state_configs[agent], estimate_cache); + constraints_set[agent], estimate_cache); if (finish.has_value()) { new_u.second.candidates.update_candidate( @@ -1243,7 +1243,7 @@ class TaskPlanner::Implementation ConstNodePtr greedy_solve( ConstNodePtr node, const std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, rmf_traffic::Time time_now) { while (!finished(*node)) @@ -1255,7 +1255,7 @@ class TaskPlanner::Implementation for (auto it = range.begin; it != range.end; ++it) { if (auto n = expand_candidate( - it, u, node, nullptr, time_now, state_configs)) + it, u, node, nullptr, time_now, constraints_set)) { if (!next_node || (n->cost_estimate < next_node->cost_estimate)) { @@ -1278,7 +1278,7 @@ class TaskPlanner::Implementation parent_node, it->second.candidate, initial_states, - state_configs, + constraints_set, time_now); if (new_charge_node) { @@ -1302,7 +1302,7 @@ class TaskPlanner::Implementation ConstNodePtr parent, Filter& filter, const std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, rmf_traffic::Time time_now) { std::vector new_nodes; @@ -1314,7 +1314,7 @@ class TaskPlanner::Implementation for (auto it = range.begin; it!= range.end; it++) { if (auto new_node = expand_candidate( - it, u, parent, &filter, time_now, state_configs)) + it, u, parent, &filter, time_now, constraints_set)) new_nodes.push_back(std::move(new_node)); } } @@ -1323,7 +1323,7 @@ class TaskPlanner::Implementation for (std::size_t i = 0; i < parent->assigned_tasks.size(); ++i) { if (auto new_node = expand_charger( - parent, i, initial_states, state_configs, time_now)) + parent, i, initial_states, constraints_set, time_now)) new_nodes.push_back(new_node); } @@ -1349,7 +1349,7 @@ class TaskPlanner::Implementation ConstNodePtr solve( ConstNodePtr initial_node, const std::vector& initial_states, - const std::vector& state_configs, + const std::vector& constraints_set, const std::size_t num_tasks, rmf_traffic::Time time_now, std::function interrupter) @@ -1380,7 +1380,7 @@ class TaskPlanner::Implementation // Apply possible actions to expand the node const auto new_nodes = expand( - top, filter, initial_states, state_configs, time_now); + top, filter, initial_states, constraints_set, time_now); // Add copies and with a newly assigned task to queue for (const auto&n : new_nodes) @@ -1407,13 +1407,13 @@ TaskPlanner::TaskPlanner(std::shared_ptr config) auto TaskPlanner::greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, - std::vector state_configs, + std::vector constraints_set, std::vector requests) -> Result { return _pimpl->complete_solve( time_now, initial_states, - state_configs, + constraints_set, requests, nullptr, true); @@ -1423,14 +1423,14 @@ auto TaskPlanner::greedy_plan( auto TaskPlanner::optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, - std::vector state_configs, + std::vector constraints_set, std::vector requests, std::function interrupter) -> Result { return _pimpl->complete_solve( time_now, initial_states, - state_configs, + constraints_set, requests, interrupter, false); diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index c4062b94e..6d2ebb1f6 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -100,7 +100,7 @@ std::string ChargeBattery::id() const //============================================================================== rmf_utils::optional ChargeBattery::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const { // Important to return nullopt if a charging task is not needed. In the task @@ -173,7 +173,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( } // If a robot cannot reach its charging dock given its initial battery soc - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; } diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index ece55075f..edff11ade 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -110,7 +110,7 @@ std::string Clean::id() const //============================================================================== rmf_utils::optional Clean::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ @@ -176,7 +176,7 @@ rmf_utils::optional Clean::estimate_finish( variant_battery_drain); } - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; } @@ -202,7 +202,7 @@ rmf_utils::optional Clean::estimate_finish( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_ambient; - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) { return rmf_utils::nullopt; } @@ -215,7 +215,7 @@ rmf_utils::optional Clean::estimate_finish( if (_pimpl->drain_battery) { battery_soc -= _pimpl->invariant_battery_drain; - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; // Check if the robot has enough charge to head back to nearest charger @@ -262,7 +262,7 @@ rmf_utils::optional Clean::estimate_finish( } } - if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + if (battery_soc - retreat_battery_drain <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; state.battery_soc(battery_soc); diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 5cc1377f5..94c0dbc29 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -127,7 +127,7 @@ std::string Delivery::id() const //============================================================================== rmf_utils::optional Delivery::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const { rmf_traffic::agv::Plan::Start final_plan_start{ @@ -193,7 +193,7 @@ rmf_utils::optional Delivery::estimate_finish( variant_battery_drain); } - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; } @@ -212,7 +212,7 @@ rmf_utils::optional Delivery::estimate_finish( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_device; - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) { return rmf_utils::nullopt; } @@ -225,7 +225,7 @@ rmf_utils::optional Delivery::estimate_finish( if (_pimpl->drain_battery) { battery_soc -= _pimpl->invariant_battery_drain; - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; // Check if the robot has enough charge to head back to nearest charger @@ -272,7 +272,7 @@ rmf_utils::optional Delivery::estimate_finish( } } - if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + if (battery_soc - retreat_battery_drain <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; state.battery_soc(battery_soc); diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index dc63d0589..6152e1079 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -129,7 +129,7 @@ std::string Loop::id() const //============================================================================== rmf_utils::optional Loop::estimate_finish( const agv::State& initial_state, - const agv::StateConfig& state_config, + const agv::Constraints& task_planning_constraints, const std::shared_ptr estimate_cache) const { @@ -186,7 +186,7 @@ rmf_utils::optional Loop::estimate_finish( variant_battery_drain); } - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; } @@ -206,7 +206,7 @@ rmf_utils::optional Loop::estimate_finish( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_device; - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) { return rmf_utils::nullopt; } @@ -221,7 +221,7 @@ rmf_utils::optional Loop::estimate_finish( if (_pimpl->drain_battery) { battery_soc -= _pimpl->invariant_battery_drain; - if (battery_soc <= state_config.threshold_soc()) + if (battery_soc <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; if ( _pimpl->finish_waypoint != initial_state.charging_waypoint()) @@ -268,7 +268,7 @@ rmf_utils::optional Loop::estimate_finish( retreat_battery_drain); } - if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + if (battery_soc - retreat_battery_drain <= task_planning_constraints.threshold_soc()) return rmf_utils::nullopt; } } diff --git a/rmf_task/test/unit/agv/test_StateConfig.cpp b/rmf_task/test/unit/agv/test_Constraints.cpp similarity index 67% rename from rmf_task/test/unit/agv/test_StateConfig.cpp rename to rmf_task/test/unit/agv/test_Constraints.cpp index 15f50a15c..f0d737bd5 100644 --- a/rmf_task/test/unit/agv/test_StateConfig.cpp +++ b/rmf_task/test/unit/agv/test_Constraints.cpp @@ -18,41 +18,41 @@ #include #include -#include +#include #include -SCENARIO("Robot State Configs") +SCENARIO("Test Constraints") { - std::unique_ptr basic_state_config; + std::unique_ptr constraints; WHEN("Minimum battery threshold") { CHECK_NOTHROW( - basic_state_config.reset(new rmf_task::agv::StateConfig{0.0})); + constraints.reset(new rmf_task::agv::Constraints{0.0})); } WHEN("Maximum battery threshold") { CHECK_NOTHROW( - basic_state_config.reset(new rmf_task::agv::StateConfig{1.0})); + constraints.reset(new rmf_task::agv::Constraints{1.0})); } WHEN("Half battery threshold") { CHECK_NOTHROW( - basic_state_config.reset(new rmf_task::agv::StateConfig{0.5})); + constraints.reset(new rmf_task::agv::Constraints{0.5})); } WHEN("Below minimum battery threshold") { CHECK_THROWS( - basic_state_config.reset(new rmf_task::agv::StateConfig{0.0 - 1e-4})); + constraints.reset(new rmf_task::agv::Constraints{0.0 - 1e-4})); } WHEN("Above maximum battery threshold") { CHECK_THROWS( - basic_state_config.reset(new rmf_task::agv::StateConfig{1.0 + 1e-4})); + constraints.reset(new rmf_task::agv::Constraints{1.0 + 1e-4})); } } diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 4a329ba1a..f16aa4cd6 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -184,10 +184,10 @@ SCENARIO("Grid World") rmf_task::agv::State{second_location, 2, 1.0} }; - std::vector state_configs = + std::vector task_planning_constraints = { - rmf_task::agv::StateConfig{0.2}, - rmf_task::agv::StateConfig{0.2} + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} }; std::vector requests = @@ -242,29 +242,29 @@ SCENARIO("Grid World") auto start_time = std::chrono::steady_clock::now(); const auto greedy_result = task_planner.greedy_plan( - now, initial_states, state_configs, requests); + now, initial_states, task_planning_constraints, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); - std::cout << "Greedy solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", *greedy_assignments, greedy_cost); + // std::cout << "Greedy solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Greedy", *greedy_assignments, greedy_cost); // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_result = task_planner.optimal_plan( - now, initial_states, state_configs, requests, nullptr); + now, initial_states, task_planning_constraints, requests, nullptr); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); - std::cout << "Optimal solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", *optimal_assignments, optimal_cost); + // std::cout << "Optimal solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } @@ -283,10 +283,10 @@ SCENARIO("Grid World") rmf_task::agv::State{second_location, 2, 1.0} }; - std::vector state_configs = + std::vector task_planning_constraints = { - rmf_task::agv::StateConfig{0.2}, - rmf_task::agv::StateConfig{0.2} + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} }; std::vector requests = @@ -445,29 +445,29 @@ SCENARIO("Grid World") auto start_time = std::chrono::steady_clock::now(); const auto greedy_result = task_planner.greedy_plan( - now, initial_states, state_configs, requests); + now, initial_states, task_planning_constraints, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); - std::cout << "Greedy solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", *greedy_assignments, greedy_cost); + // std::cout << "Greedy solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Greedy", *greedy_assignments, greedy_cost); // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_result = task_planner.optimal_plan( - now, initial_states, state_configs, requests, nullptr); + now, initial_states, task_planning_constraints, requests, nullptr); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); - std::cout << "Optimal solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", *optimal_assignments, optimal_cost); + // std::cout << "Optimal solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); } @@ -487,10 +487,10 @@ SCENARIO("Grid World") rmf_task::agv::State{second_location, 2, initial_soc} }; - std::vector state_configs = + std::vector task_planning_constraints = { - rmf_task::agv::StateConfig{0.2}, - rmf_task::agv::StateConfig{0.2} + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} }; std::vector requests = @@ -558,29 +558,29 @@ SCENARIO("Grid World") auto start_time = std::chrono::steady_clock::now(); const auto greedy_result = task_planner.greedy_plan( - now, initial_states, state_configs, requests); + now, initial_states, task_planning_constraints, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); - std::cout << "Greedy solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", *greedy_assignments, greedy_cost); + // std::cout << "Greedy solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Greedy", *greedy_assignments, greedy_cost); // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_result = task_planner.optimal_plan( - now, initial_states, state_configs, requests, nullptr); + now, initial_states, task_planning_constraints, requests, nullptr); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); - std::cout << "Optimal solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", *optimal_assignments, optimal_cost); + // std::cout << "Optimal solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); @@ -610,10 +610,10 @@ SCENARIO("Grid World") rmf_task::agv::State{second_location, 2, 1.0} }; - std::vector state_configs = + std::vector task_planning_constraints = { - rmf_task::agv::StateConfig{0.2}, - rmf_task::agv::StateConfig{0.2} + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} }; std::vector requests = @@ -772,29 +772,29 @@ SCENARIO("Grid World") auto start_time = std::chrono::steady_clock::now(); const auto greedy_result = task_planner.greedy_plan( - now, initial_states, state_configs, requests); + now, initial_states, task_planning_constraints, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE(greedy_assignments); const double greedy_cost = task_planner.compute_cost(*greedy_assignments); auto finish_time = std::chrono::steady_clock::now(); - std::cout << "Greedy solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Greedy", *greedy_assignments, greedy_cost); + // std::cout << "Greedy solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Greedy", *greedy_assignments, greedy_cost); // Create new TaskPlanner to reset cache so that measured run times // remain independent of one another task_planner = TaskPlanner(task_config); start_time = std::chrono::steady_clock::now(); const auto optimal_result = task_planner.optimal_plan( - now, initial_states, state_configs, requests, nullptr); + now, initial_states, task_planning_constraints, requests, nullptr); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); const double optimal_cost = task_planner.compute_cost(*optimal_assignments); finish_time = std::chrono::steady_clock::now(); - std::cout << "Optimal solution found in: " - << (finish_time - start_time).count() / 1e9 << std::endl; - display_solution("Optimal", *optimal_assignments, optimal_cost); + // std::cout << "Optimal solution found in: " + // << (finish_time - start_time).count() / 1e9 << std::endl; + // display_solution("Optimal", *optimal_assignments, optimal_cost); REQUIRE(optimal_cost <= greedy_cost); @@ -812,9 +812,9 @@ SCENARIO("Grid World") rmf_task::agv::State{first_location, 13, 1.0}, }; - std::vector state_configs = + std::vector task_planning_constraints = { - rmf_task::agv::StateConfig{0.2}, + rmf_task::agv::Constraints{0.2}, }; std::vector requests = @@ -840,7 +840,7 @@ SCENARIO("Grid World") TaskPlanner task_planner(task_config); const auto greedy_result = task_planner.greedy_plan( - now, initial_states, state_configs, requests); + now, initial_states, task_planning_constraints, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE_FALSE(greedy_assignments); @@ -852,7 +852,7 @@ SCENARIO("Grid World") // remain independent of one another task_planner = TaskPlanner(task_config); const auto optimal_result = task_planner.optimal_plan( - now, initial_states, state_configs, requests, nullptr); + now, initial_states, task_planning_constraints, requests, nullptr); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE_FALSE(optimal_assignments); @@ -873,9 +873,9 @@ SCENARIO("Grid World") rmf_task::agv::State{first_location, 13, 0.0}, }; - std::vector state_configs = + std::vector task_planning_constraints = { - rmf_task::agv::StateConfig{0.2}, + rmf_task::agv::Constraints{0.2}, }; std::vector requests = @@ -901,7 +901,7 @@ SCENARIO("Grid World") TaskPlanner task_planner(task_config); const auto greedy_result = task_planner.greedy_plan( - now, initial_states, state_configs, requests); + now, initial_states, task_planning_constraints, requests); const auto greedy_assignments = std::get_if< TaskPlanner::Assignments>(&greedy_result); REQUIRE_FALSE(greedy_assignments); @@ -913,7 +913,7 @@ SCENARIO("Grid World") // remain independent of one another task_planner = TaskPlanner(task_config); const auto optimal_result = task_planner.optimal_plan( - now, initial_states, state_configs, requests, nullptr); + now, initial_states, task_planning_constraints, requests, nullptr); const auto optimal_assignments = std::get_if< TaskPlanner::Assignments>(&optimal_result); REQUIRE_FALSE(optimal_assignments); From 29f4a28821b7240d0df08160b4d293bda60d257c Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 22 Dec 2020 17:45:00 +0800 Subject: [PATCH 121/128] Reworked PairHash --- rmf_task/include/rmf_task/Estimate.hpp | 7 +++- rmf_task/src/rmf_task/Estimate.cpp | 46 +++++++++++++++-------- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 5 ++- 3 files changed, 38 insertions(+), 20 deletions(-) diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index c27d0f5bb..84be63ab2 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -62,8 +62,11 @@ class Estimate class EstimateCache { public: - /// Constructs an empty EstimateCache - EstimateCache(); + /// Constructs an EstimateCache + /// + /// \param[in] N + /// The maximum number of waypoints in the navigation graph + EstimateCache(std::size_t N); /// Struct containing the estimated duration and charge required to travel between /// a waypoint pair. diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 025afd4da..e59e710f0 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -71,26 +71,40 @@ Estimate& Estimate::wait_until(rmf_traffic::Time new_wait_until) //============================================================================== class EstimateCache::Implementation { - public: - struct PairHash +public: + + Implementation(std::size_t N) + : _cache(N, PairHash(N)) + { + + } + + struct PairHash + { + PairHash(std::size_t N) + { + // We add 1 to N because + _shift = std::ceil(std::log2(N+1)); + } + + size_t operator()(const std::pair& p) const { - size_t operator()(const std::pair& p) const - { - const auto hash_first = std::hash()(p.first); - const auto hash_second = std::hash()(p.second); - return hash_first + hash_second + (hash_second << 10) ; - } - }; - - using Cache = std::unordered_map, - CacheElement, PairHash>; - Cache _cache; - std::shared_ptr _mutex = std::make_shared(); + return p.first + (p.second << _shift); + } + + std::size_t _shift; + }; + + using Cache = std::unordered_map, + CacheElement, PairHash>; + + Cache _cache; + std::shared_ptr _mutex = std::make_shared(); }; //============================================================================== -EstimateCache::EstimateCache() - : _pimpl(rmf_utils::make_impl(Implementation())) +EstimateCache::EstimateCache(std::size_t N) + : _pimpl(rmf_utils::make_impl(Implementation(N))) {} //============================================================================== diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d1927ee3c..6050317a3 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -1396,8 +1396,9 @@ class TaskPlanner::Implementation TaskPlanner::TaskPlanner(std::shared_ptr config) : _pimpl(rmf_utils::make_impl( Implementation{ - std::move(config), - std::make_shared() + config, + std::make_shared( + config->planner()->get_configuration().graph().num_waypoints()) })) { // Do nothing From f7e5d4b1cf7f7fcdaa96a287a397a6daf765d972 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 22 Dec 2020 18:01:06 +0800 Subject: [PATCH 122/128] Corrected shift in PairHash --- rmf_task/src/rmf_task/Estimate.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index e59e710f0..308581bd8 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -83,8 +83,7 @@ class EstimateCache::Implementation { PairHash(std::size_t N) { - // We add 1 to N because - _shift = std::ceil(std::log2(N+1)); + _shift = std::ceil(std::log2(N)); } size_t operator()(const std::pair& p) const From 5889f1df3a59cd8c271fc15ac74e7ca03b01d940 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 22 Dec 2020 18:08:09 +0800 Subject: [PATCH 123/128] Return task_planning_constraints by const ref --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 79cdcf0de..4428fc611 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -228,7 +228,7 @@ RobotContext& RobotContext::current_task_end_state( } //============================================================================== -const rmf_task::agv::Constraints RobotContext::task_planning_constraints() const +const rmf_task::agv::Constraints& RobotContext::task_planning_constraints() const { return _task_planning_constraints; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index e467742b8..2cfebcdff 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -124,7 +124,7 @@ class RobotContext const rmf_task::agv::State& current_task_end_state() const; /// Get the task planning constraints of this robot - const rmf_task::agv::Constraints task_planning_constraints() const; + const rmf_task::agv::Constraints& task_planning_constraints() const; /// Get the current battery state of charge double current_battery_soc() const; From 228db522b5e89f086799719af1bb3770c2b09f5d Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 4 Jan 2021 14:11:44 +0800 Subject: [PATCH 124/128] Updated Changelogs --- rmf_battery/CHANGELOG.rst | 17 +++++++++++++++++ rmf_fleet_adapter/CHANGELOG.rst | 7 +++++++ rmf_fleet_msgs/CHANGELOG.rst | 4 ++++ rmf_task/CHANGELOG.rst | 9 +++++++++ rmf_task_msgs/CHANGELOG.rst | 4 ++++ 5 files changed, 41 insertions(+) create mode 100644 rmf_battery/CHANGELOG.rst create mode 100644 rmf_task/CHANGELOG.rst diff --git a/rmf_battery/CHANGELOG.rst b/rmf_battery/CHANGELOG.rst new file mode 100644 index 000000000..5f2854bf0 --- /dev/null +++ b/rmf_battery/CHANGELOG.rst @@ -0,0 +1,17 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmf_battery +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2021-XX-XX) +------------------ +* Provides core `rmf_battery::agv` utilities + * `BatterySystem` - Describe battery properties of the vehicle + * `MechanicalSystem` - Describe mechanical properties of the vehicle + * `PowerSystem` - Describe a power system on-board the vehicle +* Provides `rmf_battery::agv::SimpleMotionPowerSink` + * Sample implementation of `rmf_battery::MotionPowerSink` + * Compute change in battery charge due to locomotion over a specified `rmf_traffic::Trajectory` +* Provides `rmf_traffic::agv::SimpleDevicePowerSink` + * Sample implementation of `rmf_battery::DevicePowerSink` + * Compute change in battery charge due to power systems over a specified duration +* Contributors: Yadunund diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index dd8251ee4..a6974bdc7 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2021-XX-XX) +------------------ +* Migrating to a task dispatcher framework: [#217](https://github.com/osrf/rmf_core/pull/217) + * The `rmf_fleet_adapter::agv` component interacts with a dispatcher node over topics with `rmf_task` prefix as specified in `rmf_fleet_adapter/StandardNames.hpp` + * Support for executing tasks at specified timepoints + * Support for `Loop`, `Delivery`, `Clean` and `ChargeBattery` tasks + 1.2.0 (2021-01-XX) ------------------ * Automatically publish fleet states from the fleet adapter API: [#232](https://github.com/osrf/rmf_core/pull/232) diff --git a/rmf_fleet_msgs/CHANGELOG.rst b/rmf_fleet_msgs/CHANGELOG.rst index 9088d98cb..b24161998 100644 --- a/rmf_fleet_msgs/CHANGELOG.rst +++ b/rmf_fleet_msgs/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package rmf_fleet_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2021-XX-XX) +------------------ +* Added Docking related messages: [#217](https://github.com/osrf/rmf_core/pull/217) + 1.2.0 (2021-01-XX) ------------------ * Adding pause command: [#226](https://github.com/osrf/rmf_core/pull/226) diff --git a/rmf_task/CHANGELOG.rst b/rmf_task/CHANGELOG.rst new file mode 100644 index 000000000..baafc875a --- /dev/null +++ b/rmf_task/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmf_task +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2021-XX-XX) +------------------ +* Provides `rmf_task::requests` to describe various task requests which may be used for allocation planning +* Provides `rmf_task::agv::TaskPlanner` object that can generate task allocation plans where a set of `rmf_task::requests` are optimally distributed across a set of agents + * `rmf_task::requests::ChargeBattery` requests are automatically injected into the allocation set where necessary diff --git a/rmf_task_msgs/CHANGELOG.rst b/rmf_task_msgs/CHANGELOG.rst index 6b4c19bd1..b32155155 100644 --- a/rmf_task_msgs/CHANGELOG.rst +++ b/rmf_task_msgs/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package rmf_task_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2021-XX-XX) +------------------ +* Added task dispatcher related messages and services: [#217](https://github.com/osrf/rmf_core/pull/217) + 1.1.0 (2020-09-24) ------------------ * Add Ingestor Messages (`#164 `_) From 2b73ed4a27d81a67c153de7fa2bd1fbc494fec49 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 5 Jan 2021 16:48:13 +0800 Subject: [PATCH 125/128] Feature/task dispatcher (#228) * prototyping a minimal dispatcher with new task_msgs * add new exec * skeleton for bidder and nomination * further complete bidder and dispatcher * create simple test exec with bidding pipeline * first version restructuring of new bidding mechanism * preliminary test run of bidder and dispatcher nodes * update codes * redesign task action interfaces and test out task action interactions * restructure rmf tasks dir and enhance task tracking on dispatcher * add simple test on bidding * refactor and define dispatchernode api * halfway game thru restructuring of code * expose dispatcher api and change action msg fields * dispatcher lib api * continuation of work on restucturing dispatcher api * update msg definitions and apis * adding srv msg, basic integration test * cleanups and further complete dispatcher node * lint, use TaskSummary as TaskStatus * minor changes based on comments * update msgs for dispatcher * Update dispatcher api * extend impl for dispatcher, and add dockerfile * sequential bidding and further refactoring, then test * latest minor fix on UB * merge with new msgs * cleanup diff * add loop type * receiving stray task summarry msg * merge with dispatcher-demo and rm charging task from cache on every reassignment * merge latest master on traffic light * refactoring actino task templates * refactor and cleanups again * remove charge battery id gen * first review cleanup iteration * more cleanups * remove template action * further refactoring * fix compilations * con't refactor * create TaskDescription.Msg in TaskProfile * append task_description and update task_id gen * reorg apis and add internal_Auctioneer * fix issue on ignoring stray taskstatus * change default eval and update cancel task impl * switch to std::optional * remove max limit terminated tasks according to submission time * refactor headers * fix bug on unusual short bid duration --- Dockerfile | 23 + rmf_fleet_adapter/src/full_control/main.cpp | 2 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 2 +- .../agv/FleetUpdateHandle.cpp | 14 +- rmf_task_msgs/CMakeLists.txt | 1 + rmf_task_msgs/msg/TaskDescription.msg | 13 + rmf_task_msgs/msg/TaskProfile.msg | 15 +- rmf_task_msgs/srv/SubmitTask.srv | 12 +- rmf_task_ros2/CMakeLists.txt | 95 ++++ .../include/rmf_task_ros2/Dispatcher.hpp | 141 ++++++ .../include/rmf_task_ros2/StandardNames.hpp | 38 ++ .../include/rmf_task_ros2/TaskStatus.hpp | 68 +++ .../rmf_task_ros2/bidding/Auctioneer.hpp | 119 +++++ .../rmf_task_ros2/bidding/MinimalBidder.hpp | 87 ++++ .../rmf_task_ros2/bidding/Submission.hpp | 44 ++ rmf_task_ros2/package.xml | 26 ++ rmf_task_ros2/src/dispatcher_node/main.cpp | 34 ++ rmf_task_ros2/src/mock_bidder/main.cpp | 119 +++++ .../src/rmf_task_ros2/Dispatcher.cpp | 440 ++++++++++++++++++ .../src/rmf_task_ros2/TaskStatus.cpp | 60 +++ .../src/rmf_task_ros2/action/Client.cpp | 179 +++++++ .../src/rmf_task_ros2/action/Client.hpp | 108 +++++ .../src/rmf_task_ros2/action/Server.cpp | 97 ++++ .../src/rmf_task_ros2/action/Server.hpp | 91 ++++ .../src/rmf_task_ros2/bidding/Auctioneer.cpp | 253 ++++++++++ .../rmf_task_ros2/bidding/MinimalBidder.cpp | 132 ++++++ .../bidding/internal_Auctioneer.hpp | 94 ++++ .../test/action/test_ActionInterface.cpp | 187 ++++++++ rmf_task_ros2/test/bidding/test_Evaluator.cpp | 116 +++++ rmf_task_ros2/test/bidding/test_SelfBid.cpp | 156 +++++++ .../test/dispatcher/test_Dispatcher.cpp | 242 ++++++++++ rmf_task_ros2/test/main.cpp | 21 + 32 files changed, 2997 insertions(+), 32 deletions(-) create mode 100644 Dockerfile create mode 100644 rmf_task_msgs/msg/TaskDescription.msg create mode 100644 rmf_task_ros2/CMakeLists.txt create mode 100644 rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp create mode 100644 rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp create mode 100644 rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp create mode 100644 rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp create mode 100644 rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp create mode 100644 rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp create mode 100644 rmf_task_ros2/package.xml create mode 100644 rmf_task_ros2/src/dispatcher_node/main.cpp create mode 100644 rmf_task_ros2/src/mock_bidder/main.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp create mode 100644 rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp create mode 100644 rmf_task_ros2/test/action/test_ActionInterface.cpp create mode 100644 rmf_task_ros2/test/bidding/test_Evaluator.cpp create mode 100644 rmf_task_ros2/test/bidding/test_SelfBid.cpp create mode 100644 rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp create mode 100644 rmf_task_ros2/test/main.cpp diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 000000000..cc9032378 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,23 @@ +ARG TAG="foxy-ros-base-focal" +FROM ros:$TAG + +ENV HOME /home/ws_rmf/ + +RUN apt-get update && apt-get install -y g++-8 \ + ros-foxy-rmw-cyclonedds-cpp +RUN mkdir -p /home/ws_rmf + +WORKDIR /home/ws_rmf/ +COPY . src +RUN rosdep update +RUN rosdep install --from-paths src --ignore-src --rosdistro foxy -yr + +RUN /ros_entrypoint.sh \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RELEASE && \ + sed -i '$isource "/home/ws_rmf/install/setup.bash"' /ros_entrypoint.sh && \ + rm -rf build devel src + +# todo: should have a multistage build + +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 55e91b316..71264415b 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -614,7 +614,7 @@ std::shared_ptr make_fleet( connections->fleet->accept_task_requests( [task_types](const rmf_task_msgs::msg::TaskProfile& msg) { - if (task_types.find(msg.task_type.type) != task_types.end()) + if (task_types.find(msg.description.task_type.type) != task_types.end()) return true; return false; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index e1928c6cd..69e466148 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -244,7 +244,7 @@ void TaskManager::set_queue( msg.state = msg.STATE_QUEUED; msg.robot_name = _context->name(); msg.fleet_name = _context->description().owner(); - msg.task_profile.task_type = task_type_msg; + msg.task_profile.description.task_type = task_type_msg; msg.start_time = rmf_traffic_ros2::convert( _queue.back()->deployment_time()); msg.start_time = rmf_traffic_ros2::convert( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index f95a6cef9..8670e71d4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -146,8 +146,9 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Determine task type and convert to request pointer rmf_task::ConstRequestPtr new_request = nullptr; const auto& task_profile = msg->task_profile; - const auto& task_type = task_profile.task_type; - const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.start_time); + const auto& task_type = task_profile.description.task_type; + const rmf_traffic::Time start_time = + rmf_traffic_ros2::convert(task_profile.description.start_time); // TODO (YV) get rid of ID field in RequestPtr std::string id = msg->task_profile.task_id; const auto& graph = planner->get_configuration().graph(); @@ -155,7 +156,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Process Cleaning task if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) { - if (task_profile.clean.start_waypoint.empty()) + if (task_profile.description.clean.start_waypoint.empty()) { RCLCPP_ERROR( node->get_logger(), @@ -166,7 +167,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } // Check for valid start waypoint - const std::string start_wp_name = task_profile.clean.start_waypoint; + const std::string start_wp_name = + task_profile.description.clean.start_waypoint; const auto start_wp = graph.find_waypoint(start_wp_name); if (!start_wp) { @@ -245,7 +247,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { - const auto& delivery = task_profile.delivery; + const auto& delivery = task_profile.description.delivery; if (delivery.pickup_place_name.empty()) { RCLCPP_ERROR( @@ -340,7 +342,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { - const auto& loop = task_profile.loop; + const auto& loop = task_profile.description.loop; if (loop.start_name.empty()) { RCLCPP_ERROR( diff --git a/rmf_task_msgs/CMakeLists.txt b/rmf_task_msgs/CMakeLists.txt index ed5a750cf..90a1d44ae 100644 --- a/rmf_task_msgs/CMakeLists.txt +++ b/rmf_task_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(msg_files "msg/Behavior.msg" "msg/BehaviorParameter.msg" "msg/Station.msg" + "msg/TaskDescription.msg" "msg/TaskSummary.msg" "msg/Tasks.msg" "msg/Loop.msg" diff --git a/rmf_task_msgs/msg/TaskDescription.msg b/rmf_task_msgs/msg/TaskDescription.msg new file mode 100644 index 000000000..4f6cb8bf7 --- /dev/null +++ b/rmf_task_msgs/msg/TaskDescription.msg @@ -0,0 +1,13 @@ +# Desired start time of a task +builtin_interfaces/Time start_time + +# Task type +TaskType task_type + +# Task definitions +Station station +Loop loop +Delivery delivery +# Charge charge +Clean clean +# Patrol patrol diff --git a/rmf_task_msgs/msg/TaskProfile.msg b/rmf_task_msgs/msg/TaskProfile.msg index 571474f2d..7c75dd5a8 100644 --- a/rmf_task_msgs/msg/TaskProfile.msg +++ b/rmf_task_msgs/msg/TaskProfile.msg @@ -4,17 +4,4 @@ string task_id # Task submission time builtin_interfaces/Time submission_time -# Desired start time of a task -builtin_interfaces/Time start_time - -# Task type -TaskType task_type - -# Task definitions -Station station -Loop loop -Delivery delivery -# Charge charge -Clean clean -# Patrol patrol - +TaskDescription description diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv index 7115aff51..08d048e87 100644 --- a/rmf_task_msgs/srv/SubmitTask.srv +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -6,16 +6,8 @@ string requester # Desired start time of a task builtin_interfaces/Time start_time -# Task type -TaskType task_type - -# Task definitions -Station station -Loop loop -Delivery delivery -# Charge charge -Clean clean -# Patrol patrol +# desciption of task +TaskDescription description # fleet selection evaluator uint8 evaluator diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt new file mode 100644 index 000000000..208358372 --- /dev/null +++ b/rmf_task_ros2/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 3.5) + +project(rmf_task_ros2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +include(GNUInstallDirs) + +find_package(ament_cmake REQUIRED) +find_package(rmf_traffic REQUIRED) +find_package(rmf_traffic_ros2 REQUIRED) +find_package(rmf_task_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) + +file(GLOB_RECURSE core_lib_srcs "src/rmf_task_ros2/*.cpp") +add_library(rmf_task_ros2 SHARED ${core_lib_srcs}) + +target_link_libraries(rmf_task_ros2 + PUBLIC + rmf_traffic::rmf_traffic + rmf_traffic_ros2::rmf_traffic_ros2 + ${rmf_task_msgs_LIBRARIES} + ${rclcpp_LIBRARIES} +) + +target_include_directories(rmf_task_ros2 + PUBLIC + $ + $ + ${rmf_traffic_ros2_INCLUDE_DIRS} + ${rmf_task_msgs_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} +) + +ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET) +ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp) + +#=============================================================================== + +find_package(ament_cmake_catch2 QUIET) +if(BUILD_TESTING AND ament_cmake_catch2_FOUND) + file(GLOB_RECURSE unit_test_srcs "test/*.cpp") + + ament_add_catch2( + test_rmf_task_ros2 test/main.cpp ${unit_test_srcs} + TIMEOUT 300) + target_link_libraries(test_rmf_task_ros2 + rmf_task_ros2 + rmf_traffic::rmf_traffic + rmf_traffic_ros2::rmf_traffic_ros2 + ) + + target_include_directories(test_rmf_task_ros2 + PRIVATE + $ + ) +endif() + +#=============================================================================== + +add_executable(rmf_bidder_node + src/mock_bidder/main.cpp +) +target_link_libraries(rmf_bidder_node PUBLIC rmf_task_ros2) + +#=============================================================================== + +add_executable(rmf_task_dispatcher + src/dispatcher_node/main.cpp +) +target_link_libraries(rmf_task_dispatcher PUBLIC rmf_task_ros2) + +#=============================================================================== +install( + DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install( + TARGETS rmf_task_ros2 rmf_task_dispatcher rmf_bidder_node + EXPORT rmf_task_ros2 + RUNTIME DESTINATION lib/rmf_task_ros2 + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +ament_package() diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp new file mode 100644 index 000000000..f01ebf656 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -0,0 +1,141 @@ +/* + * 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_ROS2__DISPATCHER_HPP +#define RMF_TASK_ROS2__DISPATCHER_HPP + +#include +#include +#include +#include + +#include +#include + + +namespace rmf_task_ros2 { + +//============================================================================== +/// This dispatcher class holds an instance which handles the dispatching of +/// tasks to all downstream RMF fleet adapters. +class Dispatcher : public std::enable_shared_from_this +{ +public: + using DispatchTasks = std::unordered_map; + using TaskDescription = rmf_task_msgs::msg::TaskDescription; + + /// Initialize an rclcpp context and make an dispatcher instance. This will + /// instantiate an rclcpp::Node, a task dispatcher node. Dispatcher node will + /// allow you to dispatch submitted task to the best fleet/robot within RMF. + /// + /// \param[in] dispatcher_node_name + /// The ROS 2 node to manage the Dispatching of Task + /// + /// \sa init_and_make_node() + static std::shared_ptr init_and_make_node( + const std::string dispatcher_node_name); + + /// Similarly this will init the dispatcher, but you will also need to init + /// rclcpp via rclcpp::init(~). + /// + /// \param[in] dispatcher_node_name + /// The ROS 2 node to manage the Dispatching of Task + /// + /// \sa make_node() + static std::shared_ptr make_node( + const std::string dispatcher_node_name); + + /// Create a dispatcher by providing the ros2 node + /// + /// \param[in] node + /// ROS 2 node instance + /// + /// \sa make() + static std::shared_ptr make( + const std::shared_ptr& node); + + /// Submit task to dispatcher node. Calling this function will immediately + /// trigger the bidding process, then the task "action". Once submmitted, + /// Task State will be in 'Pending' State, till the task is awarded to a fleet + /// then the state will turn to 'Queued' + /// + /// \param [in] task_description + /// Submit a task to dispatch + /// + /// \return task_id + /// self-generated task_id, nullopt is submit task failed + std::optional submit_task( + const TaskDescription& task_description); + + /// Cancel an active task which was previously submitted to Dispatcher. This + /// will terminate the task with a State of: `Canceled`. If a task is + /// `Queued` or `Executing`, this function will send a cancel req to + /// the respective fleet adapter. It is the responsibility of the fleet adapter + /// to make sure it cancels the task internally. + /// + /// \param [in] task_id + /// Task to cancel + /// + /// \return true if success + bool cancel_task(const TaskID& task_id); + + /// Check the state of a submited task. It can be either active or terminated + /// + /// \param [in] task_id + /// task_id obtained from `submit_task()` + /// + /// \return State of the task, nullopt if task is not available + const rmf_utils::optional get_task_state( + const TaskID& task_id) const; + + /// Get a mutable ref of active tasks map list handled by dispatcher + const DispatchTasks& active_tasks() const; + + /// Get a mutable ref of terminated tasks map list + const DispatchTasks& terminated_tasks() const; + + using StatusCallback = std::function; + + /// Trigger this callback when a task status is changed. This will return the + /// Changed task status. + /// + /// \param [in] callback function + void on_change(StatusCallback on_change_fn); + + /// Change the default evaluator to a custom evaluator, which is used by + /// bidding auctioneer. Default evaluator is: `LeastFleetDiffCostEvaluator` + /// + /// \param [in] evaluator + /// evaluator used to select the best bid from fleets + void evaluator(std::shared_ptr evaluator); + + /// Get the rclcpp::Node that this dispatcher will be using for communication. + std::shared_ptr node(); + + /// spin dispatcher node + void spin(); + + class Implementation; + +private: + Dispatcher(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__DISPATCHER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp new file mode 100644 index 000000000..f45a95044 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp @@ -0,0 +1,38 @@ +/* + * 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_ROS2__STANDARDNAMES_HPP +#define RMF_TASK_ROS2__STANDARDNAMES_HPP + +#include + +namespace rmf_task_ros2 { + +const std::string Prefix = "rmf_task/"; +const std::string BidNoticeTopicName = Prefix + "bid_notice"; +const std::string BidProposalTopicName = Prefix + "bid_proposal"; + +const std::string SubmitTaskSrvName = "submit_task"; +const std::string CancelTaskSrvName = "cancel_task"; +const std::string GetTaskListSrvName = "get_tasks"; + +const std::string TaskRequestTopicName = Prefix + "dispatch_request"; +const std::string TaskStatusTopicName = "task_summaries"; + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__STANDARDNAMES_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp new file mode 100644 index 000000000..a8f96fcd6 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp @@ -0,0 +1,68 @@ +/* + * 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_ROS2__TASK_STATUS_HPP +#define RMF_TASK_ROS2__TASK_STATUS_HPP + +#include +#include +#include +#include + +namespace rmf_task_ros2 { + +//============================================================================== +using TaskProfile = rmf_task_msgs::msg::TaskProfile; +using StatusMsg = rmf_task_msgs::msg::TaskSummary; +using TaskID = std::string; + +//============================================================================== +/// \note TaskStatus struct is based on TaskSummary.msg +struct TaskStatus +{ + enum class State : uint8_t + { + Queued = StatusMsg::STATE_QUEUED, + Executing = StatusMsg::STATE_ACTIVE, + Completed = StatusMsg::STATE_COMPLETED, + Failed = StatusMsg::STATE_FAILED, + Canceled = StatusMsg::STATE_CANCELED, + Pending = StatusMsg::STATE_PENDING + }; + + std::string fleet_name; + TaskProfile task_profile; + rmf_traffic::Time start_time; + rmf_traffic::Time end_time; + std::string robot_name; + std::string status; // verbose msg + State state = State::Pending; // default + + bool is_terminated() const; +}; + +using TaskStatusPtr = std::shared_ptr; + +// ============================================================================== +TaskStatus convert_status(const StatusMsg& from); + +// ============================================================================== +StatusMsg convert_status(const TaskStatus& from); + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__TASK_STATUS_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp new file mode 100644 index 000000000..4b9b5a87d --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp @@ -0,0 +1,119 @@ +/* + * 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_ROS2__BIDDING__AUCTIONEER_HPP +#define RMF_TASK_ROS2__BIDDING__AUCTIONEER_HPP + +#include +#include +#include + +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +/// The Auctioneer class is responsible for initiating and mediating the bidding +/// process during task dispatching. Hence, this class instance is solely used +/// within the Dispatcher class +class Auctioneer : public std::enable_shared_from_this +{ +public: + /// Callback which will provide the winner when a bid is concluded + /// + /// \param[in] task_id + /// bidding task id + /// + /// \param[in] winner + /// single winner from all submissions. nullopt if non + using BiddingResultCallback = + std::function winner)>; + + /// Create an instance of the Auctioneer. This instance will handle all + /// the task dispatching bidding mechanism. A default evaluator is used. + /// + /// \param[in] node + /// ros2 node which will manage the bidding + /// + /// \param[in] result_callback + /// This callback fn will be called when a bidding result is concluded + /// + /// \sa make() + static std::shared_ptr make( + const std::shared_ptr& node, + BiddingResultCallback result_callback); + + /// Start a bidding process by provide a bidding task. Note the each + /// bidding process is conducted sequentially + /// + /// \param[in] bid_notice + /// bidding task, task which will call for bid + void start_bidding(const BidNotice& bid_notice); + + /// A pure abstract interface class for the auctioneer to choose the best + /// choosing the best submissions. + class Evaluator + { + public: + + /// Given a list of submissions, choose the one that is the "best". It is + /// up to the implementation of the Evaluator to decide how to rank. + virtual std::size_t choose(const Submissions& submissions) const = 0; + + virtual ~Evaluator() = default; + }; + + /// Provide a custom evaluator which will be used to choose the best bid + /// If no selection is given, Default is: LeastFleetDiffCostEvaluator + /// + /// \param[in] evaluator + void select_evaluator(std::shared_ptr evaluator); + + class Implementation; + +private: + Auctioneer(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +//============================================================================== +class LeastFleetDiffCostEvaluator : public Auctioneer::Evaluator +{ +public: + std::size_t choose(const Submissions& submissions) const final; +}; + +//============================================================================== +class LeastFleetCostEvaluator : public Auctioneer::Evaluator +{ +public: + std::size_t choose(const Submissions& submissions) const final; +}; + +//============================================================================== +class QuickestFinishEvaluator : public Auctioneer::Evaluator +{ +public: + std::size_t choose(const Submissions& submissions) const final; +}; + +} // namespace bidding +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__BIDDING__AUCTIONEER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp new file mode 100644 index 000000000..ef53db6fc --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp @@ -0,0 +1,87 @@ +/* + * 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_ROS2__BIDDING__MINIMALBIDDER_HPP +#define RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP + +#include + +#include +#include + +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +class MinimalBidder +{ +public: + /// Type of Task in RMF + using TaskTypeMsg = rmf_task_msgs::msg::TaskType; + enum class TaskType + { + Station = TaskTypeMsg::TYPE_STATION, + Loop = TaskTypeMsg::TYPE_LOOP, + Delivery = TaskTypeMsg::TYPE_DELIVERY, + ChargeBattery = TaskTypeMsg::TYPE_CHARGE_BATTERY, + Clean = TaskTypeMsg::TYPE_CLEAN, + Patrol = TaskTypeMsg::TYPE_PATROL + }; + + /// Callback function when a bid notice is received from the autioneer + /// + /// \param[in] notice + /// bid notice msg + /// + /// \return submission + /// Estimates of a task. This submission is used by dispatcher for eval + using ParseSubmissionCallback = + std::function; + + + /// Create a bidder to bid for incoming task requests from Task Dispatcher + /// + /// \param[in] node + /// ROS 2 node instance + /// + /// \param[in] fleet_name + /// Name of the bidder + /// + /// \param[in] valid_task_types + /// A list of valid tasks types which are supported by the bidder + /// + /// \param[in] submission_cb + /// fn which is used to provide a bid submission during a call for bid + static std::shared_ptr make( + const std::shared_ptr& node, + const std::string& fleet_name, + const std::unordered_set& valid_task_types, + ParseSubmissionCallback submission_cb); + + class Implementation; + +private: + MinimalBidder(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +} // namespace bidding +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp new file mode 100644 index 000000000..b9b8cc813 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp @@ -0,0 +1,44 @@ +/* + * 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_ROS2__BIDDING__SUBMISSION_HPP +#define RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP + +#include +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +struct Submission +{ + std::string fleet_name; + std::string robot_name; + double prev_cost = 0.0; + double new_cost = std::numeric_limits::max(); + rmf_traffic::Time finish_time; +}; + +//============================================================================== +using Submissions = std::vector; +using BidNotice = rmf_task_msgs::msg::BidNotice; + +} // namespace bidding +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml new file mode 100644 index 000000000..268800c53 --- /dev/null +++ b/rmf_task_ros2/package.xml @@ -0,0 +1,26 @@ + + + + rmf_task_ros2 + 1.1.0 + A package managing the dispatching of tasks in RMF system. + youliang + Apache License 2.0 + + ament_cmake + + rmf_utils + rmf_traffic + rmf_traffic_ros2 + rmf_task_msgs + rclcpp + + eigen + + ament_cmake_catch2 + rmf_cmake_uncrustify + + + ament_cmake + + diff --git a/rmf_task_ros2/src/dispatcher_node/main.cpp b/rmf_task_ros2/src/dispatcher_node/main.cpp new file mode 100644 index 000000000..a96d066fd --- /dev/null +++ b/rmf_task_ros2/src/dispatcher_node/main.cpp @@ -0,0 +1,34 @@ +/* + * 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 + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + std::cout << "~Initializing Dispatcher Node~" << std::endl; + + auto dispatcher = rmf_task_ros2::Dispatcher::make_node( + "rmf_dispatcher_node"); + + const auto& node = dispatcher->node(); + RCLCPP_INFO(node->get_logger(), "Starting task dispatcher node"); + dispatcher->spin(); + RCLCPP_INFO(node->get_logger(), "Closing down task dispatcher"); + rclcpp::shutdown(); +} diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp new file mode 100644 index 000000000..7340c6b30 --- /dev/null +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -0,0 +1,119 @@ +/* + * 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. + * +*/ + +/// Note: This is a testing bidder node script + +#include +#include "../rmf_task_ros2/action/Server.hpp" + +#include +#include + +using namespace rmf_task_ros2; +using TaskType = bidding::MinimalBidder::TaskType; + +int main(int argc, char* argv[]) +{ + std::string fleet_name = "dummy_fleet"; + + rclcpp::init(argc, argv); + std::shared_ptr node = rclcpp::Node::make_shared(fleet_name); + + RCLCPP_INFO( + node->get_logger(), + "Beginning example task bidder node"); + + //============================================================================ + // Create Bidder instance + std::shared_ptr bidder = bidding::MinimalBidder::make( + node, + fleet_name, + { TaskType::Clean, TaskType::Delivery }, + [](const bidding::BidNotice& notice) + { + // Here user will provice the best robot as a bid submission + std::cout << "[MockBidder] Providing best estimates" << std::endl; + auto req_start_time = + rmf_traffic_ros2::convert(notice.task_profile.description.start_time); + + bidding::Submission best_robot_estimate; + best_robot_estimate.robot_name = "dumbot"; + best_robot_estimate.prev_cost = 10.2; + best_robot_estimate.new_cost = 13.5; + best_robot_estimate.finish_time = + rmf_traffic::time::apply_offset(req_start_time, 7); + return best_robot_estimate; + } + ); + + //============================================================================ + // Create sample RMF task action server + auto action_server = action::Server::make(node, fleet_name); + + action_server->register_callbacks( + [&action_server, &node](const TaskProfile& task_profile) + { + std::cout << "[MockBidder] ~Start Queue Task: " + << task_profile.task_id<now()); + status.end_time = + rmf_traffic::time::apply_offset(status.start_time, 7); + + const auto id = profile.task_id; + std::cout << " [MockBidder] Queued, TaskID: " << id << std::endl; + action_server->update_status(status); + + std::this_thread::sleep_for(std::chrono::seconds(2)); + std::cout << " [MockBidder] Executing, TaskID: " << id << std::endl; + status.state = TaskStatus::State::Executing; + action_server->update_status(status); + + std::this_thread::sleep_for(std::chrono::seconds(5)); + std::cout << " [MockBidder] Completed, TaskID: " << id << std::endl; + status.state = TaskStatus::State::Completed; + action_server->update_status(status); + }, task_profile + ); + t.detach(); + + return true; //successs (send State::Queued) + }, + [&action_server](const TaskProfile& task_profile) + { + std::cout << "[MockBidder] ~Cancel Executing Task: " + << task_profile.task_id<get_logger(), + "Closing down task bidder node"); + + rclcpp::shutdown(); +} diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp new file mode 100644 index 000000000..1133177f6 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -0,0 +1,440 @@ +/* + * 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 + +#include "action/Client.hpp" + +#include +#include +#include + +#include + +namespace rmf_task_ros2 { + +//============================================================================== +class Dispatcher::Implementation +{ +public: + std::shared_ptr node; + std::shared_ptr auctioneer; + std::shared_ptr action_client; + + using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; + using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; + using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; + + rclcpp::Service::SharedPtr submit_task_srv; + rclcpp::Service::SharedPtr cancel_task_srv; + rclcpp::Service::SharedPtr get_task_list_srv; + + StatusCallback on_change_fn; + + DispatchTasks active_dispatch_tasks; + DispatchTasks terminal_dispatch_tasks; + std::size_t task_counter = 0; // index for generating task_id + double bidding_time_window; + int terminated_tasks_max_size; + + std::unordered_map task_type_name = + { + {0, "Station"}, + {1, "Loop"}, + {2, "Delivery"}, + {3, "ChargeBattery"}, + {4, "Clean"}, + {5, "Patrol"} + }; + + Implementation(std::shared_ptr node_) + : node{std::move(node_)} + { + // ros2 param + bidding_time_window = + node->declare_parameter("bidding_time_window", 2.0); + RCLCPP_INFO(node->get_logger(), + " Declared Time Window Param as: %f secs", bidding_time_window); + terminated_tasks_max_size = + node->declare_parameter("terminated_tasks_max_size", 100); + RCLCPP_INFO(node->get_logger(), + " Declared Terminated Tasks Max Size Param as: %d", + terminated_tasks_max_size); + + // Setup up stream srv interfaces + submit_task_srv = node->create_service( + rmf_task_ros2::SubmitTaskSrvName, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + switch (request->evaluator) + { + using namespace rmf_task_ros2::bidding; + case SubmitTaskSrv::Request::LOWEST_DIFF_COST_EVAL: + this->auctioneer->select_evaluator( + std::make_shared()); + break; + case SubmitTaskSrv::Request::LOWEST_COST_EVAL: + this->auctioneer->select_evaluator( + std::make_shared()); + break; + case SubmitTaskSrv::Request::QUICKEST_FINISH_EVAL: + this->auctioneer->select_evaluator( + std::make_shared()); + break; + default: + RCLCPP_WARN(this->node->get_logger(), + "Selected Evaluator is invalid, switch back to previous"); + break; + } + + const auto id = this->submit_task(request->description); + if (id == std::nullopt) + { + response->success = false; + return; + } + + response->task_id = *id; + response->success = true; + } + ); + + cancel_task_srv = node->create_service( + rmf_task_ros2::CancelTaskSrvName, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + auto id = request->task_id; + response->success = this->cancel_task(id); + } + ); + + get_task_list_srv = node->create_service( + rmf_task_ros2::GetTaskListSrvName, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + for (auto task : (this->active_dispatch_tasks)) + { + response->active_tasks.push_back( + rmf_task_ros2::convert_status(*(task.second))); + } + + // Terminated Tasks + for (auto task : (this->terminal_dispatch_tasks)) + { + response->terminated_tasks.push_back( + rmf_task_ros2::convert_status(*(task.second))); + } + response->success = true; + } + ); + } + + void start() + { + using namespace std::placeholders; + auctioneer = bidding::Auctioneer::make(node, + std::bind(&Implementation::receive_bidding_winner_cb, this, _1, _2)); + action_client->on_terminate( + std::bind(&Implementation::terminate_task, this, _1)); + action_client->on_change( + std::bind(&Implementation::task_status_cb, this, _1)); + } + + std::optional submit_task(const TaskDescription& description) + { + TaskProfile submitted_task; + submitted_task.submission_time = node->now(); + submitted_task.description = description; + + const auto task_type = static_cast(description.task_type.type); + + if (!task_type_name.count(task_type)) + { + RCLCPP_ERROR(node->get_logger(), "TaskType: %d is invalid", task_type); + return std::nullopt; + } + + // auto generate a task_id for a given submitted task + submitted_task.task_id = + task_type_name[task_type] + std::to_string(task_counter++); + + // add task to internal cache + TaskStatus status; + status.task_profile = submitted_task; + auto new_task_status = std::make_shared(status); + active_dispatch_tasks[submitted_task.task_id] = new_task_status; + + if (on_change_fn) + on_change_fn(new_task_status); + + bidding::BidNotice bid_notice; + bid_notice.task_profile = submitted_task; + bid_notice.time_window = rmf_traffic_ros2::convert( + rmf_traffic::time::from_seconds(bidding_time_window)); + auctioneer->start_bidding(bid_notice); + + return submitted_task.task_id; + } + + bool cancel_task(const TaskID& task_id) + { + // check if key exists + const auto it = active_dispatch_tasks.find(task_id); + if (it == active_dispatch_tasks.end()) + return false; + + // Cancel bidding. This will remove the bidding process + const auto& cancel_task_status = it->second; + if (cancel_task_status->state == TaskStatus::State::Pending) + { + cancel_task_status->state = TaskStatus::State::Canceled; + terminate_task(cancel_task_status); + + if (on_change_fn) + on_change_fn(cancel_task_status); + + return true; + } + + // Cancel action task, this will only send a cancel to FA. up to + // the FA whether to cancel the task. On change is implemented + // internally in action client + return action_client->cancel_task(cancel_task_status->task_profile); + } + + const std::optional get_task_state( + const TaskID& task_id) const + { + // check if taskid exists in active tasks + auto it = active_dispatch_tasks.find(task_id); + if (it != active_dispatch_tasks.end()) + return it->second->state; + + // check if taskid exists in terminated tasks + it = terminal_dispatch_tasks.find(task_id); + if (it != terminal_dispatch_tasks.end()) + return it->second->state; + + return std::nullopt; + } + + void receive_bidding_winner_cb( + const TaskID& task_id, + const rmf_utils::optional winner) + { + const auto it = active_dispatch_tasks.find(task_id); + if (it == active_dispatch_tasks.end()) + return; + const auto& pending_task_status = it->second; + + if (!winner) + { + RCLCPP_WARN(node->get_logger(), "[Dispatch::Bidding Result] task " + "%s has no submissions during bidding :(", task_id.c_str()); + pending_task_status->state = TaskStatus::State::Failed; + terminate_task(pending_task_status); + + if (on_change_fn) + on_change_fn(pending_task_status); + + return; + } + + // now we know which fleet will execute the task + pending_task_status->fleet_name = winner->fleet_name; + + RCLCPP_INFO(node->get_logger(), "[Dispatch::Bidding Result] task " + "%s is accepted by Fleet adapter %s", + task_id.c_str(), winner->fleet_name.c_str()); + + // Remove previous self-generated charging task from "active_dispatch_tasks" + // this is to prevent duplicated charging task (as certain queued charging + // tasks are not terminated when task is reassigned). + // TODO: a better way to impl this + for (auto it = active_dispatch_tasks.begin(); + it != active_dispatch_tasks.end(); ) + { + const auto type = it->second->task_profile.description.task_type.type; + const bool is_fleet_name = (winner->fleet_name == it->second->fleet_name); + const bool is_charging_task = + (type == rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY); + + if (is_charging_task && is_fleet_name) + it = active_dispatch_tasks.erase(it); + else + ++it; + } + + // add task to action server + action_client->add_task( + winner->fleet_name, + pending_task_status->task_profile, + pending_task_status); + + // Note: this might be untrue since task might be ignored by server + pending_task_status->state = TaskStatus::State::Queued; + } + + void terminate_task(const TaskStatusPtr terminate_status) + { + assert(terminate_status->is_terminated()); + + // prevent terminal_dispatch_tasks from piling up meaning + if (terminal_dispatch_tasks.size() >= terminated_tasks_max_size) + { + RCLCPP_WARN(node->get_logger(), + "Terminated tasks reached max size, remove earliest submited task"); + + auto rm_task = terminal_dispatch_tasks.begin(); + for (auto it = rm_task++; it != terminal_dispatch_tasks.end(); it++) + { + const auto t1 = it->second->task_profile.submission_time; + const auto t2 = rm_task->second->task_profile.submission_time; + if (rmf_traffic_ros2::convert(t1) < rmf_traffic_ros2::convert(t2)) + rm_task = it; + } + terminal_dispatch_tasks.erase(terminal_dispatch_tasks.begin() ); + } + + const auto id = terminate_status->task_profile.task_id; + RCLCPP_WARN(node->get_logger(), "Terminate Task ID: %s", id.c_str()); + + // destroy prev status ptr and recreate one + auto status = std::make_shared(*terminate_status); + (terminal_dispatch_tasks)[id] = status; + active_dispatch_tasks.erase(id); + } + + void task_status_cb(const TaskStatusPtr status) + { + // This is to solve the issue that the dispatcher is not aware of those + // "stray" tasks that are not dispatched by the dispatcher. This will add + // the stray tasks when an unknown TaskSummary is heard. + const std::string id = status->task_profile.task_id; + const auto it = active_dispatch_tasks.find(id); + if (it == active_dispatch_tasks.end()) + { + active_dispatch_tasks[id] = status; + RCLCPP_WARN(node->get_logger(), + "Add previously unheard task: %s", id.c_str()); + } + + if (on_change_fn) + on_change_fn(status); + } +}; + +//============================================================================== +std::shared_ptr Dispatcher::init_and_make_node( + const std::string dispatcher_node_name) +{ + rclcpp::init(0, nullptr); + return make_node(dispatcher_node_name); +} + +//============================================================================== +std::shared_ptr Dispatcher::make_node( + const std::string dispatcher_node_name) +{ + return make(rclcpp::Node::make_shared(dispatcher_node_name)); +} + +//============================================================================== +std::shared_ptr Dispatcher::make( + const std::shared_ptr& node) +{ + auto pimpl = rmf_utils::make_impl(node); + pimpl->action_client = action::Client::make(node); + + auto dispatcher = std::shared_ptr(new Dispatcher()); + dispatcher->_pimpl = std::move(pimpl); + dispatcher->_pimpl->start(); + return dispatcher; +} + +//============================================================================== +std::optional Dispatcher::submit_task( + const TaskDescription& task_description) +{ + return _pimpl->submit_task(task_description); +} + +//============================================================================== +bool Dispatcher::cancel_task(const TaskID& task_id) +{ + return _pimpl->cancel_task(task_id); +} + +//============================================================================== +const std::optional Dispatcher::get_task_state( + const TaskID& task_id) const +{ + return _pimpl->get_task_state(task_id); +} + +//============================================================================== +const Dispatcher::DispatchTasks& Dispatcher::active_tasks() const +{ + return _pimpl->active_dispatch_tasks; +} + +//============================================================================== +const Dispatcher::DispatchTasks& Dispatcher::terminated_tasks() const +{ + return _pimpl->terminal_dispatch_tasks; +} + +//============================================================================== +void Dispatcher::on_change(StatusCallback on_change_fn) +{ + _pimpl->on_change_fn = on_change_fn; +} + +//============================================================================== +void Dispatcher::evaluator( + std::shared_ptr evaluator) +{ + _pimpl->auctioneer->select_evaluator(evaluator); +} + +//============================================================================== +std::shared_ptr Dispatcher::node() +{ + return _pimpl->node; +} + +//============================================================================== +void Dispatcher::spin() +{ + rclcpp::spin(_pimpl->node); +} + +//============================================================================== +Dispatcher::Dispatcher() +{ + // Do Nothing +} + +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp new file mode 100644 index 000000000..cd06e3c94 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp @@ -0,0 +1,60 @@ +/* + * 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_ros2 { + +// ============================================================================== +bool TaskStatus::is_terminated() const +{ + return (state == State::Failed) || + (state == State::Completed) || + (state == State::Canceled); +} + +// ============================================================================== +TaskStatus convert_status(const StatusMsg& from) +{ + TaskStatus status; + status.fleet_name = from.fleet_name; + status.task_profile = from.task_profile; + status.start_time = rmf_traffic_ros2::convert(from.start_time); + status.end_time = rmf_traffic_ros2::convert(from.end_time); + status.robot_name = from.robot_name; + status.status = from.status; + status.state = static_cast(from.state); + return status; +} + +// ============================================================================== +StatusMsg convert_status(const TaskStatus& from) +{ + StatusMsg status; + status.fleet_name = from.fleet_name; + status.task_id = from.task_profile.task_id; // duplication + status.task_profile = from.task_profile; + status.start_time = rmf_traffic_ros2::convert(from.start_time); + status.end_time = rmf_traffic_ros2::convert(from.end_time); + status.robot_name = from.robot_name; + status.status = from.status; + status.state = static_cast(from.state); + return status; +} + +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp new file mode 100644 index 000000000..faca48311 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -0,0 +1,179 @@ +/* + * 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 "Client.hpp" + +namespace rmf_task_ros2 { +namespace action { + +std::shared_ptr +Client::make(std::shared_ptr node) +{ + return std::shared_ptr(new Client(node)); +} + +//============================================================================== +Client::Client(std::shared_ptr node) +: _node(node) +{ + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + _request_msg_pub = _node->create_publisher( + TaskRequestTopicName, dispatch_qos); + + _status_msg_sub = _node->create_subscription( + TaskStatusTopicName, dispatch_qos, + [&](const std::unique_ptr msg) + { + auto task_id = msg->task_profile.task_id; + // status update mode + if (_active_task_status.count(task_id)) + { + auto weak_status = _active_task_status[task_id].lock(); + + if (!weak_status) + { + RCLCPP_INFO(_node->get_logger(), "status has expired"); + _active_task_status.erase(task_id); + return; + } + + // TODO: hack to retain task profile and fleet name (to remove) + auto cache_profile = weak_status->task_profile; + // update status to ptr + *weak_status = convert_status(*msg); + weak_status->task_profile = cache_profile; + + if (_on_change_callback) + _on_change_callback(weak_status); + + // if active task terminated + if (weak_status->is_terminated()) + { + RCLCPP_INFO(_node->get_logger(), + "[action] Done Terminated Task: %s", task_id.c_str()); + _active_task_status.erase(task_id); + + if (_on_terminate_callback) + _on_terminate_callback(weak_status); + } + } + else + { + // will still provide onchange even if the task_id is unknown. + RCLCPP_WARN(_node->get_logger(), + "[action] Unknown task: %s", task_id.c_str()); + auto task_status = std::make_shared(convert_status(*msg)); + + if (_on_change_callback) + _on_change_callback(task_status); + + if (!task_status->is_terminated()) + _active_task_status[task_id] = task_status; + } + }); +} + +//============================================================================== +void Client::add_task( + const std::string& fleet_name, + const TaskProfile& task_profile, + TaskStatusPtr status_ptr) +{ + // send request and wait for acknowledgement + RequestMsg request_msg; + request_msg.fleet_name = fleet_name; + request_msg.task_profile = task_profile; + request_msg.method = RequestMsg::ADD; + _request_msg_pub->publish(request_msg); + + // save status ptr + status_ptr->fleet_name = fleet_name; + status_ptr->task_profile = task_profile; + _active_task_status[task_profile.task_id] = status_ptr; + RCLCPP_INFO(_node->get_logger(), "Add Action Task: %s", + task_profile.task_id.c_str()); + return; +} + +//============================================================================== +bool Client::cancel_task( + const TaskProfile& task_profile) +{ + const auto task_id = task_profile.task_id; + RCLCPP_INFO(_node->get_logger(), "Cancel Active Task: %s", task_id.c_str()); + + // check if task is previously added + if (!_active_task_status.count(task_id)) + { + RCLCPP_WARN(_node->get_logger(), + "[action] Not found Task: %s", task_id.c_str()); + return false; + } + + auto weak_status = _active_task_status[task_id].lock(); + + if (!weak_status) + { + std::cerr << "weak status has expired, cancel failed \n"; + _active_task_status.erase(task_id); + return false; + } + + // send cancel + RequestMsg request_msg; + request_msg.fleet_name = weak_status->fleet_name; + request_msg.task_profile = task_profile; + request_msg.method = RequestMsg::CANCEL; + _request_msg_pub->publish(request_msg); + return true; +} + +//============================================================================== +int Client::size() +{ + for (auto it = _active_task_status.begin(); it != _active_task_status.end(); ) + { + if (auto weak_status = it->second.lock() ) + { + if (weak_status->is_terminated()) + it = _active_task_status.erase(it); + else + ++it; + } + else + it = _active_task_status.erase(it); + } + return _active_task_status.size(); +} + +//============================================================================== +void Client::on_change( + StatusCallback status_cb_fn) +{ + _on_change_callback = std::move(status_cb_fn); +} + +//============================================================================== +void Client::on_terminate( + StatusCallback status_cb_fn) +{ + _on_terminate_callback = std::move(status_cb_fn); +} + +} // namespace action +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp new file mode 100644 index 000000000..f1fa804eb --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp @@ -0,0 +1,108 @@ +/* + * 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 SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP +#define SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP + +#include + +#include +#include +#include + +namespace rmf_task_ros2 { +namespace action { + +//============================================================================== +// Task Action Client -- responsible for initiating a rmf task to a target +// fleet. The fleet will work on the requested task and provides a status to +// the client when the task progresses. Termination will be triggered when the +// task ends. + +class Client +{ +public: + /// make an action client + /// + /// \param[in] node + /// ros2 node instance + static std::shared_ptr make( + std::shared_ptr node); + + /// Add a task to a targeted fleet + /// + /// \param[in] fleet_name + /// Target fleet which will execute this task + /// + /// \param[in] task_profile + /// Task Description which will be executed + /// + /// \param[out] status_ptr + /// Will update the status of the task here + void add_task( + const std::string& fleet_name, + const TaskProfile& task_profile, + TaskStatusPtr status_ptr); + + /// Cancel an added task + /// + /// \param[in] task_profile + /// Task which to cancel + /// + /// \return bool which indicate if cancel task is success + bool cancel_task(const TaskProfile& task_profile); + + /// Get the number of active task being track by client + /// + /// \return number of active task + int size(); + + /// Callback Function which will trigger during an event + /// + /// \param[in] status + /// Status of a task which the event is triggered + using StatusCallback = std::function; + + /// Callback when a task status has changed + /// + /// \param[in] status_cb_fn + /// Status callback function + void on_change(StatusCallback status_cb_fn); + + /// Callback when a task is terminated + /// + /// \param[in] status_cb_fn + /// Status callback function + void on_terminate(StatusCallback status_cb_fn); + +private: + Client(std::shared_ptr node); + + using RequestMsg = rmf_task_msgs::msg::DispatchRequest; + + std::shared_ptr _node; + StatusCallback _on_change_callback; + StatusCallback _on_terminate_callback; + std::unordered_map> _active_task_status; + rclcpp::Publisher::SharedPtr _request_msg_pub; + rclcpp::Subscription::SharedPtr _status_msg_sub; +}; + +} // namespace action +} // namespace rmf_task_ros2 + +#endif // SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp new file mode 100644 index 000000000..f0d760835 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp @@ -0,0 +1,97 @@ +/* + * 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 "Server.hpp" + +namespace rmf_task_ros2 { +namespace action { + +std::shared_ptr Server::make( + std::shared_ptr node, + const std::string& fleet_name) +{ + return std::shared_ptr(new Server(node, fleet_name)); +} + +//============================================================================== +void Server::register_callbacks( + AddTaskCallback add_task_cb_fn, + CancelTaskCallback cancel_task_cb_fn) +{ + _add_task_cb_fn = std::move(add_task_cb_fn); + _cancel_task_cb_fn = std::move(cancel_task_cb_fn); +} + +//============================================================================== +void Server::update_status( + const TaskStatus& task_status) +{ + auto msg = convert_status(task_status); + msg.fleet_name = _fleet_name; + _status_msg_pub->publish(msg); +} + +//============================================================================== +Server::Server( + std::shared_ptr node, + const std::string& fleet_name) +: _node(node), _fleet_name(fleet_name) +{ + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + _request_msg_sub = _node->create_subscription( + TaskRequestTopicName, dispatch_qos, + [&](const std::unique_ptr msg) + { + if (msg->fleet_name != _fleet_name) + return;// not me + + RCLCPP_INFO(_node->get_logger(), "[action] Receive a task request!"); + StatusMsg status_msg; + status_msg.fleet_name = _fleet_name; + status_msg.state = StatusMsg::STATE_FAILED; + status_msg.task_profile = msg->task_profile; + + switch (msg->method) + { + // Add Request + case RequestMsg::ADD: + { + if (_add_task_cb_fn && _add_task_cb_fn(msg->task_profile)) + status_msg.state = StatusMsg::STATE_QUEUED; + break; + } + // Cancel Request + case RequestMsg::CANCEL: + { + if (_cancel_task_cb_fn && _cancel_task_cb_fn(msg->task_profile)) + status_msg.state = StatusMsg::STATE_CANCELED; + break; + } + default: + RCLCPP_ERROR(_node->get_logger(), "Request Method is not supported!"); + } + + _status_msg_pub->publish(status_msg); + }); + + _status_msg_pub = _node->create_publisher( + TaskStatusTopicName, dispatch_qos); +} + +} // namespace action +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp new file mode 100644 index 000000000..e85514bca --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp @@ -0,0 +1,91 @@ +/* + * 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 SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP +#define SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP + +#include + +#include +#include +#include +#include + +using TaskProfile = rmf_task_msgs::msg::TaskProfile; + +namespace rmf_task_ros2 { +namespace action { + +//============================================================================== +/// Task Action Server - This is used within the fleet adapter with the role of +/// receiving incoming dispatch requests (from a action_client/Dispatcher), +/// then execute the task accordingly. +class Server +{ +public: + /// initialize action server + /// + /// \param[in] node + /// ros2 node instance + /// + /// \param[in] fleet_name + /// action server id (e.g. fleet adapter name) + static std::shared_ptr make( + std::shared_ptr node, + const std::string& fleet_name); + + using AddTaskCallback = std::function; + using CancelTaskCallback = std::function; + + /// Add event callback functions. These functions will be triggered when a + /// relevant task requests msg is received. + /// + /// \param[in] add_task_cb + /// When a new task request is called. + /// + /// \param[in] cancel_task_cb + /// when a cancel task request is called + void register_callbacks( + AddTaskCallback add_task_cb, + CancelTaskCallback cancel_task_cb); + + /// Use this to send a status update to action client + /// A On Change update is recommended to inform the task progress + /// + /// \param[in] task_status + /// latest status of the task + void update_status(const TaskStatus& task_status); + +private: + Server( + std::shared_ptr node, + const std::string& fleet_name); + + using RequestMsg = rmf_task_msgs::msg::DispatchRequest; + + std::shared_ptr _node; + std::string _fleet_name; + AddTaskCallback _add_task_cb_fn; + CancelTaskCallback _cancel_task_cb_fn; + rclcpp::Subscription::SharedPtr _request_msg_sub; + rclcpp::Publisher::SharedPtr _status_msg_pub; +}; + +} // namespace action +} // namespace rmf_task_ros2 + +#endif // SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp new file mode 100644 index 000000000..519c43637 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp @@ -0,0 +1,253 @@ +/* + * 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 "internal_Auctioneer.hpp" + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +Submission convert(const BidProposal& from) +{ + Submission submission; + submission.fleet_name = from.fleet_name; + submission.robot_name = from.robot_name; + submission.prev_cost = from.prev_cost; + submission.new_cost = from.new_cost; + submission.finish_time = rmf_traffic_ros2::convert(from.finish_time); + return submission; +} + +//============================================================================== +Auctioneer::Implementation::Implementation( + const std::shared_ptr& node_, + BiddingResultCallback result_callback) +: node{std::move(node_)}, + bidding_result_callback{std::move(result_callback)} +{ + // default evaluator + evaluator = std::make_shared(); + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + bid_notice_pub = node->create_publisher( + rmf_task_ros2::BidNoticeTopicName, dispatch_qos); + + bid_proposal_sub = node->create_subscription( + rmf_task_ros2::BidProposalTopicName, dispatch_qos, + [&](const BidProposal::UniquePtr msg) + { + this->receive_proposal(*msg); + }); + + timer = node->create_wall_timer(std::chrono::milliseconds(500), [&]() + { + this->check_bidding_process(); + }); +} + +//============================================================================== +void Auctioneer::Implementation::start_bidding( + const BidNotice& bid_notice) +{ + RCLCPP_INFO(node->get_logger(), "[Auctioneer] Add Bidding task %s to queue", + bid_notice.task_profile.task_id.c_str()); + + BiddingTask bidding_task; + bidding_task.bid_notice = bid_notice; + bidding_task.start_time = node->now(); + queue_bidding_tasks.push(bidding_task); +} + +//============================================================================== +void Auctioneer::Implementation::receive_proposal( + const BidProposal& msg) +{ + const auto id = msg.task_profile.task_id; + RCLCPP_DEBUG(node->get_logger(), + "[Auctioneer] Receive proposal from task_id: %s | from: %s", + id.c_str(), msg.fleet_name.c_str()); + + // check if bidding task is initiated by the auctioneer previously + // add submited proposal to the current bidding tasks list + if (queue_bidding_tasks.front().bid_notice.task_profile.task_id == id) + queue_bidding_tasks.front().submissions.push_back(convert(msg)); +} + +//============================================================================== +// determine the winner within a bidding task instance +void Auctioneer::Implementation::check_bidding_process() +{ + if (queue_bidding_tasks.size() == 0) + return; + + // Executing the task at the front queue + auto front_task = queue_bidding_tasks.front(); + + if (bidding_in_proccess) + { + if (determine_winner(front_task)) + { + queue_bidding_tasks.pop(); + bidding_in_proccess = false; + } + } + else + { + RCLCPP_DEBUG(node->get_logger(), " - Start new bidding task: %s", + front_task.bid_notice.task_profile.task_id.c_str()); + queue_bidding_tasks.front().start_time = node->now(); + bid_notice_pub->publish(front_task.bid_notice); + bidding_in_proccess = true; + } +} + +//============================================================================== +bool Auctioneer::Implementation::determine_winner( + const BiddingTask& bidding_task) +{ + const auto duration = node->now() - bidding_task.start_time; + + if (duration > bidding_task.bid_notice.time_window) + { + auto id = bidding_task.bid_notice.task_profile.task_id; + RCLCPP_INFO(node->get_logger(), "Bidding Deadline reached: %s", + id.c_str()); + std::optional winner = std::nullopt; + + if (bidding_task.submissions.size() == 0) + { + RCLCPP_WARN(node->get_logger(), + "Bidding task has not received any bids"); + } + else + { + winner = evaluate(bidding_task.submissions); + RCLCPP_INFO(node->get_logger(), + "Found winning Fleet Adapter: %s, from %d submissions", + winner->fleet_name.c_str(), bidding_task.submissions.size()); + } + + // Call the user defined callback function + if (bidding_result_callback) + bidding_result_callback(id, winner); + + return true; + } + return false; +} + +//============================================================================== +std::optional Auctioneer::Implementation::evaluate( + const Submissions& submissions) +{ + if (submissions.size() == 0) + return std::nullopt; + + if (!evaluator) + { + RCLCPP_WARN(node->get_logger(), "Evaluator is not set"); + return std::nullopt; + } + + const std::size_t choice = evaluator->choose(submissions); + + if (choice >= submissions.size()) + return std::nullopt; + + return submissions[choice]; +} + +//============================================================================== +std::shared_ptr Auctioneer::make( + const std::shared_ptr& node, + BiddingResultCallback result_callback) +{ + auto pimpl = rmf_utils::make_unique_impl( + node, result_callback); + auto auctioneer = std::shared_ptr(new Auctioneer()); + auctioneer->_pimpl = std::move(pimpl); + return auctioneer; +} + +//============================================================================== +void Auctioneer::start_bidding(const BidNotice& bid_notice) +{ + _pimpl->start_bidding(bid_notice); +} + +//============================================================================== +void Auctioneer::select_evaluator( + std::shared_ptr evaluator) +{ + _pimpl->evaluator = std::move(evaluator); +} + +//============================================================================== +Auctioneer::Auctioneer() +{ + // do nothing +} + +//============================================================================== +std::size_t LeastFleetDiffCostEvaluator::choose( + const Submissions& submissions) const +{ + auto winner_it = submissions.begin(); + float winner_cost_diff = winner_it->new_cost - winner_it->prev_cost; + for (auto nominee_it = ++submissions.begin(); + nominee_it != submissions.end(); ++nominee_it) + { + float nominee_cost_diff = nominee_it->new_cost - nominee_it->prev_cost; + if (nominee_cost_diff < winner_cost_diff) + { + winner_it = nominee_it; + winner_cost_diff = nominee_cost_diff; + } + } + return std::distance(submissions.begin(), winner_it); +} + +//============================================================================== +std::size_t LeastFleetCostEvaluator::choose( + const Submissions& submissions) const +{ + auto winner_it = submissions.begin(); + for (auto nominee_it = ++submissions.begin(); + nominee_it != submissions.end(); ++nominee_it) + { + if (nominee_it->new_cost < winner_it->new_cost) + winner_it = nominee_it; + } + return std::distance(submissions.begin(), winner_it); +} + +//============================================================================== +std::size_t QuickestFinishEvaluator::choose( + const Submissions& submissions) const +{ + auto winner_it = submissions.begin(); + for (auto nominee_it = ++submissions.begin(); + nominee_it != submissions.end(); ++nominee_it) + { + if (nominee_it->finish_time < winner_it->finish_time) + winner_it = nominee_it; + } + return std::distance(submissions.begin(), winner_it); +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp new file mode 100644 index 000000000..bd1230acb --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp @@ -0,0 +1,132 @@ +/* + * 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 + +#include +#include +#include + +namespace rmf_task_ros2 { +namespace bidding { + +using BidProposal = rmf_task_msgs::msg::BidProposal; + +//============================================================================== +BidProposal convert(const Submission& from) +{ + BidProposal proposal_msg; + proposal_msg.fleet_name = from.fleet_name; + proposal_msg.robot_name = from.robot_name; + proposal_msg.prev_cost = from.prev_cost; + proposal_msg.new_cost = from.new_cost; + proposal_msg.finish_time = rmf_traffic_ros2::convert(from.finish_time); + return proposal_msg; +} + +//============================================================================== +class MinimalBidder::Implementation +{ +public: + + std::shared_ptr node; + std::string fleet_name; + std::unordered_set valid_task_types; + ParseSubmissionCallback get_submission_fn; + + using BidNoticeSub = rclcpp::Subscription; + BidNoticeSub::SharedPtr dispatch_notice_sub; + + using BidProposalPub = rclcpp::Publisher; + BidProposalPub::SharedPtr dispatch_proposal_pub; + + Implementation( + std::shared_ptr node_, + const std::string& fleet_name_, + const std::unordered_set& valid_task_types_, + ParseSubmissionCallback submission_cb) + : node{std::move(node_)}, + fleet_name{std::move(fleet_name_)}, + valid_task_types{std::move(valid_task_types_)}, + get_submission_fn{std::move(submission_cb)} + { + const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); + + dispatch_notice_sub = node->create_subscription( + rmf_task_ros2::BidNoticeTopicName, dispatch_qos, + [&](const BidNotice::UniquePtr msg) + { + this->receive_notice(*msg); + }); + + dispatch_proposal_pub = node->create_publisher( + rmf_task_ros2::BidProposalTopicName, dispatch_qos); + } + + // Callback fn when a dispatch notice is received + void receive_notice(const BidNotice& msg) + { + RCLCPP_INFO(node->get_logger(), + "[Bidder] Received Bidding notice for task_id: %s", + msg.task_profile.task_id.c_str()); + + const auto task_type = (msg.task_profile.description.task_type.type); + + // check if task type is valid + if (!valid_task_types.count(static_cast(task_type))) + { + RCLCPP_WARN(node->get_logger(), "%s: task type %d", + fleet_name.c_str(), task_type); + return; + } + + // check if get submission function is declared + if (!get_submission_fn) + return; + + // Submit proposal + const auto bid_submission = get_submission_fn(msg); + auto best_proposal = convert(bid_submission); + best_proposal.fleet_name = fleet_name; + best_proposal.task_profile = msg.task_profile; + dispatch_proposal_pub->publish(best_proposal); + } +}; + +//============================================================================== +std::shared_ptr MinimalBidder::make( + const std::shared_ptr& node, + const std::string& fleet_name, + const std::unordered_set& valid_task_types, + ParseSubmissionCallback submission_cb) +{ + auto pimpl = rmf_utils::make_unique_impl( + node, fleet_name, valid_task_types, submission_cb); + auto bidder = std::shared_ptr(new MinimalBidder()); + bidder->_pimpl = std::move(pimpl); + return bidder; +} + +//============================================================================== +MinimalBidder::MinimalBidder() +{ + // do nothing +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp new file mode 100644 index 000000000..57cf1a0eb --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp @@ -0,0 +1,94 @@ +/* + * 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 SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP +#define SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP + +#include +#include +#include + +#include +#include + +namespace rmf_task_ros2 { +namespace bidding { + +using BidProposal = rmf_task_msgs::msg::BidProposal; + +//============================================================================== +class Auctioneer::Implementation +{ +public: + std::shared_ptr node; + rclcpp::TimerBase::SharedPtr timer; + BiddingResultCallback bidding_result_callback; + std::shared_ptr evaluator; + + struct BiddingTask + { + BidNotice bid_notice; + builtin_interfaces::msg::Time start_time; + std::vector submissions; + }; + + bool bidding_in_proccess = false; + std::queue queue_bidding_tasks; + + using BidNoticePub = rclcpp::Publisher; + BidNoticePub::SharedPtr bid_notice_pub; + + using BidProposalSub = rclcpp::Subscription; + BidProposalSub::SharedPtr bid_proposal_sub; + + Implementation( + const std::shared_ptr& node_, + BiddingResultCallback result_callback); + + /// Start a bidding process + void start_bidding(const BidNotice& bid_notice); + + // Receive proposal and evaluate + void receive_proposal(const BidProposal& msg); + + // determine the winner within a bidding task instance + void check_bidding_process(); + + bool determine_winner(const BiddingTask& bidding_task); + + std::optional evaluate(const Submissions& submissions); + + static const Implementation& get(const Auctioneer& auctioneer) + { + return *auctioneer._pimpl; + } +}; + +//============================================================================== +std::optional evaluate( + const Auctioneer& auctioneer, + const Submissions& submissions) +{ + auto fimpl = Auctioneer::Implementation::get(auctioneer); + return fimpl.evaluate(submissions); +} + +} // namespace bidding +} // namespace rmf_task_ros2 + +#endif // SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP diff --git a/rmf_task_ros2/test/action/test_ActionInterface.cpp b/rmf_task_ros2/test/action/test_ActionInterface.cpp new file mode 100644 index 000000000..11241a6e0 --- /dev/null +++ b/rmf_task_ros2/test/action/test_ActionInterface.cpp @@ -0,0 +1,187 @@ +/* + * 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 "../../src/rmf_task_ros2/action/Client.hpp" +#include "../../src/rmf_task_ros2/action/Server.hpp" +#include + +#include +#include +#include + +namespace rmf_task_ros2 { +namespace action { + +//============================================================================== +SCENARIO("Action communication with client and server", "[ActionInterface]") +{ + TaskProfile task_profile1; + task_profile1.task_id = "task1"; + task_profile1.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_STATION; + + TaskProfile task_profile2; + task_profile2.task_id = "task2"; + task_profile2.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_STATION; + + //============================================================================ + + // received task to test + std::optional test_add_task; + std::optional test_cancel_task; + + // Creating 1 auctioneer and 1 bidder + rclcpp::init(0, nullptr); + auto node = rclcpp::Node::make_shared("test_ActionInferface"); + auto action_server = Server::make(node, "test_server"); + auto action_client = Client::make(node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // received test request msg from client + action_server->register_callbacks( + // add task + [&test_add_task](const TaskProfile& task_profile) + { + test_add_task = task_profile; + return true; + }, + // cancel task + [&test_cancel_task](const TaskProfile& task_profile) + { + test_cancel_task = task_profile; + return true; + } + ); + + // ROS Spin: forever incompleted future + std::promise ready_promise; + std::shared_future ready_future(ready_promise.get_future()); + + WHEN("Add Task") + { + // Add invalid Task! + TaskStatusPtr status_ptr(new TaskStatus); + + action_client->add_task("wrong_server", task_profile1, status_ptr); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + // should not receive cuz incorrect serverid + REQUIRE(status_ptr->state == TaskStatus::State::Pending); + + action_client->add_task("test_server", task_profile1, status_ptr); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + // check status + REQUIRE(status_ptr->state == TaskStatus::State::Queued); + + // status ptr is destroyed, should not have anymore tracking + status_ptr.reset(); + REQUIRE(action_client->size() == 0); + } + + WHEN("Cancel Task") + { + // send valid task + TaskStatusPtr status_ptr(new TaskStatus); + action_client->add_task("test_server", task_profile2, status_ptr); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + // Invalid Cancel Task! + bool cancel_success = action_client->cancel_task(task_profile1); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + REQUIRE(!test_cancel_task); + REQUIRE(cancel_success == false); // cancel failed + REQUIRE(action_client->size() == 1); + + // Valid Cancel task + action_client->cancel_task(task_profile2); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + REQUIRE(test_cancel_task); + REQUIRE(*test_cancel_task == task_profile2); + REQUIRE(status_ptr->is_terminated()); + REQUIRE(action_client->size() == 0); + } + + //============================================================================ + + std::optional test_task_onchange; + std::optional test_task_onterminate; + + // received status update from server + action_client->on_change( + [&test_task_onchange](const TaskStatusPtr status) + { + test_task_onchange = *status; + } + ); + action_client->on_terminate( + [&test_task_onterminate](const TaskStatusPtr status) + { + test_task_onterminate = *status; + } + ); + + WHEN("On Change and On Terminate Task") + { + TaskStatusPtr status_ptr(new TaskStatus); + action_client->add_task("test_server", task_profile1, status_ptr); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + REQUIRE(test_task_onchange); + REQUIRE(test_task_onchange->state == TaskStatus::State::Queued); + + TaskStatus server_task; + server_task.task_profile = task_profile1; + server_task.state = TaskStatus::State::Executing; + + // Update it as executing + action_server->update_status(server_task); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(1.5)); + + REQUIRE(test_task_onchange->state == TaskStatus::State::Executing); + REQUIRE(!test_task_onterminate); // havnt terminated yet + + // completion + server_task.state = TaskStatus::State::Completed; + // Update it as executing + action_server->update_status(server_task); + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(0.5)); + + REQUIRE(test_task_onterminate); + REQUIRE(test_task_onterminate->state == TaskStatus::State::Completed); + } + rclcpp::shutdown(); +} + +} // namespace action +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/bidding/test_Evaluator.cpp b/rmf_task_ros2/test/bidding/test_Evaluator.cpp new file mode 100644 index 000000000..cc9159ad9 --- /dev/null +++ b/rmf_task_ros2/test/bidding/test_Evaluator.cpp @@ -0,0 +1,116 @@ +/* + * 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 "../../src/rmf_task_ros2/bidding/internal_Auctioneer.hpp" +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +auto now = std::chrono::steady_clock::now(); + +Submission submission1{ + "fleet1", "", 2.3, 3.4, rmf_traffic::time::apply_offset(now, 5) +}; +Submission submission2{ + "fleet2", "", 3.5, 3.6, rmf_traffic::time::apply_offset(now, 5.5) +}; +Submission submission3{ + "fleet3", "", 0.0, 1.4, rmf_traffic::time::apply_offset(now, 3) +}; +Submission submission4{ + "fleet4", "", 5.0, 5.4, rmf_traffic::time::apply_offset(now, 4) +}; +Submission submission5{ + "fleet5", "", 0.5, 0.8, rmf_traffic::time::apply_offset(now, 3.5) +}; + +//============================================================================== +SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]") +{ + rclcpp::init(0, nullptr); + auto node = rclcpp::Node::make_shared("test_selfbidding"); + auto auctioneer = Auctioneer::make(node, + [](const std::string&, const std::optional) {}); + + WHEN("Least Diff Cost Evaluator") + { + auto eval = std::make_shared(); + auctioneer->select_evaluator(eval); + + AND_WHEN("0 submissions") + { + std::vector submissions{}; + auto winner = evaluate(*auctioneer, submissions); + REQUIRE(!winner); // no winner + } + AND_WHEN("5 submissions") + { + std::vector submissions{ + submission1, submission2, submission3, submission4, submission5 }; + auto winner = evaluate(*auctioneer, submissions); + REQUIRE(winner->fleet_name == "fleet2"); // least diff cost agent + } + } + + WHEN("Least Fleet Cost Evaluator") + { + auto eval = std::make_shared(); + auctioneer->select_evaluator(eval); + + AND_WHEN("0 submissions") + { + std::vector submissions{}; + auto winner = evaluate(*auctioneer, submissions); + REQUIRE(!winner); // no winner + } + AND_WHEN("5 submissions") + { + std::vector submissions{ + submission1, submission2, submission3, submission4, submission5 }; + auto winner = evaluate(*auctioneer, submissions); + REQUIRE(winner->fleet_name == "fleet5"); // least diff cost agent + } + } + + WHEN("Quickest Finish Time Evaluator") + { + auto eval = std::make_shared(); + auctioneer->select_evaluator(eval); + + AND_WHEN("0 submissions") + { + std::vector submissions{}; + auto winner = evaluate(*auctioneer, submissions); + REQUIRE(!winner); // no winner + } + AND_WHEN("5 submissions") + { + std::vector submissions{ + submission1, submission2, submission3, submission4, submission5 }; + auto winner = evaluate(*auctioneer, submissions); + REQUIRE(winner->fleet_name == "fleet3"); // least diff cost agent + } + } + + rclcpp::shutdown(); +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp new file mode 100644 index 000000000..4656e70e4 --- /dev/null +++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp @@ -0,0 +1,156 @@ +/* + * 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 +#include +#include + +#include +#include +#include + +namespace rmf_task_ros2 { +namespace bidding { + +using TaskProfile = rmf_task_msgs::msg::TaskProfile; +using TaskType = bidding::MinimalBidder::TaskType; + +//============================================================================== +BidNotice bidding_task1; +BidNotice bidding_task2; + +// set time window to 2s +auto timeout = rmf_traffic_ros2::convert(rmf_traffic::time::from_seconds(2.0)); + +//============================================================================== +SCENARIO("Auction with 2 Bids", "[TwoBids]") +{ + // Initializing bidding task + bidding_task1.task_profile.task_id = "bid1"; + bidding_task1.time_window = timeout; + bidding_task1.task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_STATION; + + bidding_task2.task_profile.task_id = "bid2"; + bidding_task2.time_window = timeout; + bidding_task2.task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; + + //============================================================================ + // test received msg + std::optional test_notice_bidder1; + std::optional test_notice_bidder2; + std::string r_result_id = ""; + std::string r_result_winner = ""; + + // Creating 1 auctioneer and 1 bidder + rclcpp::init(0, nullptr); + auto node = rclcpp::Node::make_shared("test_selfbidding"); + + auto auctioneer = Auctioneer::make( + node, + /// Bidding Result Callback Function + [&r_result_id, &r_result_winner]( + const std::string& task_id, const std::optional winner) + { + if (!winner) + return; + r_result_id = task_id; + r_result_winner = winner->fleet_name; + return; + } + ); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto bidder1 = MinimalBidder::make( + node, "bidder1", { TaskType::Station, TaskType::Delivery }, + [&test_notice_bidder1](const BidNotice& notice) + { + Submission best_robot_estimate; + test_notice_bidder1 = notice.task_profile; + return best_robot_estimate; + } + ); + + auto bidder2 = MinimalBidder::make( + node, "bidder2", { TaskType::Delivery, TaskType::Clean }, + [&test_notice_bidder2](const BidNotice& notice) + { + // TaskType should not be supported + Submission best_robot_estimate; + best_robot_estimate.new_cost = 2.3; // lower cost than bidder1 + test_notice_bidder2 = notice.task_profile; + return best_robot_estimate; + } + ); + + // ROS Spin: forever incompleted future + std::promise ready_promise; + std::shared_future ready_future(ready_promise.get_future()); + + WHEN("First 'Station' Task Bid") + { + // start bidding + bidding_task1.task_profile.submission_time = node->now(); + auctioneer->start_bidding(bidding_task1); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(1.0)); + + // Check if bidder 1 & 2 receive BidNotice1 + REQUIRE(test_notice_bidder1); + REQUIRE(test_notice_bidder1->task_id == bidding_task1.task_profile.task_id); + REQUIRE(!test_notice_bidder2); // bidder2 doesnt support tasktype + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(2.5)); + + // Check if Auctioneer received Bid from bidder1 + REQUIRE(r_result_winner == "bidder1"); + REQUIRE(r_result_id == "bid1"); + } + + WHEN("Second 'Delivery' Task bid") + { + // start bidding + bidding_task2.task_profile.submission_time = node->now(); + auctioneer->start_bidding(bidding_task2); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(1.0)); + + // Check if bidder 1 & 2 receive BidNotice2 + auto task2_profile = bidding_task2.task_profile; + REQUIRE(test_notice_bidder1->task_id == task2_profile.task_id); + REQUIRE(test_notice_bidder2->task_id == task2_profile.task_id); + + executor.spin_until_future_complete(ready_future, + rmf_traffic::time::from_seconds(2.5)); + + // Check if Auctioneer received Bid from bidder1 + REQUIRE(r_result_winner == "bidder2"); + REQUIRE(r_result_id == "bid2"); + } + + rclcpp::shutdown(); +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp new file mode 100644 index 000000000..266bdd2e9 --- /dev/null +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -0,0 +1,242 @@ +/* + * 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 + +// mock Fleet Adapter to test dispatcher +#include +#include "../../src/rmf_task_ros2/action/Server.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace rmf_task_ros2 { + +//============================================================================== +SCENARIO("Dispatcehr API Test", "[Dispatcher]") +{ + Dispatcher::TaskDescription task_desc1; + Dispatcher::TaskDescription task_desc2; + task_desc1.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_STATION; + task_desc2.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN; + + //============================================================================ + auto dispatcher = Dispatcher::init_and_make_node("test_dispatcher_node"); + + auto spin_thread = std::thread( + [&dispatcher]() + { + dispatcher->spin(); + }); + spin_thread.detach(); + + + WHEN("Check service interfaces") + { + using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; + using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; + using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; + + auto submit_client = dispatcher->node()->create_client( + rmf_task_ros2::SubmitTaskSrvName); + REQUIRE(submit_client->wait_for_service(std::chrono::milliseconds(0))); + auto cancel_client = dispatcher->node()->create_client( + rmf_task_ros2::CancelTaskSrvName); + REQUIRE(cancel_client->wait_for_service(std::chrono::milliseconds(0))); + auto get_tasks_client = dispatcher->node()->create_client( + rmf_task_ros2::GetTaskListSrvName); + REQUIRE(get_tasks_client->wait_for_service(std::chrono::milliseconds(0))); + } + + WHEN("Add 1 and cancel task") + { + // add task + const auto id = dispatcher->submit_task(task_desc1); + REQUIRE(dispatcher->active_tasks().size() == 1); + REQUIRE(dispatcher->terminated_tasks().size() == 0); + REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + + // cancel task + REQUIRE(dispatcher->cancel_task(*id)); + REQUIRE(dispatcher->active_tasks().size() == 0); + REQUIRE(dispatcher->terminated_tasks().size() == 1); + + // check random id + REQUIRE(!(dispatcher->get_task_state("non_existence_id"))); + + // add an invalid task + task_desc2.task_type.type = 10; // this is invalid + REQUIRE(dispatcher->submit_task(task_desc2) == std::nullopt); + } + + //============================================================================ + // test on change fn callback + int change_times = 0; + TaskProfile test_taskprofile; + dispatcher->on_change( + [&change_times, &test_taskprofile](const TaskStatusPtr status) + { + test_taskprofile = status->task_profile; + change_times++; + } + ); + + WHEN("Track Task till Bidding Failed") + { + // Submit first task and wait for bidding + auto id = dispatcher->submit_task(task_desc1); + REQUIRE(dispatcher->active_tasks().size() == 1); + REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + + // Default 2s timeout, wait 3s for timetout, should fail here + std::this_thread::sleep_for(std::chrono::milliseconds(3500)); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Failed); + REQUIRE(dispatcher->terminated_tasks().size() == 1); + REQUIRE(test_taskprofile.task_id == id); + CHECK(change_times == 2); // add and failed + + // Submit another task + id = dispatcher->submit_task(task_desc2); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + REQUIRE(dispatcher->terminated_tasks().size() == 2); + REQUIRE(test_taskprofile.task_id == *id); + CHECK(change_times == 4); // add and failed x2 + } + + //============================================================================ + // Setup Mock Fleetadapter: mock bidder to test + using TaskType = bidding::MinimalBidder::TaskType; + auto node = dispatcher->node(); + auto bidder = bidding::MinimalBidder::make( + node, + "dummy_fleet", + { TaskType::Station, TaskType::Clean }, + [](const bidding::BidNotice&) + { + // Provide a best estimate + bidding::Submission best_robot_estimate; + best_robot_estimate.new_cost = 13.5; + return best_robot_estimate; + } + ); + + //============================================================================ + // Setup Mock Fleetadapter: action server to test + auto action_server = action::Server::make(node, "dummy_fleet"); + + bool task_canceled_flag = false; + + action_server->register_callbacks( + // Add Task callback + [&action_server, &task_canceled_flag](const TaskProfile& task_profile) + { + // Start action task + auto t = std::thread( + [&action_server, &task_canceled_flag](auto profile) + { + TaskStatus status; + status.task_profile = profile; + status.robot_name = "dumbot"; + std::this_thread::sleep_for(std::chrono::seconds(2)); + + if (task_canceled_flag) + { + // std::cout << "[task impl] Cancelled!" << std::endl; + return; + } + + // Executing + status.state = TaskStatus::State::Executing; + action_server->update_status(status); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Completed + status.state = TaskStatus::State::Completed; + action_server->update_status(status); + }, task_profile + ); + t.detach(); + return true; //successs (send State::Queued) + }, + // Cancel Task callback + [&action_server, &task_canceled_flag](const TaskProfile&) + { + task_canceled_flag = true; + return true; //success ,send State::Canceled when dispatcher->cancel_task + } + ); + + //============================================================================ + WHEN("Full Dispatch cycle") + { + const auto id = dispatcher->submit_task(task_desc1); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + std::this_thread::sleep_for(std::chrono::milliseconds(3500)); + + // now should queue the task + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Queued); + REQUIRE(dispatcher->terminated_tasks().size() == 0); + CHECK(change_times == 2); // Pending and Queued + + std::this_thread::sleep_for(std::chrono::seconds(3)); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Completed); + REQUIRE(dispatcher->active_tasks().size() == 0); + REQUIRE(dispatcher->terminated_tasks().size() == 1); + CHECK(change_times == 4); // Pending > Queued > Executing > Completed + + // Add auto generated ChargeBattery Task from fleet adapter + TaskStatus status; + status.task_profile.task_id = "ChargeBattery10"; + status.state = TaskStatus::State::Queued; + status.task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; + action_server->update_status(status); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + CHECK(change_times == 5); // new stray charge task + REQUIRE(dispatcher->active_tasks().size() == 1); + } + + WHEN("Half way cancel Dispatch cycle") + { + const auto id = dispatcher->submit_task(task_desc2); + CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + REQUIRE(dispatcher->active_tasks().size() == 1); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + // cancel the task after QUEUED State + REQUIRE(dispatcher->cancel_task(*id)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + REQUIRE(dispatcher->active_tasks().size() == 0); + REQUIRE(dispatcher->terminated_tasks().size() == 1); + REQUIRE(dispatcher->terminated_tasks().begin()->first == *id); + auto status = dispatcher->terminated_tasks().begin()->second; + CHECK(status->state == TaskStatus::State::Canceled); + CHECK(change_times == 3); // Pending -> Queued -> Canceled + } + + rclcpp::shutdown(); +} + +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/main.cpp b/rmf_task_ros2/test/main.cpp new file mode 100644 index 000000000..23e837acb --- /dev/null +++ b/rmf_task_ros2/test/main.cpp @@ -0,0 +1,21 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#define CATCH_CONFIG_MAIN +#include + +// This will create the main(int argc, char* argv[]) entry point for testing From bcba5e7871e663ec4c16ed712f6238cfcde35f3c Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 6 Jan 2021 16:28:04 +0800 Subject: [PATCH 126/128] loop delivery tests for dispatcher (#236) * unit tests for loop and delivery requests with task dispatcher * minor cleanup * refactor dispatch_task() method --- .../agv/test/MockAdapter.hpp | 30 +- .../agv/FleetUpdateHandle.cpp | 2 +- .../agv/test/MockAdapter.cpp | 29 +- .../test/tasks/test_Delivery.cpp | 258 +++++++++++------- rmf_fleet_adapter/test/tasks/test_Loop.cpp | 252 ++++++++++------- 5 files changed, 340 insertions(+), 231 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp index 29129b643..0a9225abe 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp @@ -22,8 +22,7 @@ #include -#include -#include +#include #include @@ -41,21 +40,21 @@ class MockAdapter : public std::enable_shared_from_this /// Create a mock adapter MockAdapter( - const std::string& node_name, - const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); + const std::string& node_name, + const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); /// Add a fleet to test std::shared_ptr add_fleet( - const std::string& fleet_name, - rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph); + const std::string& fleet_name, + rmf_traffic::agv::VehicleTraits traits, + rmf_traffic::agv::Graph navigation_graph); TrafficLight::UpdateHandlePtr add_traffic_light( - std::shared_ptr command, - const std::string& fleet_name, - const std::string& robot_name, - rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::Profile profile); + std::shared_ptr command, + const std::string& fleet_name, + const std::string& robot_name, + rmf_traffic::agv::VehicleTraits traits, + rmf_traffic::Profile profile); /// Get the rclcpp Node for this adapter std::shared_ptr node(); @@ -69,11 +68,8 @@ class MockAdapter : public std::enable_shared_from_this /// Stop this adapter from spinning void stop(); - /// Submit a delivery request - void request_delivery(const rmf_task_msgs::msg::Delivery& request); - - /// Submit a loop request - void request_loop(const rmf_task_msgs::msg::Loop& request); + /// Submit a task request + void dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile); class Implementation; private: diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 8670e71d4..64480f3ab 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -101,7 +101,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( { if (task_managers.empty()) return; - + if (msg->task_profile.task_id.empty()) return; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index 609b6cc8b..07896224e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -139,19 +139,32 @@ void MockAdapter::stop() } //============================================================================== -void MockAdapter::request_delivery(const rmf_task_msgs::msg::Delivery& request) +void MockAdapter::dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile) { + for (auto& fleet : _pimpl->fleets) + { + auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); + if (!fimpl.accept_task) + continue; + + // NOTE: althought the current adapter supports multiple fleets. The test + // here assumses using a single fleet for each adapter + rmf_task_msgs::msg::BidNotice bid; + bid.task_profile = profile; + fimpl.bid_notice_cb( + std::make_shared(bid)); + + rmf_task_msgs::msg::DispatchRequest req; + req.task_profile = profile; + req.fleet_name = fimpl.name; + req.method = req.ADD; + fimpl.dispatch_request_cb( + std::make_shared(req)); + } // TODO fix this function which is used in the test script test_Delivery // rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); } -//============================================================================== -void MockAdapter::request_loop(const rmf_task_msgs::msg::Loop& request) -{ - // TODO fix this function which is used in the test script test_Loop. - // rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets); -} - } // namespace test } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp index e6717032d..5ef1cbfaf 100644 --- a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp +++ b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp @@ -33,6 +33,10 @@ #include #include +#include +#include +#include + #include #include "../thread_cooldown.hpp" @@ -50,22 +54,22 @@ class MockQuietDispenser using DispenserResult = rmf_dispenser_msgs::msg::DispenserResult; MockQuietDispenser( - std::shared_ptr node, - std::string name) - : _node(std::move(node)), - _name(std::move(name)) + std::shared_ptr node, + std::string name) + : _node(std::move(node)), + _name(std::move(name)) { _request_sub = _node->create_subscription( - rmf_fleet_adapter::DispenserRequestTopicName, - rclcpp::SystemDefaultsQoS(), - [this](DispenserRequest::SharedPtr msg) - { - _process_request(*msg); - }); + rmf_fleet_adapter::DispenserRequestTopicName, + rclcpp::SystemDefaultsQoS(), + [this](DispenserRequest::SharedPtr msg) + { + _process_request(*msg); + }); _result_pub = _node->create_publisher( - rmf_fleet_adapter::DispenserResultTopicName, - rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::DispenserResultTopicName, + rclcpp::SystemDefaultsQoS()); } std::promise success_promise; @@ -93,20 +97,20 @@ class MockQuietDispenser { using namespace std::chrono_literals; _timer = _node->create_wall_timer( - 10ms, [this, msg]() - { - _timer.reset(); - _task_complete_map[msg.request_guid] = true; + 10ms, [this, msg]() + { + _timer.reset(); + _task_complete_map[msg.request_guid] = true; - DispenserResult result; - result.time = _node->now(); - result.status = DispenserResult::SUCCESS; - result.source_guid = _name; - result.request_guid = msg.request_guid; - _result_pub->publish(result); + DispenserResult result; + result.time = _node->now(); + result.status = DispenserResult::SUCCESS; + result.source_guid = _name; + result.request_guid = msg.request_guid; + _result_pub->publish(result); - success_promise.set_value(true); - }); + success_promise.set_value(true); + }); } DispenserResult result; @@ -131,64 +135,64 @@ class MockFlakyIngestor using IngestorState = rmf_ingestor_msgs::msg::IngestorState; MockFlakyIngestor( - std::shared_ptr node, - std::string name) - : _node(std::move(node)), - _name(std::move(name)) + std::shared_ptr node, + std::string name) + : _node(std::move(node)), + _name(std::move(name)) { _request_sub = _node->create_subscription( - rmf_fleet_adapter::IngestorRequestTopicName, - rclcpp::SystemDefaultsQoS(), - [this](IngestorRequest::SharedPtr msg) - { - _process_request(*msg); - }); + rmf_fleet_adapter::IngestorRequestTopicName, + rclcpp::SystemDefaultsQoS(), + [this](IngestorRequest::SharedPtr msg) + { + _process_request(*msg); + }); _state_pub = _node->create_publisher( - rmf_fleet_adapter::IngestorStateTopicName, - rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::IngestorStateTopicName, + rclcpp::SystemDefaultsQoS()); using namespace std::chrono_literals; _timer = _node->create_wall_timer( - 100ms, [this]() - { - IngestorState msg; - msg.guid = _name; + 100ms, [this]() + { + IngestorState msg; + msg.guid = _name; - if (_request_queue.empty()) - msg.mode = IngestorState::IDLE; - else - msg.mode = IngestorState::BUSY; + if (_request_queue.empty()) + msg.mode = IngestorState::IDLE; + else + msg.mode = IngestorState::BUSY; - msg.time = _node->now(); - msg.seconds_remaining = 0.1; + msg.time = _node->now(); + msg.seconds_remaining = 0.1; - for (auto& r : _request_queue) - { - msg.request_guid_queue.push_back(r.request.request_guid); - ++r.publish_count; - } + for (auto& r : _request_queue) + { + msg.request_guid_queue.push_back(r.request.request_guid); + ++r.publish_count; + } - const std::size_t initial_count = _request_queue.size(); + const std::size_t initial_count = _request_queue.size(); - _request_queue.erase(std::remove_if( - _request_queue.begin(), _request_queue.end(), - [](const auto& r) - { - return r.publish_count > 2; - }), _request_queue.end()); + _request_queue.erase(std::remove_if( + _request_queue.begin(), _request_queue.end(), + [](const auto& r) + { + return r.publish_count > 2; + }), _request_queue.end()); - if (_request_queue.size() < initial_count) - { - if (!_fulfilled_promise) + if (_request_queue.size() < initial_count) { - _fulfilled_promise = true; - success_promise.set_value(true); + if (!_fulfilled_promise) + { + _fulfilled_promise = true; + success_promise.set_value(true); + } } - } - _state_pub->publish(msg); - }); + _state_pub->publish(msg); + }); } std::promise success_promise; @@ -264,14 +268,14 @@ SCENARIO("Test Delivery") }; auto add_dock_lane = [&]( - const std::size_t w0, - const std::size_t w1, - std::string dock_name) - { - using Lane = rmf_traffic::agv::Graph::Lane; - graph.add_lane({w0, Lane::Event::make(Lane::Dock(dock_name, 10s))}, w1); - graph.add_lane(w1, w0); - }; + const std::size_t w0, + const std::size_t w1, + std::string dock_name) + { + using Lane = rmf_traffic::agv::Graph::Lane; + graph.add_lane({w0, Lane::Event::make(Lane::Dock(dock_name, 10s))}, w1); + graph.add_lane(w1, w0); + }; add_bidir_lane(0, 1); // 0 1 add_bidir_lane(1, 2); // 2 3 @@ -292,6 +296,7 @@ SCENARIO("Test Delivery") const std::string dropoff_name = "dropoff"; REQUIRE(graph.add_key(dropoff_name, 10)); + const std::string delivery_id = "test_delivery"; rmf_traffic::Profile profile{ rmf_traffic::geometry::make_final_convex< @@ -307,65 +312,106 @@ SCENARIO("Test Delivery") auto rcl_context = std::make_shared(); rcl_context->init(0, nullptr); rmf_fleet_adapter::agv::test::MockAdapter adapter( - "test_Delivery", rclcpp::NodeOptions().context(rcl_context)); + "test_Delivery", rclcpp::NodeOptions().context(rcl_context)); std::promise completed_promise; bool at_least_one_incomplete = false; auto completed_future = completed_promise.get_future(); const auto task_sub = adapter.node()->create_subscription< - rmf_task_msgs::msg::TaskSummary>( - rmf_fleet_adapter::TaskSummaryTopicName, rclcpp::SystemDefaultsQoS(), - [&completed_promise, &at_least_one_incomplete]( - const rmf_task_msgs::msg::TaskSummary::SharedPtr msg) - { - if (msg->STATE_COMPLETED == msg->state) - completed_promise.set_value(true); - else - at_least_one_incomplete = true; - }); + rmf_task_msgs::msg::TaskSummary>( + rmf_fleet_adapter::TaskSummaryTopicName, rclcpp::SystemDefaultsQoS(), + [&completed_promise, &at_least_one_incomplete]( + const rmf_task_msgs::msg::TaskSummary::SharedPtr msg) + { + if (msg->STATE_COMPLETED == msg->state) + completed_promise.set_value(true); + else + at_least_one_incomplete = true; + }); const auto fleet = adapter.add_fleet("test_fleet", traits, graph); - fleet->accept_delivery_requests( - [](const rmf_task_msgs::msg::Delivery&) - { - // Accept all delivery requests - return true; - }); + + // Configure default battery param + using BatterySystem = rmf_battery::agv::BatterySystem; + using PowerSystem = rmf_battery::agv::PowerSystem; + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + + auto battery_system = std::make_shared( + *BatterySystem::make(24.0, 40.0, 8.8)); + + auto mechanical_system = MechanicalSystem::make(70.0, 40.0, 0.22); + auto motion_sink = std::make_shared( + *battery_system, *mechanical_system); + + auto ambient_power_system = PowerSystem::make(20.0); + auto ambient_sink = std::make_shared( + *battery_system, *ambient_power_system); + + auto tool_power_system = PowerSystem::make(10.0); + auto tool_sink = std::make_shared( + *battery_system, *tool_power_system); + + fleet->account_for_battery_drain(false); + fleet->set_recharge_threshold(0.2); + fleet->set_task_planner_params( + battery_system, motion_sink, ambient_sink, tool_sink); + + fleet->accept_task_requests( + [&delivery_id](const rmf_task_msgs::msg::TaskProfile& task) + { + // Accept all delivery task requests + CHECK(task.description.task_type.type == + rmf_task_msgs::msg::TaskType::TYPE_DELIVERY); + CHECK(task.task_id == delivery_id); + return true; + }); const auto now = rmf_traffic_ros2::convert(adapter.node()->now()); const rmf_traffic::agv::Plan::StartSet starts = {{now, 0, 0.0}}; auto robot_cmd = std::make_shared< - rmf_fleet_adapter_test::MockRobotCommand>(adapter.node(), graph); + rmf_fleet_adapter_test::MockRobotCommand>(adapter.node(), graph); fleet->add_robot( - robot_cmd, "T0", profile, starts, - [&robot_cmd](rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) - { - robot_cmd->updater = std::move(updater); - }); + robot_cmd, "T0", profile, starts, + [&robot_cmd](rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) + { + // assume battery soc is full + updater->update_battery_soc(1.0); + robot_cmd->updater = std::move(updater); + }); const std::string quiet_dispenser_name = "quiet"; auto quiet_dispenser = MockQuietDispenser( - adapter.node(), quiet_dispenser_name); + adapter.node(), quiet_dispenser_name); auto quiet_future = quiet_dispenser.success_promise.get_future(); const std::string flaky_ingestor_name = "flaky"; auto flaky_ingestor = MockFlakyIngestor( - adapter.node(), flaky_ingestor_name); + adapter.node(), flaky_ingestor_name); auto flaky_future = flaky_ingestor.success_promise.get_future(); adapter.start(); - rmf_task_msgs::msg::Delivery request; - request.task_id = "test_delivery"; + // Note: wait for task_manager to start, else TM will be suspicously "empty" + std::this_thread::sleep_for(1s); - request.pickup_place_name = pickup_name; - request.pickup_dispenser = quiet_dispenser_name; + // Dispatch Delivery Task + rmf_task_msgs::msg::TaskProfile task_profile; + task_profile.task_id = delivery_id; + task_profile.description.start_time = adapter.node()->now(); + task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; - request.dropoff_place_name = dropoff_name; - request.dropoff_ingestor = flaky_ingestor_name; + rmf_task_msgs::msg::Delivery delivery; + delivery.pickup_place_name = pickup_name; + delivery.pickup_dispenser = quiet_dispenser_name; + delivery.dropoff_place_name = dropoff_name; + delivery.dropoff_ingestor = flaky_ingestor_name; - adapter.request_delivery(request); + task_profile.description.delivery = delivery; + adapter.dispatch_task(task_profile); const auto quiet_status = quiet_future.wait_for(15s); REQUIRE(quiet_status == std::future_status::ready); diff --git a/rmf_fleet_adapter/test/tasks/test_Loop.cpp b/rmf_fleet_adapter/test/tasks/test_Loop.cpp index c737efaf6..665f953f3 100644 --- a/rmf_fleet_adapter/test/tasks/test_Loop.cpp +++ b/rmf_fleet_adapter/test/tasks/test_Loop.cpp @@ -29,6 +29,10 @@ #include #include +#include +#include +#include + #include #include "../thread_cooldown.hpp" @@ -82,14 +86,14 @@ SCENARIO("Test loop requests") }; auto add_dock_lane = [&]( - const std::size_t w0, - const std::size_t w1, - std::string dock_name) - { - using Lane = rmf_traffic::agv::Graph::Lane; - graph.add_lane({w0, Lane::Event::make(Lane::Dock(dock_name, 10s))}, w1); - graph.add_lane(w1, w0); - }; + const std::size_t w0, + const std::size_t w1, + std::string dock_name) + { + using Lane = rmf_traffic::agv::Graph::Lane; + graph.add_lane({w0, Lane::Event::make(Lane::Dock(dock_name, 10s))}, w1); + graph.add_lane(w1, w0); + }; add_bidir_lane(0, 1); // 0 1 add_bidir_lane(1, 2); // 2 3 @@ -128,7 +132,7 @@ SCENARIO("Test loop requests") auto rcl_context = std::make_shared(); rcl_context->init(0, nullptr); rmf_fleet_adapter::agv::test::MockAdapter adapter( - "test_Loop", rclcpp::NodeOptions().context(rcl_context)); + "test_Loop", rclcpp::NodeOptions().context(rcl_context)); const std::string loop_0 = "loop_0"; std::promise task_0_completed_promise; @@ -149,107 +153,157 @@ SCENARIO("Test loop requests") std::vector finding_a_plan_1_statuses; const auto task_sub = adapter.node()->create_subscription< - rmf_task_msgs::msg::TaskSummary>( - rmf_fleet_adapter::TaskSummaryTopicName, rclcpp::SystemDefaultsQoS(), - [&task_0_completed_promise, &loop_0, &at_least_one_incomplete_task_0, - &completed_0_count, &last_task_0_msg, &finding_a_plan_0_count, - &task_1_completed_promise, &loop_1, &at_least_one_incomplete_task_1, - &completed_1_count, &last_task_1_msg, &finding_a_plan_1_count, - &finding_a_plan_0_statuses, &finding_a_plan_1_statuses]( - const rmf_task_msgs::msg::TaskSummary::SharedPtr msg) - { - if (msg->STATE_COMPLETED == msg->state) + rmf_task_msgs::msg::TaskSummary>( + rmf_fleet_adapter::TaskSummaryTopicName, rclcpp::SystemDefaultsQoS(), + [&task_0_completed_promise, &loop_0, &at_least_one_incomplete_task_0, + &completed_0_count, &last_task_0_msg, &finding_a_plan_0_count, + &task_1_completed_promise, &loop_1, &at_least_one_incomplete_task_1, + &completed_1_count, &last_task_1_msg, &finding_a_plan_1_count, + &finding_a_plan_0_statuses, &finding_a_plan_1_statuses]( + const rmf_task_msgs::msg::TaskSummary::SharedPtr msg) { - if (msg->task_id == loop_0) + if (msg->STATE_COMPLETED == msg->state) { - if (completed_0_count == 0) - task_0_completed_promise.set_value(true); - - ++completed_0_count; + if (msg->task_id == loop_0) + { + if (completed_0_count == 0) + task_0_completed_promise.set_value(true); + + ++completed_0_count; + } + else if (msg->task_id == loop_1) + { + if (completed_1_count == 0) + task_1_completed_promise.set_value(true); + + ++completed_1_count; + } + else + CHECK(false); } - else if (msg->task_id == loop_1) + else { - if (completed_1_count == 0) - task_1_completed_promise.set_value(true); - - ++completed_1_count; + if (msg->task_id == loop_0) + at_least_one_incomplete_task_0 = true; + else if (msg->task_id == loop_1) + at_least_one_incomplete_task_1 = true; + else + CHECK(false); } - else - CHECK(false); - } - else - { - if (msg->task_id == loop_0) - at_least_one_incomplete_task_0 = true; - else if (msg->task_id == loop_1) - at_least_one_incomplete_task_1 = true; - else - CHECK(false); - } - if (msg->task_id == loop_0) - { - last_task_0_msg = *msg; - if (msg->status.find("Finding a plan for") != std::string::npos) + if (msg->task_id == loop_0) { - ++finding_a_plan_0_count; - finding_a_plan_0_statuses.push_back(msg->status); + last_task_0_msg = *msg; + if (msg->status.find("Finding a plan for") != std::string::npos) + { + ++finding_a_plan_0_count; + finding_a_plan_0_statuses.push_back(msg->status); + } } - } - else if (msg->task_id == loop_1) - { - last_task_1_msg = *msg; - if (msg->status.find("Finding a plan for") != std::string::npos) + else if (msg->task_id == loop_1) { - ++finding_a_plan_1_count; - finding_a_plan_1_statuses.push_back(msg->status); + last_task_1_msg = *msg; + if (msg->status.find("Finding a plan for") != std::string::npos) + { + ++finding_a_plan_1_count; + finding_a_plan_1_statuses.push_back(msg->status); + } } - } - }); + }); const std::size_t n_loops = 5; const std::string fleet_type = "test_fleet"; const auto fleet = adapter.add_fleet(fleet_type, traits, graph); - const auto now = rmf_traffic_ros2::convert(adapter.node()->now()); + // Configure default battery param + using BatterySystem = rmf_battery::agv::BatterySystem; + using PowerSystem = rmf_battery::agv::PowerSystem; + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + + auto battery_system = std::make_shared( + *BatterySystem::make(24.0, 40.0, 8.8)); + + auto mechanical_system = MechanicalSystem::make(70.0, 40.0, 0.22); + auto motion_sink = std::make_shared( + *battery_system, *mechanical_system); + + auto ambient_power_system = PowerSystem::make(20.0); + auto ambient_sink = std::make_shared( + *battery_system, *ambient_power_system); + + auto tool_power_system = PowerSystem::make(10.0); + auto tool_sink = std::make_shared( + *battery_system, *tool_power_system); + fleet->account_for_battery_drain(false); + fleet->set_recharge_threshold(0.2); + fleet->set_task_planner_params( + battery_system, motion_sink, ambient_sink, tool_sink); + + fleet->accept_task_requests( + [](const rmf_task_msgs::msg::TaskProfile& task) + { + // Accept all loop task requests + CHECK(task.description.task_type.type == + rmf_task_msgs::msg::TaskType::TYPE_LOOP); + return true; + }); + + // Add Robot T0 + const auto now = rmf_traffic_ros2::convert(adapter.node()->now()); const rmf_traffic::agv::Plan::StartSet starts_0 = {{now, 0, 0.0}}; auto robot_cmd_0 = std::make_shared< - rmf_fleet_adapter_test::MockRobotCommand>(adapter.node(), graph); + rmf_fleet_adapter_test::MockRobotCommand>(adapter.node(), graph); fleet->add_robot( - robot_cmd_0, "T0", profile, starts_0, - [&robot_cmd_0](rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) - { - robot_cmd_0->updater = std::move(updater); - }); + robot_cmd_0, "T0", profile, starts_0, + [&robot_cmd_0](rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) + { + // assume battery soc is full + updater->update_battery_soc(1.0); + robot_cmd_0->updater = std::move(updater); + }); + // Add Robot T1 const rmf_traffic::agv::Plan::StartSet starts_1 = {{now, 7, 0.0}}; auto robot_cmd_1 = std::make_shared< - rmf_fleet_adapter_test::MockRobotCommand>(adapter.node(), graph); + rmf_fleet_adapter_test::MockRobotCommand>(adapter.node(), graph); fleet->add_robot( - robot_cmd_1, "T1", profile, starts_1, - [&robot_cmd_1](rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) - { - robot_cmd_1->updater = std::move(updater); - }); + robot_cmd_1, "T1", profile, starts_1, + [&robot_cmd_1](rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) + { + // assume battery soc is full + updater->update_battery_soc(1.0); + robot_cmd_1->updater = std::move(updater); + }); adapter.start(); - rmf_task_msgs::msg::Loop request; - request.task_id = loop_0; - request.num_loops = n_loops; - request.robot_type = fleet_type; - request.start_name = south; - request.finish_name = east; - adapter.request_loop(request); - - request.task_id = loop_1; - request.start_name = north; - request.finish_name = east; - adapter.request_loop(request); - - const auto task_0_completed_status = task_0_completed_future.wait_for(60s); + // Note: wait for task_manager to start, else TM will be suspicously "empty" + std::this_thread::sleep_for(1s); + + rmf_task_msgs::msg::TaskProfile task_profile; + task_profile.description.start_time = adapter.node()->now(); + task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_LOOP; + + // Dsipatch Loop 0 Task + task_profile.task_id = loop_0; + task_profile.description.loop.num_loops = n_loops; + task_profile.description.loop.robot_type = fleet_type; + task_profile.description.loop.start_name = south; + task_profile.description.loop.finish_name = east; + adapter.dispatch_task(task_profile); + + // Dispatch Loop 1 Task + task_profile.task_id = loop_1; + task_profile.description.loop.start_name = north; + task_profile.description.loop.finish_name = east; + adapter.dispatch_task(task_profile); + + const auto task_0_completed_status = task_0_completed_future.wait_for(20s); CHECK(task_0_completed_status == std::future_status::ready); CHECK(at_least_one_incomplete_task_0); if (task_0_completed_status != std::future_status::ready) @@ -259,7 +313,7 @@ SCENARIO("Test loop requests") << std::endl; } - const auto task_1_completed_status = task_1_completed_future.wait_for(60s); + const auto task_1_completed_status = task_1_completed_future.wait_for(20s); CHECK(task_1_completed_status == std::future_status::ready); CHECK(at_least_one_incomplete_task_1); if (task_1_completed_status != std::future_status::ready) @@ -271,28 +325,28 @@ SCENARIO("Test loop requests") using VisitMap = std::unordered_map; const auto visited_wp = [](std::size_t wp, const VisitMap& v, std::size_t num) - { - const auto it = v.find(wp); - if (it == v.end()) - return false; + { + const auto it = v.find(wp); + if (it == v.end()) + return false; - return num <= it->second; - }; + return num <= it->second; + }; const auto visited_north = [&visited_wp](const VisitMap& v, std::size_t num) - { - return visited_wp(10, v, num); - }; + { + return visited_wp(10, v, num); + }; const auto visited_east = [&visited_wp](const VisitMap& v, std::size_t num) - { - return visited_wp(7, v, num); - }; + { + return visited_wp(7, v, num); + }; const auto visited_south = [&visited_wp](const VisitMap& v, std::size_t num) - { - return visited_wp(0, v, num); - }; + { + return visited_wp(0, v, num); + }; // Note: I don't assume which robot will be selected for each loop request, so // I expect that either robot will be selected for either request, but that From 89a68c0f0bbc8a26994c302c37f9d275713d141a Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 11 Jan 2021 14:37:03 +0800 Subject: [PATCH 127/128] Implementation of EstimateCache uses std::mutex object and not shared pointer --- rmf_task/include/rmf_task/Estimate.hpp | 2 +- rmf_task/src/rmf_task/Estimate.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rmf_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp index 84be63ab2..9993affcd 100644 --- a/rmf_task/include/rmf_task/Estimate.hpp +++ b/rmf_task/include/rmf_task/Estimate.hpp @@ -86,7 +86,7 @@ class EstimateCache class Implementation; private: - rmf_utils::impl_ptr _pimpl; + rmf_utils::unique_impl_ptr _pimpl; }; } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp index 308581bd8..17ede2078 100644 --- a/rmf_task/src/rmf_task/Estimate.cpp +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -98,19 +98,19 @@ class EstimateCache::Implementation CacheElement, PairHash>; Cache _cache; - std::shared_ptr _mutex = std::make_shared(); + mutable std::mutex _mutex; }; //============================================================================== EstimateCache::EstimateCache(std::size_t N) - : _pimpl(rmf_utils::make_impl(Implementation(N))) + : _pimpl(rmf_utils::make_unique_impl(N)) {} //============================================================================== std::optional EstimateCache::get( std::pair waypoints) const { - std::lock_guard guard(*_pimpl->_mutex); + std::lock_guard guard(_pimpl->_mutex); auto it = _pimpl->_cache.find(waypoints); if (it != _pimpl->_cache.end()) { @@ -123,7 +123,7 @@ std::optional EstimateCache::get( void EstimateCache::set(std::pair waypoints, rmf_traffic::Duration duration, double dsoc) { - std::lock_guard guard(*_pimpl->_mutex); + std::lock_guard guard(_pimpl->_mutex); _pimpl->_cache[waypoints] = CacheElement{duration, dsoc}; } From dac010c67ea4703d1ac4ccd657c02cfdedd48d3a Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 13 Jan 2021 11:30:44 +0800 Subject: [PATCH 128/128] add changelog and clean printout logs --- .../agv/test/MockAdapter.cpp | 2 -- rmf_task_ros2/CHANGELOG.rst | 7 +++++++ .../src/rmf_task_ros2/Dispatcher.cpp | 13 ++++++------ .../src/rmf_task_ros2/action/Client.cpp | 20 ++++++++++--------- .../src/rmf_task_ros2/action/Server.cpp | 3 ++- .../src/rmf_task_ros2/bidding/Auctioneer.cpp | 10 +++++----- .../rmf_task_ros2/bidding/MinimalBidder.cpp | 4 ++-- 7 files changed, 34 insertions(+), 25 deletions(-) create mode 100644 rmf_task_ros2/CHANGELOG.rst diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index 07896224e..680df36e7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -161,8 +161,6 @@ void MockAdapter::dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile) fimpl.dispatch_request_cb( std::make_shared(req)); } - // TODO fix this function which is used in the test script test_Delivery - // rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); } } // namespace test diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst new file mode 100644 index 000000000..294f256a7 --- /dev/null +++ b/rmf_task_ros2/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmf_task_ros2 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.3.0 (2021-01-13) +------------------ +* Introduce dispatcher node to facilitate dispatching of tasks: [#217](https://github.com/osrf/rmf_core/pull/217) diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 1133177f6..3a9759be0 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -205,6 +205,8 @@ class Dispatcher::Implementation if (it == active_dispatch_tasks.end()) return false; + RCLCPP_WARN(node->get_logger(), "Cancel task: [%s]", task_id.c_str()); + // Cancel bidding. This will remove the bidding process const auto& cancel_task_status = it->second; if (cancel_task_status->state == TaskStatus::State::Pending) @@ -251,8 +253,8 @@ class Dispatcher::Implementation if (!winner) { - RCLCPP_WARN(node->get_logger(), "[Dispatch::Bidding Result] task " - "%s has no submissions during bidding :(", task_id.c_str()); + RCLCPP_WARN(node->get_logger(), "Dispatcher Bidding Result: task [%s]" + " has no submissions during bidding, Task Failed", task_id.c_str()); pending_task_status->state = TaskStatus::State::Failed; terminate_task(pending_task_status); @@ -265,8 +267,8 @@ class Dispatcher::Implementation // now we know which fleet will execute the task pending_task_status->fleet_name = winner->fleet_name; - RCLCPP_INFO(node->get_logger(), "[Dispatch::Bidding Result] task " - "%s is accepted by Fleet adapter %s", + RCLCPP_INFO(node->get_logger(), "Dispatcher Bidding Result: task [%s]" + " is accepted by fleet adapter [%s]", task_id.c_str(), winner->fleet_name.c_str()); // Remove previous self-generated charging task from "active_dispatch_tasks" @@ -319,7 +321,6 @@ class Dispatcher::Implementation } const auto id = terminate_status->task_profile.task_id; - RCLCPP_WARN(node->get_logger(), "Terminate Task ID: %s", id.c_str()); // destroy prev status ptr and recreate one auto status = std::make_shared(*terminate_status); @@ -338,7 +339,7 @@ class Dispatcher::Implementation { active_dispatch_tasks[id] = status; RCLCPP_WARN(node->get_logger(), - "Add previously unheard task: %s", id.c_str()); + "Add previously unheard task: [%s]", id.c_str()); } if (on_change_fn) diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp index faca48311..6946fa98b 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -47,7 +47,7 @@ Client::Client(std::shared_ptr node) if (!weak_status) { - RCLCPP_INFO(_node->get_logger(), "status has expired"); + RCLCPP_INFO(_node->get_logger(), "Task was previously terminated"); _active_task_status.erase(task_id); return; } @@ -65,7 +65,8 @@ Client::Client(std::shared_ptr node) if (weak_status->is_terminated()) { RCLCPP_INFO(_node->get_logger(), - "[action] Done Terminated Task: %s", task_id.c_str()); + "Receive status from fleet [%s], task [%s] is now terminated", + msg->fleet_name.c_str(), task_id.c_str()); _active_task_status.erase(task_id); if (_on_terminate_callback) @@ -75,8 +76,8 @@ Client::Client(std::shared_ptr node) else { // will still provide onchange even if the task_id is unknown. - RCLCPP_WARN(_node->get_logger(), - "[action] Unknown task: %s", task_id.c_str()); + RCLCPP_DEBUG(_node->get_logger(), + "[action] Unknown task: [%s]", task_id.c_str()); auto task_status = std::make_shared(convert_status(*msg)); if (_on_change_callback) @@ -105,8 +106,8 @@ void Client::add_task( status_ptr->fleet_name = fleet_name; status_ptr->task_profile = task_profile; _active_task_status[task_profile.task_id] = status_ptr; - RCLCPP_INFO(_node->get_logger(), "Add Action Task: %s", - task_profile.task_id.c_str()); + RCLCPP_DEBUG(_node->get_logger(), "Assign task: [%s] to fleet [%s]", + task_profile.task_id.c_str(), fleet_name.c_str()); return; } @@ -115,13 +116,14 @@ bool Client::cancel_task( const TaskProfile& task_profile) { const auto task_id = task_profile.task_id; - RCLCPP_INFO(_node->get_logger(), "Cancel Active Task: %s", task_id.c_str()); + RCLCPP_DEBUG(_node->get_logger(), + "[action] Cancel Task: [%s]", task_id.c_str()); // check if task is previously added if (!_active_task_status.count(task_id)) { RCLCPP_WARN(_node->get_logger(), - "[action] Not found Task: %s", task_id.c_str()); + "Canceling an unknown task [%s]", task_id.c_str()); return false; } @@ -129,7 +131,7 @@ bool Client::cancel_task( if (!weak_status) { - std::cerr << "weak status has expired, cancel failed \n"; + RCLCPP_WARN(_node->get_logger(), "Task was previously terminated"); _active_task_status.erase(task_id); return false; } diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp index f0d760835..9e3b75a10 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp @@ -60,7 +60,8 @@ Server::Server( if (msg->fleet_name != _fleet_name) return;// not me - RCLCPP_INFO(_node->get_logger(), "[action] Receive a task request!"); + RCLCPP_INFO(_node->get_logger(), + "[Action server] Received task request!"); StatusMsg status_msg; status_msg.fleet_name = _fleet_name; status_msg.state = StatusMsg::STATE_FAILED; diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp index 519c43637..e414dd950 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp @@ -63,7 +63,7 @@ Auctioneer::Implementation::Implementation( void Auctioneer::Implementation::start_bidding( const BidNotice& bid_notice) { - RCLCPP_INFO(node->get_logger(), "[Auctioneer] Add Bidding task %s to queue", + RCLCPP_INFO(node->get_logger(), "Add Task [%s] to a bidding queue", bid_notice.task_profile.task_id.c_str()); BiddingTask bidding_task; @@ -124,20 +124,20 @@ bool Auctioneer::Implementation::determine_winner( if (duration > bidding_task.bid_notice.time_window) { auto id = bidding_task.bid_notice.task_profile.task_id; - RCLCPP_INFO(node->get_logger(), "Bidding Deadline reached: %s", + RCLCPP_DEBUG(node->get_logger(), "Bidding Deadline reached: %s", id.c_str()); std::optional winner = std::nullopt; if (bidding_task.submissions.size() == 0) { - RCLCPP_WARN(node->get_logger(), + RCLCPP_DEBUG(node->get_logger(), "Bidding task has not received any bids"); } else { winner = evaluate(bidding_task.submissions); RCLCPP_INFO(node->get_logger(), - "Found winning Fleet Adapter: %s, from %d submissions", + "Determined winning Fleet Adapter: [%s], from %d submissions", winner->fleet_name.c_str(), bidding_task.submissions.size()); } @@ -159,7 +159,7 @@ std::optional Auctioneer::Implementation::evaluate( if (!evaluator) { - RCLCPP_WARN(node->get_logger(), "Evaluator is not set"); + RCLCPP_WARN(node->get_logger(), "Bidding Evaluator is not set"); return std::nullopt; } diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp index bd1230acb..524097a4a 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp @@ -82,7 +82,7 @@ class MinimalBidder::Implementation void receive_notice(const BidNotice& msg) { RCLCPP_INFO(node->get_logger(), - "[Bidder] Received Bidding notice for task_id: %s", + "[Bidder] Received Bidding notice for task_id [%s]", msg.task_profile.task_id.c_str()); const auto task_type = (msg.task_profile.description.task_type.type); @@ -90,7 +90,7 @@ class MinimalBidder::Implementation // check if task type is valid if (!valid_task_types.count(static_cast(task_type))) { - RCLCPP_WARN(node->get_logger(), "%s: task type %d", + RCLCPP_WARN(node->get_logger(), "[%s]: task type %d is not supported", fleet_name.c_str(), task_type); return; }