request factories to accept a callback that returns the current time

Signed-off-by: Aaron Chong <[email protected]>

rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp

@@ -18,11 +18,13 @@
 #ifndef RMF_TASK__REQUESTS__FACTORY__CHARGEBATTERYFACTORY_HPP
 #define RMF_TASK__REQUESTS__FACTORY__CHARGEBATTERYFACTORY_HPP
 
+#include <functional>
 #include <optional>
 #include <string>
 
 #include <rmf_task/RequestFactory.hpp>
 #include <rmf_task/State.hpp>
+#include <rmf_traffic/Time.hpp>
 
 #include <rmf_utils/impl_ptr.hpp>
 
@@ -47,7 +49,12 @@ class ChargeBatteryFactory : public RequestFactory
   /// \param[in] requester
   ///   The identifier of the entity that owns this RequestFactory, that will be
   ///   the designated requester of each new request.
-  explicit ChargeBatteryFactory(const std::string& requester);
+  ///
+  /// \param[in] time_now_cb
+  ///   Callback function that returns the current time.
+  explicit ChargeBatteryFactory(
+    const std::string& requester,
+    std::function<rmf_traffic::Time()> time_now_cb);
 
   /// Documentation inherited
   ConstRequestPtr make_request(const State& state) const final;

rmf_task/include/rmf_task/requests/ParkRobotFactory.hpp

@@ -18,11 +18,13 @@
 #ifndef RMF_TASK__REQUESTS__FACTORY__PARKROBOTFACTORY_HPP
 #define RMF_TASK__REQUESTS__FACTORY__PARKROBOTFACTORY_HPP
 
+#include <functional>
 #include <optional>
 #include <string>
 
 #include <rmf_task/RequestFactory.hpp>
 #include <rmf_task/State.hpp>
+#include <rmf_traffic/Time.hpp>
 
 #include <rmf_utils/impl_ptr.hpp>
 
@@ -54,13 +56,17 @@ class ParkRobotFactory : public RequestFactory
   ///   The identifier of the entity that owns this RequestFactory, that will be
   ///   the designated requester of each new request.
   ///
+  /// \param[in] time_now_cb
+  ///   Callback function that returns the current time.
+  ///
   /// \param[in] parking_waypoint
   ///   The graph index of the waypoint assigned to this AGV for parking.
   ///   If nullopt, the AGV will return to its charging_waypoint and remain idle
   ///   there. It will not wait for its battery to charge up before undertaking
   ///   new tasks.
   ParkRobotFactory(
     const std::string& requester,
+    std::function<rmf_traffic::Time()> time_now_cb,
     std::optional<std::size_t> parking_waypoint = std::nullopt);
 
   /// Documentation inherited

rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp

@@ -26,31 +26,37 @@ class ChargeBatteryFactory::Implementation
 {
 public:
   std::optional<std::string> requester;
+  std::function<rmf_traffic::Time()> time_now_cb;
 };
 
 //==============================================================================
 ChargeBatteryFactory::ChargeBatteryFactory()
-: _pimpl(rmf_utils::make_impl<Implementation>(Implementation{std::nullopt}))
+: _pimpl(rmf_utils::make_impl<Implementation>(
+      Implementation{std::nullopt, nullptr}))
 {
   // Do nothing
 }
 
 //==============================================================================
-ChargeBatteryFactory::ChargeBatteryFactory(const std::string& requester)
-: _pimpl(rmf_utils::make_impl<Implementation>(Implementation{requester}))
+ChargeBatteryFactory::ChargeBatteryFactory(
+  const std::string& requester,
+  std::function<rmf_traffic::Time()> time_now_cb)
+: _pimpl(rmf_utils::make_impl<Implementation>(
+      Implementation{requester, std::move(time_now_cb)}))
 {
   // Do nothing
 }
 
 //==============================================================================
 ConstRequestPtr ChargeBatteryFactory::make_request(const State& state) const
 {
-  if (_pimpl->requester.has_value())
+
+  if (_pimpl->requester.has_value() && _pimpl->time_now_cb)
   {
     return ChargeBattery::make(
       state.time().value(),
       _pimpl->requester.value(),
-      state.time().value(),
+      _pimpl->time_now_cb(),
       nullptr,
       true);
   }

rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp

@@ -49,24 +49,26 @@ class ParkRobotFactory::Implementation
 {
 public:
   std::optional<std::string> requester;
+  std::function<rmf_traffic::Time()> time_now_cb;
   std::optional<std::size_t> parking_waypoint;
 };
 
 //==============================================================================
 ParkRobotFactory::ParkRobotFactory(
   std::optional<std::size_t> parking_waypoint)
 : _pimpl(rmf_utils::make_impl<Implementation>(
-      Implementation{std::nullopt, std::move(parking_waypoint)}))
+      Implementation{std::nullopt, nullptr, std::move(parking_waypoint)}))
 {
   // Do nothing
 }
 
 //==============================================================================
 ParkRobotFactory::ParkRobotFactory(
   const std::string& requester,
+  std::function<rmf_traffic::Time()> time_now_cb,
   std::optional<std::size_t> parking_waypoint)
-: _pimpl(rmf_utils::make_impl<Implementation>(
-      Implementation{requester, std::move(parking_waypoint)}))
+: _pimpl(rmf_utils::make_impl<Implementation>(Implementation{
+      requester, std::move(time_now_cb), std::move(parking_waypoint)}))
 {
   // Do nothing
 }
@@ -80,7 +82,7 @@ ConstRequestPtr ParkRobotFactory::make_request(const State& state) const
     _pimpl->parking_waypoint.value() :
     state.dedicated_charging_waypoint().value();
 
-  if (_pimpl->requester.has_value())
+  if (_pimpl->requester.has_value() && _pimpl->time_now_cb)
   {
     return Loop::make(
       start_waypoint,
@@ -89,7 +91,7 @@ ConstRequestPtr ParkRobotFactory::make_request(const State& state) const
       id,
       state.time().value(),
       _pimpl->requester.value(),
-      state.time().value(),
+      _pimpl->time_now_cb(),
       nullptr,
       true);
   }