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_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_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt new file mode 100644 index 000000000..d6e3b3c46 --- /dev/null +++ b/rmf_battery/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.5) +project(rmf_battery) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_BUILD_TYPE) + # Use the Release build type by default if the user has not specified one + set(CMAKE_BUILD_TYPE Release) +endif() + +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") + +include(GNUInstallDirs) + +# find dependencies +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") +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_targets(rmf_battery HAS_LIBRARY_TARGET) +ament_export_dependencies(rmf_utils) + +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() + +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/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/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..c837feb30 --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/BatterySystem.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__BATTERYSYSTEM_HPP +#define RMF_BATTERY__AGV__BATTERYSYSTEM_HPP + +#include + +#include + +namespace rmf_battery { +namespace agv { + +class BatterySystem +{ +public: + /// 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 + /// + /// \param[in] capacity + /// The nominal capacity of the battery in Ampere-hours + /// + /// \param[in] charging_current + /// The rated current in Amperes for charging the battery + static std::optional make( + double nominal_voltage, + double capacity, + double charging_current); + + /// Get the nominal voltage of this battery system + double nominal_voltage() const; + + /// Get the capacity of this battery system + double capacity() const; + + /// Get the charging current of this battery system + double charging_current() const; + + class Implementation; +private: + BatterySystem(); + 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/MechanicalSystem.hpp b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.hpp new file mode 100644 index 000000000..c433a2c9b --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/MechanicalSystem.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 RMF_BATTERY__AGV__MECHANICALSYSTEM_HPP +#define RMF_BATTERY__AGV__MECHANICALSYSTEM_HPP + +#include + +#include + +namespace rmf_battery { +namespace agv { + +class MechanicalSystem +{ +public: + + /// 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(kg) + /// + /// \param[in] moment_of_inertia + /// The moment of inertia of the robot along its yaw axis in kg.m^2 + /// + /// \param[in] friction_coefficient + /// 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, + double friction_coefficient); + + /// Get the mass of this mechanical system + double mass() const; + + /// Get the moment of inertia of this mechanical system + double moment_of_inertia() const; + + /// Get the friction coefficient of this mechanical system + double friction_coefficient() const; + + class Implementation; +private: + MechanicalSystem(); + 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..994db4cf5 --- /dev/null +++ b/rmf_battery/include/rmf_battery/agv/PowerSystem.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 RMF_BATTERY__AGV__POWERSYSTEM_HPP +#define RMF_BATTERY__AGV__POWERSYSTEM_HPP + +#include + +#include + +namespace rmf_battery { +namespace agv { + + +class PowerSystem +{ +public: + + /// 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 + static std::optional make(double nominal_power); + + /// Get the nominal power of this power system + double nominal_power() const; + + class Implementation; +private: + PowerSystem(); + 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/SimpleDevicePowerSink.hpp b/rmf_battery/include/rmf_battery/agv/SimpleDevicePowerSink.hpp new file mode 100644 index 000000000..46964a5af --- /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( + const BatterySystem& battery_system, + const 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..4aea243ef --- /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( + const BatterySystem& battery_system, + const 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/package.xml b/rmf_battery/package.xml new file mode 100644 index 000000000..dcfdacadd --- /dev/null +++ b/rmf_battery/package.xml @@ -0,0 +1,25 @@ + + + + rmf_battery + 1.0.0 + Package for modelling battery life of robots + yadu + Apache License 2.0 + + ament_cmake + + rmf_utils + rmf_utils + + rmf_traffic + + ament_cmake_catch2 + rmf_cmake_uncrustify + + eigen + + + ament_cmake + + 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..da4a5ed15 --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/BatterySystem.cpp @@ -0,0 +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. + * +*/ + +#include + +namespace rmf_battery { +namespace agv { + +//============================================================================== +class BatterySystem::Implementation +{ +public: + double nominal_voltage; + double capacity; + double charging_current; +}; + +//============================================================================== +std::optional BatterySystem::make( + double nominal_voltage, + double capacity, + double charging_current) +{ + if (nominal_voltage <= 0.0 || + capacity <= 0.0 || + charging_current <= 0.0) + { + return std::nullopt; + } + + BatterySystem battery_system; + battery_system._pimpl->nominal_voltage = nominal_voltage; + battery_system._pimpl->capacity = capacity; + battery_system._pimpl->charging_current = charging_current; + + return battery_system; +} + +//============================================================================== +BatterySystem::BatterySystem() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +double BatterySystem::nominal_voltage() const +{ + return _pimpl->nominal_voltage; +} + +//============================================================================== +double BatterySystem::capacity() const +{ + return _pimpl->capacity; +} + +//============================================================================== +double BatterySystem::charging_current() const +{ + return _pimpl->charging_current; +} + +} // 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..a07fe7ec0 --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/MechanicalSystem.cpp @@ -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. + * +*/ + +#include + +namespace rmf_battery { +namespace agv { + +//============================================================================== +class MechanicalSystem::Implementation +{ +public: + double mass; + double moment_of_inertia; + double friction_coefficient; +}; + +//============================================================================== +std::optional MechanicalSystem::make( + double mass, + double moment_of_inertia, + double friction_coefficient) +{ + 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->moment_of_inertia = moment_of_inertia; + mechanical_system._pimpl->friction_coefficient = friction_coefficient; + + return mechanical_system; +} +//============================================================================== +MechanicalSystem::MechanicalSystem() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +double MechanicalSystem::mass() const +{ + return _pimpl->mass; +} + +//============================================================================== +double MechanicalSystem::friction_coefficient() const +{ + return _pimpl->friction_coefficient; +} + +//============================================================================== +double MechanicalSystem::moment_of_inertia() const +{ + return _pimpl->moment_of_inertia; +} + +} // 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..a3eb81139 --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/PowerSystem.cpp @@ -0,0 +1,56 @@ +/* + * 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: + double nominal_power; +}; + +//============================================================================== +std::optional PowerSystem::make(double nominal_power) +{ + if (nominal_power < 0.0) + return std::nullopt; + + PowerSystem power_system; + power_system._pimpl->nominal_power = nominal_power; + + return power_system; +} + +//============================================================================== +PowerSystem::PowerSystem() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +double PowerSystem::nominal_power() const +{ + return _pimpl->nominal_power; +} + +} // namespace agv +} // namespace rmf_battery 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..b3ee31062 --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/SimpleDevicePowerSink.cpp @@ -0,0 +1,73 @@ +/* + * 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_battery { +namespace agv { + +class SimpleDevicePowerSink::Implementation +{ +public: + BatterySystem battery_system; + PowerSystem power_system; +}; + +//============================================================================== +SimpleDevicePowerSink::SimpleDevicePowerSink( + const BatterySystem& battery_system, + const 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 +{ + 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; + // 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; +} + +} // namespace agv +} // namespace rmf_battery diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp new file mode 100644 index 000000000..6ebf5fe26 --- /dev/null +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -0,0 +1,131 @@ +/* + * 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_battery { +namespace agv { + +class SimpleMotionPowerSink::Implementation +{ +public: + BatterySystem battery_system; + MechanicalSystem mechanical_system; +}; + +//============================================================================== +SimpleMotionPowerSink::SimpleMotionPowerSink( + const BatterySystem& battery_system, + const MechanicalSystem& mechanical_system) +: _pimpl(rmf_utils::make_impl( + Implementation{battery_system, mechanical_system})) +{ + // Do nothing +} + +//============================================================================== +const BatterySystem& SimpleMotionPowerSink::battery_system() const +{ + return _pimpl->battery_system; +} + +//============================================================================== +const MechanicalSystem& SimpleMotionPowerSink::mechanical_system() const +{ + return _pimpl->mechanical_system; +} + +//============================================================================== +namespace { + +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 * v * dt; +} + +} // namespace anonymous + +//============================================================================== +double SimpleMotionPowerSink::compute_change_in_charge( + const rmf_traffic::Trajectory& trajectory) const +{ + if (trajectory.size() < 2) + return 0.0; + + 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 moment_of_inertia = + _pimpl->mechanical_system.moment_of_inertia(); + const double friction = _pimpl->mechanical_system.friction_coefficient(); + + auto begin_it = trajectory.begin(); + + auto start_time = begin_it->time(); + const auto end_time = *trajectory.finish_time(); + const auto motion = rmf_traffic::Motion::compute_cubic_splines( + begin_it, trajectory.end()); + + const double sim_step = 0.5; // seconds + + // Change in energy + 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)) + { + const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); + const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 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 = std::abs(acceleration[2]); + + // Loss through acceleration + 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); + + dE += EA + EF; + } + + // Compute the charge consumed + const double dQ = dE / nominal_voltage; + // 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; +} + +} // namespace agv +} // namespace rmf_battery 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_BatterySystem.cpp b/rmf_battery/test/unit/agv/test_BatterySystem.cpp new file mode 100644 index 000000000..fdef8eba0 --- /dev/null +++ b/rmf_battery/test/unit/agv/test_BatterySystem.cpp @@ -0,0 +1,40 @@ +/* + * 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") +{ + 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 new file mode 100644 index 000000000..62337b04a --- /dev/null +++ b/rmf_battery/test/unit/agv/test_MechanicalSystem.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 + +SCENARIO("Test MechanicalSystem") +{ + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + + 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->moment_of_inertia() == 20.0); + CHECK(mechanical_system->friction_coefficient() == 0.3); + } + + WHEN("In-valid mass is supplied to make()") + { + auto mechanical_system = MechanicalSystem::make(-10.0, 20.0, 0.3); + CHECK_FALSE(mechanical_system); + } + + WHEN("In-valid inertia is supplied to make()") + { + auto mechanical_system = MechanicalSystem::make(10.0, -20.0, 0.3); + CHECK_FALSE(mechanical_system); + } + + WHEN("In-valid friction coefficient is supplied to make()") + { + auto mechanical_system = MechanicalSystem::make(10.0, 20.0, -0.3); + CHECK_FALSE(mechanical_system); + } + + WHEN("In-valid values are supplied to make()") + { + 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 new file mode 100644 index 000000000..b75adcaf7 --- /dev/null +++ b/rmf_battery/test/unit/agv/test_PowerSystem.cpp @@ -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. + * +*/ + +#include + +#include + +SCENARIO("Test PowerSystem") +{ + using PowerSystem = rmf_battery::agv::PowerSystem; + + WHEN("Valid nominal power is supplied to make()") + { + auto power_system = PowerSystem::make(60.0); + REQUIRE(power_system); + CHECK(power_system->nominal_power() == 60.0); + } + + WHEN("In-valid nominal power is supplied to make()") + { + auto power_system = PowerSystem::make(-10.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 new file mode 100644 index 000000000..5bdfb1074 --- /dev/null +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -0,0 +1,314 @@ +/* + * 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 + +SCENARIO("Test battery drain with RobotA") +{ + 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 + auto battery_system_optional = BatterySystem::make(12.0, 24.0, 2.0); + REQUIRE(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); + const MechanicalSystem& mechanical_system = *mechanical_system_optional; + + auto power_system_optional = PowerSystem::make(10.0); + REQUIRE(power_system_optional); + const PowerSystem& power_system_processor = *power_system_optional; + + 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( + {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(); + 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); + + 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())); + + 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); + } + + 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); + + 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())); + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // 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); + } +} + +SCENARIO("Test SimpleBatteryEstimator with RobotB") +{ + 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 + auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); + REQUIRE(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); + const MechanicalSystem& mechanical_system = *mechanical_system_optional; + + auto power_system_optional = PowerSystem::make(20.0); + REQUIRE(power_system_optional); + const PowerSystem& power_system_processor = *power_system_optional; + + 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( + {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(); + 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); + + 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())); + + 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); + } + + 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{20000, 0.0, 0.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())); + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // 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); + } + + 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); + + 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())); + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // 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); + } + + 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{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 = + 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())); + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // 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); + } + + 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 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())); + + 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); + } +} + +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 + auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); + REQUIRE(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); + const MechanicalSystem& mechanical_system = *mechanical_system_optional; + + auto power_system_optional = PowerSystem::make(20.0); + REQUIRE(power_system_optional); + const PowerSystem& power_system_processor = *power_system_optional; + + 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( + {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())); + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + // TODO(YV): Determine the theoretical drain for this trajectory. + REQUIRE(remaining_soc <= 1.0); + } +} diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index be6394036..31fad3f30 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-05) ------------------ * Automatically publish fleet states from the fleet adapter API: [#232](https://github.com/osrf/rmf_core/pull/232) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 68ebda63d..17219928e 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 16ef7a70c..e0576111c 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -50,6 +50,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 5621138e8..1016d1710 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,65 @@ 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. + /// + /// \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); + + /// 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 + /// should be specified. Default value is 0.2. + /// + /// \param[in] threshold + /// 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 + /// 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 +135,7 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// /// \return true to indicate that this fleet should accept the request, false /// to reject the request. + /// using AcceptDeliveryRequest = std::function; @@ -82,6 +147,7 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// 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/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index afdb8475d..5ad2b7a29 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 { @@ -84,6 +88,15 @@ 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); + /// 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. @@ -98,6 +111,25 @@ class RobotUpdateHandle rmf_utils::optional maximum_delay() const; 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/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/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 de9579eec..fad1cd873 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 99e72d8db..ade47d748 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 @@ -38,6 +42,17 @@ // the information provided by fleet drivers. #include "../rmf_fleet_adapter/estimation.hpp" +// Public rmf_traffic API headers +#include +#include + +#include +#include +#include + +#include +#include + //============================================================================== class FleetDriverRobotCommandHandle : public rmf_fleet_adapter::agv::RobotCommandHandle @@ -260,10 +275,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 +296,36 @@ 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); + + if (trajectory.size() < 2) + return; + + if (auto participant = + _travel_info.updater->unstable().get_participant()) + { + participant->set( + {rmf_traffic::Route{state.location.level_name, trajectory}}); + _dock_schedule_time = now; + } } } else @@ -289,6 +334,18 @@ 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); + 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) @@ -309,6 +366,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; @@ -463,13 +522,124 @@ std::shared_ptr make_fleet( // the fleet drivers to publish these messages. connections->fleet->fleet_state_publish_period(std::nullopt); - // If the perform_deliveries parameter is true, then we just blindly accept // all delivery requests. + // Parameters required for task planner + // Battery system + 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(), + "Invalid values supplied for battery system"); + + return nullptr; + } + auto battery_system = std::make_shared( + *battery_system_optional); + + // Mechanical system and motion_sink + auto mechanical_system_optional = rmf_fleet_adapter::get_mechanical_system( + *node, 70.0, 40.0, 0.22); + if (!mechanical_system_optional) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for mechanical system"); + + return nullptr; + } + rmf_battery::agv::MechanicalSystem& mechanical_system = + *mechanical_system_optional; + + 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); + auto ambient_power_system = rmf_battery::agv::PowerSystem::make( + ambient_power_drain); + if (!ambient_power_system) + { + 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); + auto tool_power_system = rmf_battery::agv::PowerSystem::make( + tool_power_drain); + if (!tool_power_system) + { + 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); + connections->fleet->account_for_battery_drain(drain_battery); + + // 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)) + { + 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.description.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..d0850e042 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 @@ -96,7 +100,10 @@ class Task : public std::enable_shared_from_this 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 +124,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 +159,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 b74a30bed..69e466148 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -17,6 +17,20 @@ #include "TaskManager.hpp" +#include +#include +#include +#include + +#include + +#include + +#include "tasks/Clean.hpp" +#include "tasks/ChargeBattery.hpp" +#include "tasks/Delivery.hpp" +#include "tasks/Loop.hpp" + namespace rmf_fleet_adapter { //============================================================================== @@ -38,6 +52,25 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) } }); + mgr->_task_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(); + } + }); + + 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; } @@ -51,11 +84,10 @@ 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); - 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", @@ -70,7 +102,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 +118,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->current_task_end_state(); + + // Update battery soc and finish time in the current 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); + + const double current_battery_soc = _context->current_battery_soc(); + finish_state.battery_soc(current_battery_soc); + + return finish_state; +} + //============================================================================== const agv::RobotContextPtr& TaskManager::context() { @@ -102,67 +156,327 @@ agv::ConstRobotContextPtr TaskManager::context() const return _context; } +//============================================================================== +void TaskManager::set_queue( + const std::vector& assignments) +{ + 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) + { + const auto& a = assignments[i]; + auto start = _context->current_task_end_state().location(); + 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, + start, + a.deployment_time(), + a.state()); + + _queue.push_back(task); + } + + 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, + start, + a.deployment_time(), + a.state()); + + _queue.push_back(task); + } + + 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, + start, + a.deployment_time(), + a.state()); + + _queue.push_back(task); + } + + 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, + start, + a.deployment_time(), + a.state()); + + _queue.push_back(task); + } + + 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.fleet_name = _context->description().owner(); + 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( + _queue.back()->finish_state().finish_time()); + this->_context->node()->task_summary()->publish(msg); + } +} + +//============================================================================== +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; + + std::lock_guard guard(_mutex); + 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()); + + return; + } + + 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(); + + if (now >= deployment_time) + { + // Update state in RobotContext and Assign active task + _context->current_task_end_state(_queue.front()->finish_state()); + _active_task = std::move(_queue.front()); + _queue.erase(_queue.begin()); RCLCPP_INFO( _context->node()->get_logger(), - "Finished all remaining tasks for [%s]", - _context->requester_id().c_str()); + "Beginning new task [%s] for [%s]. Remaining queue size: %d", + _active_task->id().c_str(), _context->requester_id().c_str(), + _queue.size()); - return; + _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(); + msg.fleet_name = _context->description().owner(); + 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) + { + rmf_task_msgs::msg::TaskSummary msg; + msg.state = msg.STATE_FAILED; + + try { + std::rethrow_exception(e); + } + catch(const std::exception& e) { + msg.status = e.what(); + } + + 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( + _active_task->finish_state().finish_time()); + _context->node()->task_summary()->publish(msg); + }, + [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(); + msg.fleet_name = _context->description().owner(); + 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; + }); + + _active_task->begin(); } +} - _active_task = std::move(_queue.front()); - _queue.erase(_queue.begin()); +//============================================================================== +void TaskManager::retreat_to_charger() +{ + { + std::lock_guard guard(_mutex); + if (_active_task || !_queue.empty()) + return; + } - 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()); + 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->task_planning_constraints().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); - _task_sub = _active_task->observe() - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [this, id = _active_task->id()](Task::StatusMsg msg) + if (cache_result) { - msg.task_id = id; - _context->node()->task_summary()->publish(msg); - }, - [this, id = _active_task->id()](std::exception_ptr e) + retreat_battery_drain = cache_result->dsoc; + } + else { - rmf_task_msgs::msg::TaskSummary msg; - msg.state = msg.STATE_FAILED; + 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); - try { - std::rethrow_exception(e); - } - catch(const std::exception& e) { - msg.status = e.what(); + // We assume we can always compute a plan + 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); + } - msg.task_id = id; - _context->node()->task_summary()->publish(msg); - _begin_next_task(); - }, - [this, id = _active_task->id()]() + 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)) { - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = id; - msg.state = msg.STATE_COMPLETED; - this->_context->node()->task_summary()->publish(msg); + // 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()); - _active_task = nullptr; - _begin_next_task(); - }); + const auto finish = charging_request->estimate_finish( + current_state, + _context->task_planning_constraints(), + estimate_cache); + + if (!finish) + return; + + rmf_task::agv::TaskPlanner::Assignment charging_assignment( + charging_request, + finish.value().finish_state(), + current_state.finish_time()); - _active_task->begin(); + set_queue({charging_assignment}); + + RCLCPP_INFO( + _context->node()->get_logger(), + "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_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index e5e63ffa4..45779eb74 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,6 +43,8 @@ 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); @@ -53,6 +59,20 @@ class TaskManager : public std::enable_shared_from_this const Task* current_task() 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; + + /// 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); @@ -64,6 +84,14 @@ 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; + + /// Callback for task timer which begins next task if its deployment time has passed void _begin_next_task(); }; 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 dd0c5a909..8cd9a23ea 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,14 +66,6 @@ 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 @@ -112,50 +101,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) - { - auto lock = 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) - { - auto lock = 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( 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 e2a5bab64..64480f3ab 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -28,6 +28,17 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" +#include +#include +#include + +#include +#include +#include + +#include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -67,217 +78,572 @@ 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; -//============================================================================== -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 (msg->task_profile.task_id.empty()) + return; + + if (bid_notice_assignments.find(msg->task_profile.task_id) + != bid_notice_assignments.end()) + return; + + 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 std::size_t n = request.num_loops; - if (n == 0) - return rmf_utils::nullopt; + return; + } + + 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()); + + return; + } + + 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()); + + 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.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(); - 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; + // Process Cleaning task + if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) + { + if (task_profile.description.clean.start_waypoint.empty()) + { + RCLCPP_ERROR( + node->get_logger(), + "Required param [clean.start_waypoint] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); - const auto loop_start_goal = - rmf_traffic::agv::Plan::Goal(loop_start_wp->index()); + return; + } + + // Check for valid 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) + { + 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; + } - const auto loop_end_goal = - rmf_traffic::agv::Plan::Goal(loop_end_wp->index()); + // 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()); - LoopEstimate best; - for (const auto& element : task_managers) + return; + } + const auto& clean_param = clean_param_it->second; + + // 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) + { + 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) + { + RCLCPP_INFO( + node->get_logger(), + "Unable to generate cleaning trajectory from positions specified " + " in DockSummary msg for [%s]", start_wp_name.c_str()); + + return; + } + + new_request = rmf_task::requests::Clean::make( + 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 for task_id:[%s]", id.c_str()); + } + + else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { - LoopEstimate estimate; - estimate.robot = element.first; + const auto& delivery = task_profile.description.delivery; + if (delivery.pickup_place_name.empty()) + { + RCLCPP_ERROR( + node->get_logger(), + "Required param [delivery.pickup_place_name] 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; + } + + if (delivery.pickup_dispenser.empty()) + { + RCLCPP_ERROR( + 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_ERROR( + node->get_logger(), + "Required param [delivery.dropoff_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } - rmf_traffic::Duration init_duration = std::chrono::seconds(0); - if (loop_init_plan->get_waypoints().size() > 1) + if (delivery.dropoff_place_name.empty()) { - // 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_ERROR( + node->get_logger(), + "Required param [delivery.dropoff_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; } - const auto loop_forward_start = [&]() -> rmf_traffic::agv::Plan::StartSet + if (delivery.dropoff_ingestor.empty()) { - 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_ERROR( + 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) { - if (loop_init_plan->get_waypoints().empty()) - return loop_forward_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(), delivery.pickup_place_name.c_str(), id.c_str()); + + return; + } - return loop_init_plan->get_waypoints().front().time(); - }(); + 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()); - estimate.time = - start_time + init_duration + (2*n - 1)*loop_duration; + return; + } - estimate.loop_end->time(estimate.time); + new_request = rmf_task::requests::Delivery::make( + 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 for task_id:[%s]", id.c_str()); - if (estimate.time < best.time) - best = std::move(estimate); } + else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) + { + const auto& loop = task_profile.description.loop; + if (loop.start_name.empty()) + { + RCLCPP_ERROR( + node->get_logger(), + "Required param [loop.start_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); - if (best.robot) - return best; + return; + } + + if (loop.finish_name.empty()) + { + RCLCPP_ERROR( + 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_ERROR( + node->get_logger(), + "Required param [loop.num_loops: %d] in TaskProfile is invalid." + "Rejecting BidNotice with task_id:[%s]" , loop.num_loops, 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; + } + + new_request = rmf_task::requests::Loop::make( + 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 for task_id:[%s]", id.c_str()); + } + else + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid TaskType [%d] in TaskProfile. Rejecting BidNotice with " + "task_id:[%s]", + task_type.type, id.c_str()); + + return; + } + + if (!new_request) + return; + + // Collate robot states and combine new requestptr with requestptr of + // non-charging tasks in task manager queues + std::vector states; + std::vector constraints_set; + 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()); + 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()); + + robot_name_map.insert({index, t.first->name()}); + ++index; + } + + 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 + const auto result = task_planner->optimal_plan( + rmf_traffic_ros2::convert(node->now()), + states, + constraints_set, + 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_ERROR( + node->get_logger(), + "[TaskPlanner] Failed to compute assignments for task_id:[%s]", + id.c_str()); + + return; + } + + const double cost = task_planner->compute_cost(assignments); + + // 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) + { + debug_stream << "--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; + debug_stream << " <" << a.request()->id() << ": " << request_seconds + << ", " << start_seconds + << ", "<< finish_seconds << ", " << 100* s.battery_soc() + << "%>" << 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; + 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 (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()); } //============================================================================== -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(); + // TODO: Replace this with a planner call + // when the performance improvements are finished + const double dist = (loc - p).norm(); + if (dist < min_dist) + { + min_dist = dist; + nearest_charger = wp; + } + } + + return nearest_charger; } //============================================================================== @@ -380,6 +746,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, @@ -395,6 +762,12 @@ void FleetUpdateHandle::add_robot( fleet = shared_from_this()]( rmf_traffic::schedule::Participant participant) { + 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_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), @@ -404,7 +777,10 @@ void FleetUpdateHandle::add_robot( fleet->_pimpl->planner, fleet->_pimpl->node, fleet->_pimpl->worker, - fleet->_pimpl->default_maximum_delay + fleet->_pimpl->default_maximum_delay, + state, + task_planning_constraints, + fleet->_pimpl->task_planner }); // We schedule the following operations on the worker to make sure we do not @@ -449,6 +825,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) @@ -480,71 +864,56 @@ FleetUpdateHandle& FleetUpdateHandle::fleet_state_publish_period( return *this; } -//============================================================================== -FleetUpdateHandle::FleetUpdateHandle() -{ - // Do nothing -} - -//============================================================================== -void request_delivery( - const rmf_task_msgs::msg::Delivery& request, - const std::vector>& fleets) +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) { - 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->initialized_task_planner = true; + + return _pimpl->initialized_task_planner; } - if (!chosen_fleet) - return; - - chosen_fleet->perform_delivery(request, best); + return false; } +bool FleetUpdateHandle::account_for_battery_drain(bool value) +{ + _pimpl->drain_battery = value; + return _pimpl->drain_battery; +} //============================================================================== -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; - - if (estimate->time < best.time) - { - best = *estimate; - chosen_fleet = &fimpl; - } - } - - if (!chosen_fleet) - return; - - chosen_fleet->perform_loop(request, best); + _pimpl->recharge_threshold = threshold; + return *this; } +//============================================================================== +FleetUpdateHandle::FleetUpdateHandle() +{ + // Do nothing +} } // namespace agv } // namespace rmf_fleet_adapter 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..4428fc611 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,53 @@ RobotContext& RobotContext::maximum_delay( return *this; } +//============================================================================== +const rmf_task::agv::State& RobotContext::current_task_end_state() const +{ + return _current_task_end_state; +} + +//============================================================================== +RobotContext& RobotContext::current_task_end_state( + const rmf_task::agv::State& state) +{ + _current_task_end_state = state; + return *this; +} + +//============================================================================== +const rmf_task::agv::Constraints& RobotContext::task_planning_constraints() const +{ + return _task_planning_constraints; +} + +//============================================================================== +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; +} + +//============================================================================== +const std::shared_ptr& +RobotContext::task_planner() const +{ + return _task_planner; +} + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, @@ -241,7 +288,10 @@ 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::Constraints task_planning_constraints, + std::shared_ptr task_planner) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), _itinerary(std::move(itinerary)), @@ -251,12 +301,17 @@ RobotContext::RobotContext( _worker(worker), _maximum_delay(maximum_delay), _requester_id( - _itinerary.description().owner() + "/" + _itinerary.description().name()) + _itinerary.description().owner() + "/" + _itinerary.description().name()), + _current_task_end_state(state), + _task_planning_constraints(task_planning_constraints), + _task_planner(std::move(task_planner)) { _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..2cfebcdff 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,10 @@ #include #include +#include +#include +#include + #include #include @@ -112,6 +116,30 @@ class RobotContext const TableViewerPtr& table_viewer, const ResponderPtr& responder) final; + /// 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 at the end of its + // current task + 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; + + /// 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; + + /// 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; @@ -124,7 +152,10 @@ 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::Constraints task_planning_constraints, + std::shared_ptr task_planner); std::weak_ptr _command_handle; std::vector _location; @@ -144,6 +175,14 @@ 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 _current_task_end_state; + rmf_task::agv::Constraints _task_planning_constraints; + std::shared_ptr _task_planner; }; 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 d9ba4b7f8..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,40 @@ 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) +{ + 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) @@ -194,5 +228,30 @@ 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 nullptr; +} + + } // 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 ebcc46b6d..f841821e8 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,6 +134,15 @@ 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)); @@ -126,6 +152,45 @@ class FleetUpdateHandle::Implementation rclcpp::Publisher::SharedPtr fleet_state_pub = nullptr; rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr; + // 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) { @@ -140,26 +205,65 @@ class FleetUpdateHandle::Implementation self->publish_fleet_state(); }); + // 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; - }; + 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) { @@ -171,37 +275,12 @@ 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 fleet_state_publish_period( std::optional value); void publish_fleet_state() const; }; -//============================================================================== -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/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index c39ce46d2..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,6 +31,7 @@ class RobotUpdateHandle::Implementation std::weak_ptr context; std::string name; + RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::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)); } 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 46ea62fbc..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 @@ -139,15 +139,28 @@ 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) { - 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); + 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)); + } } } // 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..32ae019b6 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,46 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default(rclcpp::Node& node, return traits; } + +//============================================================================== +std::optional 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); + + auto battery_system = rmf_battery::agv::BatterySystem::make( + voltage, capacity, charging_current); + + return battery_system; +} + +//============================================================================== +std::optional get_mechanical_system( + rclcpp::Node& node, + const double default_mass, + const double default_moment, + const double default_friction) +{ + const double mass = + get_parameter_or_default(node, "mass", default_mass); + 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, moment_of_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..f02b9b0f9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp @@ -20,10 +20,14 @@ #include +#include +#include + #include #include #include +#include namespace rmf_fleet_adapter { @@ -60,6 +64,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); +//============================================================================== +std::optional get_battery_system( + rclcpp::Node& node, + const double default_voltage, + const double default_capacity, + const double default_charging_current); + +std::optional 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 cb0557ffb..b9649941b 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 @@ -534,9 +546,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..9ef580863 --- /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(), 0.99)); + + return Task::make( + 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..c26fbebc8 --- /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( + 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..6b669de62 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -21,42 +21,41 @@ #include "Delivery.hpp" +#include + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== 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) { - const auto& graph = context->navigation_graph(); - - 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)); + phases::GoToPlace::make( + context, std::move(pickup_start), request->pickup_waypoint())); phases.push_back( std::make_unique( context, - request.task_id, - request.pickup_dispenser, + request->id(), + request->pickup_dispenser(), context->itinerary().description().owner(), - request.items)); - - const auto dropoff_wp = - graph.find_waypoint(request.dropoff_place_name)->index(); + request->items())); + auto dropoff_start = request->dropoff_start(pickup_start); phases.push_back( - phases::GoToPlace::make(context, std::move(dropoff_start), dropoff_wp)); + phases::GoToPlace::make( + context, std::move(dropoff_start), request->dropoff_waypoint())); + std::vector ingestor_items; - ingestor_items.reserve(request.items.size()); - for(auto& dispenser_item : request.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; @@ -67,12 +66,18 @@ std::shared_ptr make_delivery( phases.push_back( std::make_unique( context, - request.task_id, - request.dropoff_ingestor, + request->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->id(), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); } } // namespace task 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 8593261fc..a8098cf2c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -24,45 +24,44 @@ namespace tasks { //============================================================================== std::shared_ptr make_loop( - const rmf_task_msgs::msg::Loop& request, + 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) { - const auto& graph = context->navigation_graph(); 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(); - - 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)); - } - + phases.push_back( + phases::GoToPlace::make( + context, std::move(start), request->start_waypoint())); phases.push_back( - phases::GoToPlace::make(context, loop_start, end_wp)); + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); - for (std::size_t i=1; i < request.num_loops; ++i) + 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::GoToPlace::make( + context, loop_end, request->start_waypoint())); phases.push_back( - phases::GoToPlace::make(context, loop_start, end_wp)); + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); } - return Task::make(request.task_id, std::move(phases), context->worker()); + return Task::make( + request->id(), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); } } // namespace tasks -} // naemspace rmf_fleet_adapter +} // 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_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 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/CHANGELOG.rst b/rmf_fleet_msgs/CHANGELOG.rst index ae184d847..cfa8cbb61 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-05) ------------------ * Adding pause command: [#226](https://github.com/osrf/rmf_core/pull/226) diff --git a/rmf_fleet_msgs/CMakeLists.txt b/rmf_fleet_msgs/CMakeLists.txt index 05fd0404a..514a3b5a2 100644 --- a/rmf_fleet_msgs/CMakeLists.txt +++ b/rmf_fleet_msgs/CMakeLists.txt @@ -26,6 +26,9 @@ set(msg_files "msg/PathRequest.msg" "msg/PauseRequest.msg" "msg/ModeParameter.msg" + "msg/DockParameter.msg" + "msg/Dock.msg" + "msg/DockSummary.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} 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/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/CMakeLists.txt b/rmf_task/CMakeLists.txt new file mode 100644 index 000000000..d5d40cb5c --- /dev/null +++ b/rmf_task/CMakeLists.txt @@ -0,0 +1,137 @@ +cmake_minimum_required(VERSION 3.5.0) + +project(rmf_task VERSION 1.0.0) + +set(CMAKE_EXPORT_COMPILE_COMMANDS on) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_BUILD_TYPE) + # Use the Release build type by default if the user has not specified one + set(CMAKE_BUILD_TYPE Release) +endif() + +include(GNUInstallDirs) + +find_package(rmf_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) +find_package(rmf_cmake_uncrustify QUIET) + +# ===== RMF Tasks library +file(GLOB lib_srcs + "src/rmf_task/agv/*.cpp" + "src/rmf_task/requests/*.cpp" + "src/rmf_task/*.cpp" +) + +add_library(rmf_task SHARED + ${lib_srcs} +) + +target_link_libraries(rmf_task + PUBLIC + rmf_battery::rmf_battery + ${rmf_dispenser_msgs_LIBRARIES} +) + +target_include_directories(rmf_task + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${rmf_dispenser_msgs_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_task test/main.cpp ${unit_test_srcs} + TIMEOUT 300) + target_link_libraries(test_rmf_task + 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") + + rmf_uncrustify( + ARGN include src test + CONFIG_FILE ${uncrustify_config_file} + MAX_LINE_LENGTH 80 + ) +endif() + + +# Create cmake config files +include(CMakePackageConfigHelpers) + +set(INSTALL_CONFIG_DIR "${CMAKE_INSTALL_LIBDIR}/rmf_task/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_task-config.cmake.in" + ${PACKAGE_CONFIG_FILE} + INSTALL_DESTINATION ${INSTALL_CONFIG_DIR} +) + +write_basic_package_version_file( + ${PACKAGE_CONFIG_VERSION_FILE} + COMPATIBILITY ExactVersion +) + +install( + TARGETS rmf_task + EXPORT rmf_task-targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +install( + DIRECTORY include/rmf_task + DESTINATION include +) + +install( + FILES + ${PACKAGE_CONFIG_VERSION_FILE} + ${PACKAGE_CONFIG_FILE} + DESTINATION ${INSTALL_CONFIG_DIR} +) + +install( + EXPORT rmf_task-targets + FILE rmf_task-targets.cmake + NAMESPACE rmf_task:: + DESTINATION ${INSTALL_CONFIG_DIR} +) + +export( + EXPORT rmf_task-targets + FILE ${CMAKE_CURRENT_BINARY_DIR}/rmf_task-targets.cmake + NAMESPACE rmf_task:: +) + +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_task/include/rmf_task/Estimate.hpp b/rmf_task/include/rmf_task/Estimate.hpp new file mode 100644 index 000000000..9993affcd --- /dev/null +++ b/rmf_task/include/rmf_task/Estimate.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 RMF_TASK__ESTIMATE_HPP +#define RMF_TASK__ESTIMATE_HPP + +#include +#include + +#include +#include +#include + +namespace rmf_task { + +/// Estimates for requests +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; +private: + rmf_utils::impl_ptr _pimpl; +}; + +/// Stores computed estimates between pairs of waypoints +class EstimateCache +{ +public: + /// 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. + 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::unique_impl_ptr _pimpl; +}; + +} // namespace rmf_task + +#endif // RMF_TASK__ESTIMATE_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp new file mode 100644 index 000000000..a26b2412a --- /dev/null +++ b/rmf_task/include/rmf_task/Request.hpp @@ -0,0 +1,63 @@ +/* + * 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__TASK_HPP +#define RMF_TASK__TASK_HPP + +#include + +#include +#include +#include + +#include +#include + +namespace rmf_task { + +/// Implement this for new type of requests. +class Request +{ +public: + + using SharedPtr = std::shared_ptr; + + /// Get the id of the task + 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 + virtual rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + 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; + + /// Get the earliest start time that this task may begin + virtual rmf_traffic::Time earliest_start_time() const = 0; + + virtual ~Request() = default; +}; + +using RequestPtr = std::shared_ptr; +using ConstRequestPtr = std::shared_ptr; + +} // namespace rmf_task + +#endif // RMF_TASK__REQUEST_HPP diff --git a/rmf_task/include/rmf_task/agv/Constraints.hpp b/rmf_task/include/rmf_task/agv/Constraints.hpp new file mode 100644 index 000000000..11440946e --- /dev/null +++ b/rmf_task/include/rmf_task/agv/Constraints.hpp @@ -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. + * +*/ + +#ifndef RMF_TASK__AGV__CONSTRAINTS_HPP +#define RMF_TASK__AGV__CONSTRAINTS_HPP + +#include + +namespace rmf_task { +namespace agv { + +class Constraints +{ +public: + + /// 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. + 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. + Constraints& threshold_soc(double threshold_soc); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_task + +#endif // RMF_TASK__AGV__CONSTRAINTS_HPP diff --git a/rmf_task/include/rmf_task/agv/State.hpp b/rmf_task/include/rmf_task/agv/State.hpp new file mode 100644 index 000000000..20c99bcfa --- /dev/null +++ b/rmf_task/include/rmf_task/agv/State.hpp @@ -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. + * +*/ + +#ifndef RMF_TASK__AGV__STATE_HPP +#define RMF_TASK__AGV__STATE_HPP + +#include + +#include +#include + +#include +#include + +namespace rmf_task { +namespace agv { + +/// This state representation is used for task planning. +class State +{ +public: + + /// Constructor + /// + /// \param[in] location + /// Current state's location, which includes the time that the robot can + /// feasibly take on a new task, according to the finishing time of any + /// tasks that the robot is currently performing. + /// + /// \param[in] charging_waypoint + /// The charging waypoint index of this robot. + /// + /// \param[in] battery_soc + /// Current battery state of charge of the robot. This value needs to be + /// between 0.0 to 1.0. + State( + rmf_traffic::agv::Plan::Start location, + std::size_t charging_waypoint, + double battery_soc); + + /// The current state's location. + rmf_traffic::agv::Plan::Start location() const; + + /// Sets the state's location. + State& location(rmf_traffic::agv::Plan::Start new_location); + + /// Robot's charging waypoint index. + std::size_t charging_waypoint() const; + + /// Sets the charging waypoint index. + State& charging_waypoint(std::size_t new_charging_waypoint); + + /// The current battery state of charge of the robot. This value is between + /// 0.0 and 1.0. + double battery_soc() const; + + /// Sets a new battery state of charge value. This value needs to be between + /// 0.0 and 1.0. + State& battery_soc(double new_battery_soc); + + /// The current location waypoint index. + std::size_t waypoint() const; + + /// Sets the current location waypoint index. + State& waypoint(std::size_t new_waypoint); + + /// The time which the robot finishes its current task or when it is ready for + /// a new task. + rmf_traffic::Time finish_time() const; + + /// Sets the finish time for the robot. + State& finish_time(rmf_traffic::Time new_finish_time); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_task + +#endif // RMF_TASK__AGV__STATE_HPP diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp new file mode 100644 index 000000000..a0d9e6ac9 --- /dev/null +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -0,0 +1,206 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef RMF_TASK__AGV__TASKPLANNER_HPP +#define RMF_TASK__AGV__TASKPLANNER_HPP + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace rmf_task { +namespace agv { + +//============================================================================== +class TaskPlanner +{ +public: + + /// 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] 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( + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner); + + /// 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; + + /// 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; + + /// Set the planner + Configuration& planner(std::shared_ptr); + + class Implementation; + + private: + rmf_utils::impl_ptr _pimpl; + }; + + class Assignment + { + public: + + /// Constructor + /// + /// \param[in] request + /// The task request 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( + rmf_task::ConstRequestPtr request, + State state, + rmf_traffic::Time deployment_time); + + // Get the request of this task + rmf_task::ConstRequestPtr request() const; + + // 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 + // this assignment + const rmf_traffic::Time deployment_time() const; + + class Implementation; + + private: + 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 + /// + /// \param[in] config + /// 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( + rmf_traffic::Time time_now, + std::vector initial_states, + std::vector constraints_set, + 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. + Result optimal_plan( + rmf_traffic::Time time_now, + std::vector initial_states, + std::vector constraints_set, + 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; + +private: + rmf_utils::impl_ptr _pimpl; + +}; + + +} // namespace agv +} // namespace rmf_task + +#endif // RMF_TASK__AGV__TASKPLANNER_HPP diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp new file mode 100644 index 000000000..1d029fec8 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -0,0 +1,80 @@ +/* + * 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__REQUESTS__CHARGEBATTERY_HPP +#define RMF_TASK__REQUESTS__CHARGEBATTERY_HPP + +#include + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace rmf_task { +namespace requests { + +class ChargeBattery : public rmf_task::Request +{ +public: + + static ConstRequestPtr make( + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + rmf_traffic::Time start_time, + bool drain_battery = true); + + std::string id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) const final; + + rmf_traffic::Duration invariant_duration() const final; + + 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 + double max_charge_soc() const; + + class Implementation; +private: + ChargeBattery(); + rmf_utils::impl_ptr _pimpl; +}; + +using ChargeBatteryRequestPtr = std::shared_ptr; +using ConstChargeBatteryRequestPtr = std::shared_ptr; + +} // namespace requests +} // namespace rmf_task + +#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 new file mode 100644 index 000000000..f8c962e00 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -0,0 +1,89 @@ +/* + * 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__REQUESTS__CLEAN_HPP +#define RMF_TASK__REQUESTS__CLEAN_HPP + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace rmf_task { +namespace requests { + +class Clean : public rmf_task::Request +{ +public: + + static ConstRequestPtr make( + std::string 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, + rmf_traffic::Time start_time, + bool drain_battery = true); + + std::string id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) const final; + + rmf_traffic::Duration invariant_duration() const final; + + 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; + + class Implementation; +private: + Clean(); + rmf_utils::impl_ptr _pimpl; +}; + +using CleanRequestPtr = std::shared_ptr; +using ConstCleanRequestPtr = std::shared_ptr; + +} // namespace requests +} // namespace rmf_task + +#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 new file mode 100644 index 000000000..173e7f980 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -0,0 +1,103 @@ +/* + * 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__REQUESTS__DELIVERY_HPP +#define RMF_TASK__REQUESTS__DELIVERY_HPP + +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include + +namespace rmf_task { +namespace requests { + +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::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, + rmf_traffic::Time start_time, + bool drain_battery = true); + + std::string id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) const final; + + rmf_traffic::Duration invariant_duration() const final; + + 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; +private: + Delivery(); + rmf_utils::impl_ptr _pimpl; +}; + +using DeliveryRequestPtr = std::shared_ptr; +using ConstDeliveryRequestPtr = std::shared_ptr; + +} // namespace requests +} // namespace rmf_task + +#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 new file mode 100644 index 000000000..2c30e5d13 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -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. + * +*/ + +#ifndef RMF_TASK__REQUESTS__LOOP_HPP +#define RMF_TASK__REQUESTS__LOOP_HPP + +#include +#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::string 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::string id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) const final; + + rmf_traffic::Duration invariant_duration() const final; + + 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; +private: + Loop(); + rmf_utils::impl_ptr _pimpl; +}; + +using LoopRequestPtr = std::shared_ptr; +using ConstLoopRequestPtr = std::shared_ptr; + +} // namespace tasks +} // namespace rmf_task + +#endif // RMF_TASK__REQUESTS__LOOP_HPP diff --git a/rmf_task/package.xml b/rmf_task/package.xml new file mode 100644 index 000000000..d9deef5a9 --- /dev/null +++ b/rmf_task/package.xml @@ -0,0 +1,23 @@ + + + + rmf_task + 1.0.0 + Package for managing tasks in the Robotics Middleware Framework + Aaron + Apache License 2.0 + Aaron + + rmf_battery + rmf_utils + rmf_dispenser_msgs + + eigen + + ament_cmake_catch2 + rmf_cmake_uncrustify + + + cmake + + diff --git a/rmf_task/src/rmf_task/Estimate.cpp b/rmf_task/src/rmf_task/Estimate.cpp new file mode 100644 index 000000000..17ede2078 --- /dev/null +++ b/rmf_task/src/rmf_task/Estimate.cpp @@ -0,0 +1,130 @@ +/* + * 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_task { + +//============================================================================== +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; +} + +//============================================================================== +class EstimateCache::Implementation +{ +public: + + Implementation(std::size_t N) + : _cache(N, PairHash(N)) + { + + } + + struct PairHash + { + PairHash(std::size_t N) + { + _shift = std::ceil(std::log2(N)); + } + + size_t operator()(const std::pair& p) const + { + return p.first + (p.second << _shift); + } + + std::size_t _shift; + }; + + using Cache = std::unordered_map, + CacheElement, PairHash>; + + Cache _cache; + mutable std::mutex _mutex; +}; + +//============================================================================== +EstimateCache::EstimateCache(std::size_t N) + : _pimpl(rmf_utils::make_unique_impl(N)) +{} + +//============================================================================== +std::optional EstimateCache::get( + std::pair waypoints) const +{ + std::lock_guard guard(_pimpl->_mutex); + auto it = _pimpl->_cache.find(waypoints); + if (it != _pimpl->_cache.end()) + { + return it->second; + } + return std::nullopt; +} + +//============================================================================== +void EstimateCache::set(std::pair waypoints, + rmf_traffic::Duration duration, double dsoc) +{ + std::lock_guard guard(_pimpl->_mutex); + _pimpl->_cache[waypoints] = CacheElement{duration, dsoc}; +} + +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/Constraints.cpp b/rmf_task/src/rmf_task/agv/Constraints.cpp new file mode 100644 index 000000000..92222a126 --- /dev/null +++ b/rmf_task/src/rmf_task/agv/Constraints.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +namespace rmf_task { +namespace agv { + +class Constraints::Implementation +{ +public: + double threshold_soc; + +}; + +Constraints::Constraints(double threshold_soc) +: _pimpl(rmf_utils::make_impl( + Implementation + { + threshold_soc + })) +{ + 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 Constraints::threshold_soc() const +{ + return _pimpl->threshold_soc; +} + +auto Constraints::threshold_soc(double threshold_soc) -> Constraints& +{ + 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; +} + + +} // namespace agv +} // namespace rmf_task \ No newline at end of file diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp new file mode 100644 index 000000000..82d002d49 --- /dev/null +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +namespace rmf_task { +namespace agv { + +//============================================================================== +class State::Implementation +{ +public: + + Implementation( + rmf_traffic::agv::Plan::Start location, + std::size_t charging_waypoint, + double battery_soc) + : _location(std::move(location)), + _charging_waypoint(charging_waypoint), + _battery_soc(battery_soc) + { + if (_battery_soc < 0.0 || _battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs to be between 0.0 and 1.0."); + } + + rmf_traffic::agv::Plan::Start _location; + std::size_t _charging_waypoint; + double _battery_soc; +}; + +//============================================================================== +State::State( + rmf_traffic::agv::Plan::Start location, + std::size_t charging_waypoint, + double battery_soc) +: _pimpl(rmf_utils::make_impl( + Implementation(std::move(location), charging_waypoint, battery_soc))) +{} + +//============================================================================== +rmf_traffic::agv::Plan::Start State::location() const +{ + return _pimpl->_location; +} + +//============================================================================== +State& State::location(rmf_traffic::agv::Plan::Start new_location) +{ + _pimpl->_location = std::move(new_location); + return *this; +} + +//============================================================================== +std::size_t State::charging_waypoint() const +{ + return _pimpl->_charging_waypoint; +} + +//============================================================================== +State& State::charging_waypoint(std::size_t new_charging_waypoint) +{ + _pimpl->_charging_waypoint = new_charging_waypoint; + return *this; +} + +//============================================================================== +double State::battery_soc() const +{ + return _pimpl->_battery_soc; +} + +//============================================================================== +State& State::battery_soc(double new_battery_soc) +{ + if (new_battery_soc < 0.0 || new_battery_soc > 1.0) + throw std::invalid_argument( + "Battery State of Charge needs be between 0.0 and 1.0."); + + _pimpl->_battery_soc = new_battery_soc; + return *this; +} + +//============================================================================== +std::size_t State::waypoint() const +{ + return _pimpl->_location.waypoint(); +} + +//============================================================================== +State& State::waypoint(std::size_t new_waypoint) +{ + _pimpl->_location.waypoint(new_waypoint); + return *this; +} + +//============================================================================== +rmf_traffic::Time State::finish_time() const +{ + return _pimpl->_location.time(); +} + +//============================================================================== +State& State::finish_time(rmf_traffic::Time new_finish_time) +{ + _pimpl->_location.time(new_finish_time); + return *this; +} + +//============================================================================== +} // namespace agv +} // namespace rmf_task diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp new file mode 100644 index 000000000..6050317a3 --- /dev/null +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -0,0 +1,1459 @@ +/* + * 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 + +namespace rmf_task { +namespace agv { + +//============================================================================== +class TaskPlanner::Configuration::Implementation +{ +public: + + rmf_battery::agv::BatterySystem battery_system; + std::shared_ptr motion_sink; + std::shared_ptr ambient_sink; + std::shared_ptr planner; +}; + +TaskPlanner::Configuration::Configuration( + rmf_battery::agv::BatterySystem battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner) +: _pimpl(rmf_utils::make_impl( + Implementation{ + battery_system, + std::move(motion_sink), + std::move(ambient_sink), + std::move(planner) + })) +{ + // Do nothing +} + +//============================================================================== +rmf_battery::agv::BatterySystem& TaskPlanner::Configuration::battery_system() +{ + return _pimpl->battery_system; +} + +//============================================================================== +auto TaskPlanner::Configuration::battery_system( + rmf_battery::agv::BatterySystem battery_system) -> Configuration& +{ + _pimpl->battery_system = battery_system; + return *this; +} + +//============================================================================== +std::shared_ptr +TaskPlanner::Configuration::motion_sink() const +{ + return _pimpl->motion_sink; +} + +//============================================================================== +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; +} + +//============================================================================== +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; +} + +//============================================================================== +class TaskPlanner::Assignment::Implementation +{ +public: + + rmf_task::ConstRequestPtr request; + State state; + rmf_traffic::Time deployment_time; +}; + +//============================================================================== +TaskPlanner::Assignment::Assignment( + rmf_task::ConstRequestPtr request, + State state, + rmf_traffic::Time deployment_time) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(request), + std::move(state), + deployment_time + })) +{ + // Do nothing +} + +//============================================================================== +rmf_task::ConstRequestPtr TaskPlanner::Assignment::request() const +{ + return _pimpl->request; +} + +//============================================================================== +const State& TaskPlanner::Assignment::state() const +{ + return _pimpl->state; +} + +//============================================================================== +const rmf_traffic::Time TaskPlanner::Assignment::deployment_time() const +{ + return _pimpl->deployment_time; +} + +//============================================================================== + +namespace { + +// ============================================================================ +struct Invariant +{ + std::size_t task_id; + double earliest_start_time; + double earliest_finish_time; +}; + +// ============================================================================ +struct InvariantLess +{ + bool operator()(const Invariant& a, const Invariant& b) const + { + return a.earliest_finish_time < b.earliest_finish_time; + } +}; + +// ============================================================================ +class Candidates +{ +public: + + struct Entry + { + std::size_t candidate; + State state; + rmf_traffic::Time wait_until; + State previous_state; + bool require_charge_battery = false; + }; + + // Map finish time to Entry + using Map = std::multimap; + + static std::shared_ptr make( + const std::vector& initial_states, + const std::vector& constraints_set, + const rmf_task::Request& request, + const rmf_task::requests::ChargeBattery& charge_battery_request, + const std::shared_ptr estimate_cache, + TaskPlanner::TaskPlannerError& error); + + 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, + 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, previous_state, require_charge_battery} + }); + } + +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; + } + } +}; + +std::shared_ptr Candidates::make( + const std::vector& initial_states, + const std::vector& constraints_set, + const rmf_task::Request& request, + 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) + { + const auto& state = initial_states[i]; + const auto& constraints = constraints_set[i]; + const auto finish = request.estimate_finish( + state, constraints, estimate_cache); + if (finish.has_value()) + { + initial_map.insert({ + finish.value().finish_state().finish_time(), + Entry{ + i, + finish.value().finish_state(), + finish.value().wait_until(), + state, + false}}); + } + else + { + auto battery_estimate = + charge_battery_request.estimate_finish( + state, constraints, estimate_cache); + if (battery_estimate.has_value()) + { + auto new_finish = request.estimate_finish( + battery_estimate.value().finish_state(), constraints, estimate_cache); + 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}}); + } + 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()) + { + return nullptr; + } + + std::shared_ptr candidates( + new Candidates(std::move(initial_map))); + return candidates; +} + +// ============================================================================ +class PendingTask +{ +public: + + static std::shared_ptr make( + std::vector& initial_states, + std::vector& constraints_set, + rmf_task::ConstRequestPtr request_, + rmf_task::ConstRequestPtr charge_battery_request, + std::shared_ptr estimate_cache, + TaskPlanner::TaskPlannerError& error); + + 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_), + earliest_start_time(request->earliest_start_time()) + { + // Do nothing + } +}; + +std::shared_ptr PendingTask::make( + std::vector& initial_states, + std::vector& constraints_set, + rmf_task::ConstRequestPtr request_, + rmf_task::ConstRequestPtr charge_battery_request, + 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, constraints_set, + *request_, *battery_request, estimate_cache, error); + + if (!candidates) + return nullptr; + + std::shared_ptr pending_task( + new PendingTask(request_, *candidates)); + return pending_task; +} + + + +// ============================================================================ +struct Node +{ + struct AssignmentWrapper + { + std::size_t internal_id; + TaskPlanner::Assignment assignment; + }; + + using AssignedTasks = std::vector>; + 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; + 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() + { + 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, + earliest_start_time, + earliest_finish_time + }); + } + } + + void pop_unassigned(std::size_t task_id) + { + 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; + erase_it = it; + break; + } + } + unassigned_invariants.erase(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; + } +}; + +//============================================================================== +// 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: + + 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({{0, value}}); + } + + void add(const double earliest_start_time, const double earliest_finish_time) + { + 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 (new_end_value <= end_it->back().end) + 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) + { + // 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: + struct element { double start; double end; }; + std::vector> _stacks; +}; + +// ============================================================================ +// The type of filter used for solving the task assignment problem +enum class FilterType +{ + Passthrough, + Trie, + Hash +}; + +// ============================================================================ +class Filter +{ +public: + + Filter(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.internal_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].internal_id != b[j].internal_id) + { + return false; + } + } + } + + return true; + } + }; + + using Set = std::unordered_set; + + FilterType _type; + AgentTable _root; + Set _set; +}; + +bool Filter::ignore(const Node& node) +{ + if (_type == FilterType::Passthrough) + return false; + + if (_type == FilterType::Hash) + return !_set.insert(node.assigned_tasks).second; + + bool new_node = false; + + 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].internal_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); + +// ============================================================================ +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 +{ +public: + + std::shared_ptr config; + std::shared_ptr estimate_cache; + + ConstRequestPtr 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; + for (const auto& agent : assigned_tasks) + { + for (const auto& assignment : agent) + { + cost += compute_g_assignment(assignment); + } + } + + return cost; + } + + TaskPlanner::Assignments prune_assignments( + TaskPlanner::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 + // TODO(YV): Remove this after fixing the planner + if (std::dynamic_pointer_cast( + assignments[a].back().request())) + assignments[a].pop_back(); + } + + return assignments; + } + + Result complete_solve( + rmf_traffic::Time time_now, + std::vector& initial_states, + const std::vector& constraints_set, + const std::vector& requests, + const std::function interrupter, + bool greedy) + { + assert(initial_states.size() == constraints_set.size()); + TaskPlannerError error; + auto node = make_initial_node( + initial_states, constraints_set, requests, time_now, error); + if (!node) + return error; + + TaskPlanner::Assignments complete_assignments; + complete_assignments.resize(node->assigned_tasks.size()); + + while (node) + { + if (greedy) + node = greedy_solve(node, initial_states, constraints_set, time_now); + else + node = solve(node, initial_states, constraints_set, requests.size(), time_now, interrupter); + + if (!node) + 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.assignment); + } + } + + if (node->unassigned_tasks.empty()) + { + return prune_assignments(complete_assignments); + } + + std::vector new_tasks; + for (const auto& u : node->unassigned_tasks) + new_tasks.push_back(u.second.request); + + // copy final state estimates + std::vector estimates; + 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().assignment.state(); + } + + node = make_initial_node( + estimates, constraints_set, new_tasks, time_now, error); + if (!node) + return error; + initial_states = estimates; + } + + return complete_assignments; + } + + double compute_g(const Node& node) + { + 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) + { + 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) + { + const rmf_traffic::Time earliest_deployment_time = + u.second.candidates.best_finish_time() + - u.second.request->invariant_duration(); + 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 (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) + { + 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 = rmf_traffic::time::to_seconds(time_now.time_since_epoch()); + else + value = rmf_traffic::time::to_seconds( + assignments.back().assignment.state().finish_time().time_since_epoch()); + } + } + + 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.earliest_start_time, u.earliest_finish_time); + } + return queue.compute_cost(); + } + + double compute_f(const Node& n, const rmf_traffic::Time time_now) + { + return compute_g(n) + compute_h(n, time_now); + } + + ConstNodePtr make_initial_node( + std::vector initial_states, + std::vector constraints_set, + std::vector requests, + rmf_traffic::Time time_now, + TaskPlannerError& error) + { + auto initial_node = std::make_shared(); + + 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) + { + // 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, + constraints_set, + request, + charge_battery, + estimate_cache, + error); + + if (!pending_task) + return nullptr; + + initial_node->unassigned_tasks.insert( + { + internal_id, + *pending_task + }); + } + + initial_node->cost_estimate = compute_f(*initial_node, time_now); + + 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().assignment.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 time_now, + const std::vector& constraints_set) + + { + const auto& entry = it->second; + const auto& constraints = constraints_set[entry.candidate]; + + 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 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().assignment.request())) + { + auto charge_battery = make_charging_request(entry.previous_state.finish_time()); + auto battery_estimate = charge_battery->estimate_finish( + entry.previous_state, constraints, estimate_cache); + if (battery_estimate.has_value()) + { + assignments.push_back( + 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( + 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); + + // Update states of unassigned tasks for the candidate + bool add_charger = false; + for (auto& new_u : new_node->unassigned_tasks) + { + const auto finish = + new_u.second.request->estimate_finish( + entry.state, constraints, estimate_cache); + + if (finish.has_value()) + { + new_u.second.candidates.update_candidate( + entry.candidate, + finish.value().finish_state(), + finish.value().wait_until(), + entry.state, + false); + } + else + { + // TODO(YV): Revisit this strategy + // auto battery_estimate = + // 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(), + // constraints); + // 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; + } + } + + if (add_charger) + { + auto charge_battery = make_charging_request(entry.state.finish_time()); + auto battery_estimate = charge_battery->estimate_finish( + entry.state, constraints, estimate_cache); + if (battery_estimate.has_value()) + { + new_node->assigned_tasks[entry.candidate].push_back( + { 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 = + new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), + constraints, estimate_cache); + if (finish.has_value()) + { + new_u.second.candidates.update_candidate( + entry.candidate, finish.value().finish_state(), finish.value().wait_until(), entry.state, false); + } + else + { + // We should stop expanding this node + return nullptr; + } + } + + } + else + { + // Agent cannot make it back to the charger + return nullptr; + } + } + + // Update the cost estimate for new_node + new_node->cost_estimate = compute_f(*new_node, time_now); + 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, + const std::vector& constraints_set, + rmf_traffic::Time time_now) + { + auto new_node = std::make_shared(*parent); + // Assign charging task to an agent + State state = initial_states[agent]; + auto& assignments = new_node->assigned_tasks[agent]; + + if (!assignments.empty()) + { + if (std::dynamic_pointer_cast( + assignments.back().assignment.request())) + return nullptr; + state = assignments.back().assignment.state(); + } + + auto charge_battery = make_charging_request(state.finish_time()); + auto estimate = charge_battery->estimate_finish( + state, constraints_set[agent], estimate_cache); + if (estimate.has_value()) + { + new_node->assigned_tasks[agent].push_back( + 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 = + new_u.second.request->estimate_finish(estimate.value().finish_state(), + constraints_set[agent], estimate_cache); + if (finish.has_value()) + { + new_u.second.candidates.update_candidate( + agent, + finish.value().finish_state(), + finish.value().wait_until(), + state, + false); + } + else + { + return nullptr; + } + } + + new_node->cost_estimate = compute_f(*new_node, time_now); + new_node->latest_time = get_latest_time(*new_node); + return new_node; + } + + return nullptr; + } + + ConstNodePtr greedy_solve( + ConstNodePtr node, + const std::vector& initial_states, + const std::vector& constraints_set, + rmf_traffic::Time time_now) + { + 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, time_now, constraints_set)) + { + 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 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) + { + auto parent_node = std::make_shared(*node); + while (!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, + constraints_set, + time_now); + 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, + const std::vector& constraints_set, + rmf_traffic::Time time_now) + { + 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, time_now, constraints_set)) + 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, constraints_set, time_now)) + 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::vector& constraints_set, + const std::size_t num_tasks, + rmf_traffic::Time time_now, + std::function interrupter) + { + using PriorityQueue = std::priority_queue< + ConstNodePtr, + std::vector, + LowestCostEstimate>; + + PriorityQueue priority_queue; + priority_queue.push(std::move(initial_node)); + + Filter filter{FilterType::Hash, 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, constraints_set, time_now); + + // 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{ + config, + std::make_shared( + config->planner()->get_configuration().graph().num_waypoints()) + })) +{ + // Do nothing +} + +// ============================================================================ +auto TaskPlanner::greedy_plan( + rmf_traffic::Time time_now, + std::vector initial_states, + std::vector constraints_set, + std::vector requests) -> Result +{ + return _pimpl->complete_solve( + time_now, + initial_states, + constraints_set, + requests, + nullptr, + true); +} + +// ============================================================================ +auto TaskPlanner::optimal_plan( + rmf_traffic::Time time_now, + std::vector initial_states, + std::vector constraints_set, + std::vector requests, + std::function interrupter) -> Result +{ + return _pimpl->complete_solve( + time_now, + initial_states, + constraints_set, + requests, + interrupter, + 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 diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp new file mode 100644 index 000000000..6d2ebb1f6 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -0,0 +1,221 @@ +/* + * 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_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 +{ +public: + + 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; +}; + +//============================================================================== +rmf_task::ConstRequestPtr ChargeBattery::make( + rmf_battery::agv::BatterySystem 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::shared_ptr charge_battery(new ChargeBattery()); + charge_battery->_pimpl->id += generate_uuid(); + 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); + 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; +} + +//============================================================================== +ChargeBattery::ChargeBattery() +: _pimpl(rmf_utils::make_impl(Implementation())) +{} + +//============================================================================== +std::string ChargeBattery::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional ChargeBattery::estimate_finish( + const agv::State& initial_state, + 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 + // 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 + rmf_traffic::agv::Plan::Start final_plan_start{ + initial_state.finish_time(), + initial_state.charging_waypoint(), + initial_state.location().orientation()}; + agv::State state{ + std::move(final_plan_start), + initial_state.charging_waypoint(), + initial_state.battery_soc()}; + + 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; + double variant_battery_drain = 0.0; + rmf_traffic::Duration variant_duration(0); + + if (initial_state.waypoint() != initial_state.charging_waypoint()) + { + const auto endpoints = std::make_pair(initial_state.waypoint(), + initial_state.charging_waypoint()); + const auto& cache_result = estimate_cache->get(endpoints); + // Use memoized values if possible + if (cache_result) + { + variant_duration = cache_result->duration; + battery_soc = battery_soc - cache_result->dsoc; + } + else + { + // Compute plan to charging waypoint along with battery drain + rmf_traffic::agv::Planner::Goal goal{endpoints.second}; + const auto result = _pimpl->planner->plan( + initial_state.location(), goal); + auto itinerary_start_time = start_time; + for (const auto& itinerary : result->get_itinerary()) + { + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if (_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, + variant_battery_drain); + } + + // If a robot cannot reach its charging dock given its initial battery soc + if (battery_soc <= task_planning_constraints.threshold_soc()) + return rmf_utils::nullopt; + } + + // Default _charge_soc = 1.0 + 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(); + + 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); + + 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; +} + +//============================================================================== +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/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp new file mode 100644 index 000000000..edff11ade --- /dev/null +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -0,0 +1,325 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +namespace rmf_task { +namespace requests { + +//============================================================================== +class Clean::Implementation +{ +public: + + Implementation() + {} + + std::string 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_task::ConstRequestPtr Clean::make( + std::string 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, + rmf_traffic::Time start_time, + bool drain_battery) +{ + 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::string Clean::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional Clean::estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) const +{ + rmf_traffic::agv::Plan::Start final_plan_start{ + initial_state.finish_time(), + _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); + rmf_traffic::Duration end_duration(0); + + const rmf_traffic::Time start_time = initial_state.finish_time(); + double battery_soc = initial_state.battery_soc(); + double dSOC_motion = 0.0; + double dSOC_ambient = 0.0; + + if (initial_state.waypoint() != _pimpl->start_waypoint) + { + 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; + if (_pimpl->drain_battery) + battery_soc = battery_soc - cache_result->dsoc; + } + else + { + 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 + auto itinerary_start_time = start_time; + double variant_battery_drain = 0.0; + for (const auto& itinerary : result_to_start->get_itinerary()) + { + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if(_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, + variant_battery_drain); + } + + if (battery_soc <= task_planning_constraints.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 ? + 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 <= task_planning_constraints.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + + // Factor in invariants + state.finish_time( + wait_until + variant_duration + _pimpl->invariant_duration + end_duration); + + if (_pimpl->drain_battery) + { + battery_soc -= _pimpl->invariant_battery_drain; + 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 + double retreat_battery_drain = 0.0; + if ( _pimpl->end_waypoint != state.charging_waypoint()) + { + 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 + 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); + } + } + + if (battery_soc - retreat_battery_drain <= task_planning_constraints.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; +} + +//============================================================================== +std::size_t Clean::start_waypoint() const +{ + return _pimpl->start_waypoint; +} + +//============================================================================== +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 new file mode 100644 index 000000000..94c0dbc29 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -0,0 +1,352 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +namespace rmf_task { +namespace requests { + +//============================================================================== +class Delivery::Implementation +{ +public: + + 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; +}; + +//============================================================================== +rmf_task::ConstRequestPtr Delivery::make( + 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, + rmf_traffic::Time start_time, + 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; + + // Calculate duration of invariant component of task + delivery->_pimpl->invariant_duration = rmf_traffic::Duration{0}; + delivery->_pimpl->invariant_battery_drain = 0.0; + + if (delivery->_pimpl->pickup_waypoint != delivery->_pimpl->dropoff_waypoint) + { + rmf_traffic::agv::Planner::Start start{ + 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); + + auto itinerary_start_time = start_time; + for (const auto& itinerary : result_to_dropoff->get_itinerary()) + { + 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; + } + } + + return delivery; +} + +//============================================================================== +Delivery::Delivery() +: _pimpl(rmf_utils::make_impl(Implementation())) +{} + +//============================================================================== +std::string Delivery::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional Delivery::estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) const +{ + rmf_traffic::agv::Plan::Start final_plan_start{ + initial_state.finish_time(), + _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); + + 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; + + // 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); + const auto& cache_result = estimate_cache->get(endpoints); + // Use previously memoized values if possible + if (cache_result) + { + variant_duration = cache_result->duration; + if (_pimpl->drain_battery) + battery_soc = battery_soc - cache_result->dsoc; + } + else + { + // Compute plan to pickup waypoint along with battery drain + rmf_traffic::agv::Planner::Goal goal{endpoints.second}; + const auto result_to_pickup = _pimpl->planner->plan( + initial_state.location(), goal); + // We assume we can always compute a plan + auto itinerary_start_time = start_time; + double variant_battery_drain = 0.0; + for (const auto& itinerary : result_to_pickup->get_itinerary()) + { + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if (_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, + variant_battery_drain); + } + + if (battery_soc <= task_planning_constraints.threshold_soc()) + 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 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 <= task_planning_constraints.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + + // 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 <= task_planning_constraints.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()) + { + 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 + 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); + } + } + + if (battery_soc - retreat_battery_drain <= task_planning_constraints.threshold_soc()) + return rmf_utils::nullopt; + + state.battery_soc(battery_soc); + } + + 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; +} + +//============================================================================== +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..6152e1079 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -0,0 +1,366 @@ +/* + * 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::string 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::string 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); + + 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()) + { + 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; + +} + +//============================================================================== +Loop::Loop() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +std::string Loop::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional Loop::estimate_finish( + const agv::State& initial_state, + const agv::Constraints& task_planning_constraints, + const std::shared_ptr estimate_cache) 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) + { + 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) + { + variant_duration = cache_result->duration; + if (_pimpl->drain_battery) + battery_soc = battery_soc - cache_result->dsoc; + } + else + { + // Compute plan to start_waypoint along with battery drain + rmf_traffic::agv::Planner::Goal loop_start_goal{endpoints.second}; + const auto plan_to_start = _pimpl->planner->plan( + initial_state.location(), loop_start_goal); + // We assume we can always compute a plan + auto itinerary_start_time = start_time; + double variant_battery_drain = 0.0; + for (const auto& itinerary : plan_to_start->get_itinerary()) + { + const auto& trajectory = itinerary.trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration itinerary_duration = + finish_time - itinerary_start_time; + + if (_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, + variant_battery_drain); + } + + if (battery_soc <= task_planning_constraints.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; + + // 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 <= task_planning_constraints.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + + // 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 + double retreat_battery_drain = 0.0; + if (_pimpl->drain_battery) + { + battery_soc -= _pimpl->invariant_battery_drain; + if (battery_soc <= task_planning_constraints.threshold_soc()) + return rmf_utils::nullopt; + + if ( _pimpl->finish_waypoint != initial_state.charging_waypoint()) + { + 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 + 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); + } + + if (battery_soc - retreat_battery_drain <= task_planning_constraints.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/main.cpp b/rmf_task/test/main.cpp new file mode 100644 index 000000000..23e837acb --- /dev/null +++ b/rmf_task/test/main.cpp @@ -0,0 +1,21 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#define CATCH_CONFIG_MAIN +#include + +// This will create the main(int argc, char* argv[]) entry point for testing diff --git a/rmf_task/test/unit/agv/test_Constraints.cpp b/rmf_task/test/unit/agv/test_Constraints.cpp new file mode 100644 index 000000000..f0d737bd5 --- /dev/null +++ b/rmf_task/test/unit/agv/test_Constraints.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("Test Constraints") +{ + std::unique_ptr constraints; + + WHEN("Minimum battery threshold") + { + CHECK_NOTHROW( + constraints.reset(new rmf_task::agv::Constraints{0.0})); + } + + WHEN("Maximum battery threshold") + { + CHECK_NOTHROW( + constraints.reset(new rmf_task::agv::Constraints{1.0})); + } + + WHEN("Half battery threshold") + { + CHECK_NOTHROW( + constraints.reset(new rmf_task::agv::Constraints{0.5})); + } + + WHEN("Below minimum battery threshold") + { + CHECK_THROWS( + constraints.reset(new rmf_task::agv::Constraints{0.0 - 1e-4})); + } + + WHEN("Above maximum battery threshold") + { + CHECK_THROWS( + constraints.reset(new rmf_task::agv::Constraints{1.0 + 1e-4})); + } +} diff --git a/rmf_task/test/unit/agv/test_State.cpp b/rmf_task/test/unit/agv/test_State.cpp new file mode 100644 index 000000000..84a558134 --- /dev/null +++ b/rmf_task/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_task::agv::State{ + basic_start, + 0, + 0.0})); + } + + WHEN("Full battery") + { + CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ + basic_start, + 0, + 1.0})); + } + + WHEN("Half battery") + { + CHECK_NOTHROW(basic_state.reset(new rmf_task::agv::State{ + basic_start, + 0, + 0.5})); + } + + WHEN("Battery soc more than 1.0") + { + CHECK_THROWS(basic_state.reset(new rmf_task::agv::State{ + basic_start, + 0, + 1.0 + 1e-4})); + } + + WHEN("Battery soc less than 0.0") + { + CHECK_THROWS(basic_state.reset(new rmf_task::agv::State{ + basic_start, + 0, + 0.0 - 1e-4})); + } +} diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp new file mode 100644 index 000000000..f16aa4cd6 --- /dev/null +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -0,0 +1,925 @@ +/* + * 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 +#include + +#include + +#include + +using TaskPlanner = rmf_task::agv::TaskPlanner; + +//============================================================================== +inline bool check_implicit_charging_task_start( + const 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, + const 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(); + 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; +} + +//============================================================================== +SCENARIO("Grid World") +{ + const int grid_size = 4; + 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; + + 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); + + auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8); + REQUIRE(battery_system_optional); + BatterySystem& battery_system = *battery_system_optional; + auto mechanical_system_optional = MechanicalSystem::make(70.0, 40.0, 0.22); + REQUIRE(mechanical_system_optional); + MechanicalSystem& mechanical_system = *mechanical_system_optional; + auto power_system_optional = PowerSystem::make(20.0); + REQUIRE(power_system_optional); + PowerSystem& power_system_processor = *power_system_optional; + + std::shared_ptr motion_sink = + std::make_shared(battery_system, mechanical_system); + std::shared_ptr device_sink = + std::make_shared(battery_system, power_system_processor); + + 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_task::agv::State{first_location, 13, 1.0}, + rmf_task::agv::State{second_location, 2, 1.0} + }; + + std::vector task_planning_constraints = + { + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} + }; + + std::vector requests = + { + rmf_task::requests::Delivery::make( + "1", + 0, + "dispenser", + 3, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "2", + 15, + "dispenser", + 2, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "3", + 7, + "dispenser", + 9, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery) + }; + + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + TaskPlanner task_planner(task_config); + + auto start_time = std::chrono::steady_clock::now(); + const auto greedy_result = task_planner.greedy_plan( + 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); + + // 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, 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); + + REQUIRE(optimal_cost <= greedy_cost); + } + + 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_task::agv::State{first_location, 13, 1.0}, + rmf_task::agv::State{second_location, 2, 1.0} + }; + + std::vector task_planning_constraints = + { + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} + }; + + std::vector requests = + { + rmf_task::requests::Delivery::make( + "1", + 0, + "dispenser", + 3, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "2", + 15, + "dispenser", + 2, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "3", + 7, + "dispenser", + 9, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "4", + 8, + "dispenser", + 11, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery), + + rmf_task::requests::Delivery::make( + "5", + 10, + "dispenser", + 0, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery), + + rmf_task::requests::Delivery::make( + "6", + 4, + "dispenser", + 8, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(60000), + drain_battery), + + rmf_task::requests::Delivery::make( + "7", + 8, + "dispenser", + 14, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(60000), + drain_battery), + + rmf_task::requests::Delivery::make( + "8", + 5, + "dispenser", + 11, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(60000), + drain_battery), + + rmf_task::requests::Delivery::make( + "9", + 9, + "dispenser", + 0, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(60000), + drain_battery), + + rmf_task::requests::Delivery::make( + "10", + 1, + "dispenser", + 3, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(60000), + drain_battery), + + rmf_task::requests::Delivery::make( + "11", + 0, + "dispenser", + 12, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(60000), + drain_battery) + }; + + std::shared_ptr task_config = + std::make_shared( + battery_system, + motion_sink, + device_sink, + planner); + TaskPlanner task_planner(task_config); + + auto start_time = std::chrono::steady_clock::now(); + const auto greedy_result = task_planner.greedy_plan( + 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); + + // 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, 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); + + 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 task_planning_constraints = + { + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} + }; + + std::vector requests = + { + rmf_task::requests::Delivery::make( + "1", + 0, + "dispenser", + 3, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "2", + 15, + "dispenser", + 2, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "3", + 9, + "dispenser", + 4, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "4", + 8, + "dispenser", + 11, + "ingestor", + {}, + 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); + TaskPlanner task_planner(task_config); + + auto start_time = std::chrono::steady_clock::now(); + const auto greedy_result = task_planner.greedy_plan( + 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); + + // 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, 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); + + + 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); + } + + 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 task_planning_constraints = + { + rmf_task::agv::Constraints{0.2}, + rmf_task::agv::Constraints{0.2} + }; + + std::vector requests = + { + rmf_task::requests::Delivery::make( + "1", + 6, + "dispenser", + 3, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "2", + 10, + "dispenser", + 7, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "3", + 2, + "dispenser", + 12, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(0), + drain_battery), + + rmf_task::requests::Delivery::make( + "4", + 8, + "dispenser", + 11, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery), + + rmf_task::requests::Delivery::make( + "5", + 10, + "dispenser", + 6, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(50000), + drain_battery), + + rmf_task::requests::Delivery::make( + "6", + 2, + "dispenser", + 9, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + "7", + 3, + "dispenser", + 4, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + "8", + 5, + "dispenser", + 11, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + "9", + 9, + "dispenser", + 1, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + "10", + 1, + "dispenser", + 5, + "ingestor", + {}, + motion_sink, + device_sink, + planner, + now + rmf_traffic::time::from_seconds(70000), + drain_battery), + + rmf_task::requests::Delivery::make( + "11", + 13, + "dispenser", + 10, + "ingestor", + {}, + 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); + TaskPlanner task_planner(task_config); + + auto start_time = std::chrono::steady_clock::now(); + const auto greedy_result = task_planner.greedy_plan( + 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); + + // 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, 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); + + + REQUIRE(optimal_cost <= greedy_cost); + } + + 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; + + rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation}; + + std::vector initial_states = + { + rmf_task::agv::State{first_location, 13, 1.0}, + }; + + std::vector task_planning_constraints = + { + rmf_task::agv::Constraints{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); + TaskPlanner task_planner(task_config); + + const auto greedy_result = task_planner.greedy_plan( + now, initial_states, task_planning_constraints, 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::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, task_planning_constraints, 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::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 task_planning_constraints = + { + rmf_task::agv::Constraints{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); + TaskPlanner task_planner(task_config); + + const auto greedy_result = task_planner.greedy_plan( + now, initial_states, task_planning_constraints, 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, task_planning_constraints, 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 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 `_) diff --git a/rmf_task_msgs/CMakeLists.txt b/rmf_task_msgs/CMakeLists.txt index 5d740e7a0..90a1d44ae 100644 --- a/rmf_task_msgs/CMakeLists.txt +++ b/rmf_task_msgs/CMakeLists.txt @@ -22,13 +22,27 @@ set(msg_files "msg/Behavior.msg" "msg/BehaviorParameter.msg" "msg/Station.msg" + "msg/TaskDescription.msg" "msg/TaskSummary.msg" "msg/Tasks.msg" "msg/Loop.msg" "msg/Tow.msg" + "msg/TaskType.msg" + "msg/Clean.msg" + "msg/TaskProfile.msg" + "msg/BidNotice.msg" + "msg/BidProposal.msg" + "msg/DispatchRequest.msg" +) + +set(srv_files + "srv/SubmitTask.srv" + "srv/CancelTask.srv" + "srv/GetTaskList.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/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/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/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 new file mode 100644 index 000000000..7c75dd5a8 --- /dev/null +++ b/rmf_task_msgs/msg/TaskProfile.msg @@ -0,0 +1,7 @@ +# Unique ID to differentiate the task +string task_id + +# Task submission time +builtin_interfaces/Time submission_time + +TaskDescription description 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..a180229b5 --- /dev/null +++ b/rmf_task_msgs/msg/TaskType.msg @@ -0,0 +1,8 @@ +uint32 type +uint32 TYPE_STATION=0 +uint32 TYPE_LOOP=1 +uint32 TYPE_DELIVERY=2 +uint32 TYPE_CHARGE_BATTERY=3 +uint32 TYPE_CLEAN=4 +uint32 TYPE_PATROL=5 + diff --git a/rmf_task_msgs/srv/CancelTask.srv b/rmf_task_msgs/srv/CancelTask.srv new file mode 100644 index 000000000..a8cb16e9e --- /dev/null +++ b/rmf_task_msgs/srv/CancelTask.srv @@ -0,0 +1,12 @@ +# Cancel Task | "Delete" service call + +# Identifier for who is requesting the service +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/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 diff --git a/rmf_task_msgs/srv/SubmitTask.srv b/rmf_task_msgs/srv/SubmitTask.srv new file mode 100644 index 000000000..08d048e87 --- /dev/null +++ b/rmf_task_msgs/srv/SubmitTask.srv @@ -0,0 +1,24 @@ +# Submit Task | POST service call + +# Identifier for who is requesting the service +string requester + +# Desired start time of a task +builtin_interfaces/Time start_time + +# desciption of task +TaskDescription description + +# 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 +bool success + +# generated task ID by dispatcher node +string task_id 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/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..3a9759be0 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -0,0 +1,441 @@ +/* + * 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; + + 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) + { + 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(), "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); + + 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(), "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" + // 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; + + // 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..6946fa98b --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp @@ -0,0 +1,181 @@ +/* + * 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(), "Task was previously terminated"); + _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(), + "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) + _on_terminate_callback(weak_status); + } + } + else + { + // will still provide onchange even if the task_id is unknown. + 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) + _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_DEBUG(_node->get_logger(), "Assign task: [%s] to fleet [%s]", + task_profile.task_id.c_str(), fleet_name.c_str()); + return; +} + +//============================================================================== +bool Client::cancel_task( + const TaskProfile& task_profile) +{ + const auto task_id = task_profile.task_id; + 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(), + "Canceling an unknown task [%s]", task_id.c_str()); + return false; + } + + auto weak_status = _active_task_status[task_id].lock(); + + if (!weak_status) + { + RCLCPP_WARN(_node->get_logger(), "Task was previously terminated"); + _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..9e3b75a10 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp @@ -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. + * +*/ + +#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 server] Received 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..e414dd950 --- /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(), "Add Task [%s] to a bidding 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_DEBUG(node->get_logger(), "Bidding Deadline reached: %s", + id.c_str()); + std::optional winner = std::nullopt; + + if (bidding_task.submissions.size() == 0) + { + RCLCPP_DEBUG(node->get_logger(), + "Bidding task has not received any bids"); + } + else + { + winner = evaluate(bidding_task.submissions); + RCLCPP_INFO(node->get_logger(), + "Determined 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(), "Bidding 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..524097a4a --- /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 is not supported", + 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 diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index d8aeb2040..12c6a12dc 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 bd615558e..0c7c992e4 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 {