From 27ceaf2f1b8f233c8774bbb1c838d9b49eb730ce Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <mxgrey@intrinsic.ai>
Date: Wed, 11 Oct 2023 07:39:10 +0000
Subject: [PATCH] Provide a setting for charge tasks to run indefinitely

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
---
 .../rmf_task/requests/ChargeBattery.hpp        | 10 ++++++++++
 .../rmf_task/requests/ChargeBatteryFactory.hpp | 10 ++++++++++
 .../src/rmf_task/requests/ChargeBattery.cpp    | 18 ++++++++++++++++--
 .../rmf_task/requests/ChargeBatteryFactory.cpp | 13 +++++++++++++
 rmf_task/src/rmf_task/requests/Loop.cpp        |  1 +
 5 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp
index 4f9bd4c..5db3efc 100644
--- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp
+++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp
@@ -63,6 +63,16 @@ class ChargeBattery
       const State& initial_state,
       const Parameters& parameters) const final;
 
+    /// Set the charging task to run indefinitely. This means it will never
+    /// declare itself as finished and must instead be canceled. This can be
+    /// used for idle tasks that are canceled automatically when a task request
+    /// comes in. If indefinite is false, the robot will charge up to its
+    /// designated recharge level.
+    void set_indefinite(bool value);
+
+    /// Should this recharge task run indefinitely?
+    bool indefinite() const;
+
     class Implementation;
   private:
     Description();
diff --git a/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp b/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp
index 14e33d4..59c7178 100644
--- a/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp
+++ b/rmf_task/include/rmf_task/requests/ChargeBatteryFactory.hpp
@@ -56,6 +56,16 @@ class ChargeBatteryFactory : public RequestFactory
     const std::string& requester,
     std::function<rmf_traffic::Time()> time_now_cb);
 
+  /// Set the charging task to run indefinitely. This means it will never
+  /// declare itself as finished and must instead be canceled. This can be used
+  /// for idle tasks that are canceled automatically when a task request comes
+  /// in. If indefinite is false, the robot will charge up to its designated
+  /// recharge level.
+  void set_indefinite(bool value);
+
+  /// Does this factory produce charging requests that will run indefinitely?
+  bool indefinite() const;
+
   /// Documentation inherited
   ConstRequestPtr make_request(const State& state) const final;
 
diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp
index 3329d2b..a34aaf0 100644
--- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp
+++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp
@@ -80,7 +80,8 @@ ChargeBattery::Model::Model(
 
 //==============================================================================
 std::optional<rmf_task::Estimate>
-ChargeBattery::Model::estimate_finish(const State& initial_state,
+ChargeBattery::Model::estimate_finish(
+  const State& initial_state,
   const Constraints& task_planning_constraints,
   const TravelEstimator& travel_estimator) const
 {
@@ -162,7 +163,8 @@ rmf_traffic::Duration ChargeBattery::Model::invariant_duration() const
 //==============================================================================
 class ChargeBattery::Description::Implementation
 {
-
+public:
+  bool indefinite = false;
 };
 
 //==============================================================================
@@ -201,6 +203,18 @@ auto ChargeBattery::Description::generate_info(
   };
 }
 
+//==============================================================================
+void ChargeBattery::Description::set_indefinite(bool value)
+{
+  _pimpl->indefinite = value;
+}
+
+//==============================================================================
+bool ChargeBattery::Description::indefinite() const
+{
+  return _pimpl->indefinite;
+}
+
 //==============================================================================
 ConstRequestPtr ChargeBattery::make(
   rmf_traffic::Time earliest_start_time,
diff --git a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp
index 1200d69..7569c84 100644
--- a/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp
+++ b/rmf_task/src/rmf_task/requests/ChargeBatteryFactory.cpp
@@ -27,6 +27,7 @@ class ChargeBatteryFactory::Implementation
 public:
   std::optional<std::string> requester;
   std::function<rmf_traffic::Time()> time_now_cb;
+  bool indefinite = false;
 };
 
 //==============================================================================
@@ -47,6 +48,18 @@ ChargeBatteryFactory::ChargeBatteryFactory(
   // Do nothing
 }
 
+//==============================================================================
+void ChargeBatteryFactory::set_indefinite(bool value)
+{
+  _pimpl->indefinite = value;
+}
+
+//==============================================================================
+bool ChargeBatteryFactory::indefinite() const
+{
+  return _pimpl->indefinite;
+}
+
 //==============================================================================
 ConstRequestPtr ChargeBatteryFactory::make_request(const State& state) const
 {
diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp
index ea45126..2d771be 100644
--- a/rmf_task/src/rmf_task/requests/Loop.cpp
+++ b/rmf_task/src/rmf_task/requests/Loop.cpp
@@ -78,6 +78,7 @@ Loop::Model::Model(
     auto itinerary_start_time = _earliest_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();