update ParkRobotFactory to read and use robot's dedicated parking waypoint; load parking waypoint to robot state

rmf_task/include/rmf_task/State.hpp

@@ -57,6 +57,11 @@ class State : public CompositeData
   std::optional<std::size_t> dedicated_charging_waypoint() const;
   State& dedicated_charging_waypoint(std::size_t new_charging_waypoint);
 
+  /// The dedicated parking spot for this robot
+  RMF_TASK_DEFINE_COMPONENT(std::size_t, DedicatedParkingPoint);
+  std::optional<std::size_t> dedicated_parking_waypoint() const;
+  State& dedicated_parking_waypoint(std::size_t new_parking_waypoint);
+
   /// The current battery state of charge of the robot. This value is between
   /// 0.0 and 1.0.
   RMF_TASK_DEFINE_COMPONENT(double, CurrentBatterySoC);
@@ -78,6 +83,12 @@ class State : public CompositeData
     std::size_t charging_point,
     double battery_soc);
 
+  /// load the dedicated parking waypoint to the robot's State
+  ///
+  /// \param[in] parking_point
+  ///   The robot's dedicated parking point.
+  State& load_parking_waypoint(std::size_t parking_point);
+
   /// Load the plan start into the State. The location info will be split into
   /// CurrentWaypoint, CurrentOrientation, and CurrentTime data.
   State& load(const rmf_traffic::agv::Plan::Start& location);

rmf_task/include/rmf_task/Task.hpp

@@ -80,14 +80,11 @@ class Task::Booking
   /// \param[in] automatic
   ///   Whether this booking was automatically generated
   ///
-  /// \param[in] labels
-  ///   Labels to describe the purpose of the task dispatch request.
   Booking(
     std::string id,
     rmf_traffic::Time earliest_start_time,
     ConstPriorityPtr priority,
-    bool automatic = false,
-    const std::vector<std::string>& labels = {});
+    bool automatic = false);
 
   /// Constructor
   ///
@@ -110,16 +107,13 @@ class Task::Booking
   ///   Whether this booking was automatically generated, default value as
   ///   false.
   ///
-  /// \param[in] labels
-  ///   Labels to describe the purpose of the task dispatch request.
   Booking(
     std::string id,
     rmf_traffic::Time earliest_start_time,
     ConstPriorityPtr priority,
     const std::string& requester,
     rmf_traffic::Time request_time,
-    bool automatic = false,
-    const std::vector<std::string>& labels = {});
+    bool automatic = false);
 
   /// The unique id for this booking.
   const std::string& id() const;
@@ -141,9 +135,6 @@ class Task::Booking
   // Returns true if this booking was automatically generated.
   bool automatic() const;
 
-  /// Get the labels that describe the purpose of the task dispatch request.
-  std::vector<std::string> labels() const;
-
   class Implementation;
 private:
   rmf_utils::impl_ptr<Implementation> _pimpl;

rmf_task/src/rmf_task/State.cpp

@@ -85,6 +85,22 @@ State& State::dedicated_charging_waypoint(std::size_t new_charging_waypoint)
   return *this;
 }
 
+//==============================================================================
+std::optional<std::size_t> State::dedicated_parking_waypoint() const
+{
+  if (const auto* p = get<DedicatedParkingPoint>())
+    return p->value;
+
+  return std::nullopt;
+}
+
+//==============================================================================
+State& State::dedicated_parking_waypoint(std::size_t new_parking_waypoint)
+{
+  with<DedicatedParkingPoint>(new_parking_waypoint);
+  return *this;
+}
+
 //==============================================================================
 std::optional<double> State::battery_soc() const
 {
@@ -121,6 +137,13 @@ State& State::load_basic(
   return *this;
 }
 
+//==============================================================================
+State& State::load_parking_waypoint(std::size_t input_parking_point)
+{
+  dedicated_parking_waypoint(input_parking_point);
+  return *this;
+}
+
 //==============================================================================
 State& State::load(const rmf_traffic::agv::Plan::Start& location)
 {

rmf_task/src/rmf_task/Task.cpp

@@ -31,25 +31,22 @@ class Task::Booking::Implementation
   std::optional<std::string> requester;
   std::optional<rmf_traffic::Time> request_time;
   bool automatic;
-  std::vector<std::string> labels;
 };
 
 //==============================================================================
 Task::Booking::Booking(
   std::string id,
   rmf_traffic::Time earliest_start_time,
   ConstPriorityPtr priority,
-  bool automatic,
-  const std::vector<std::string>& labels)
+  bool automatic)
 : _pimpl(rmf_utils::make_impl<Implementation>(
       Implementation{
         std::move(id),
         earliest_start_time,
         std::move(priority),
         std::nullopt,
         std::nullopt,
-        automatic,
-        labels,
+        automatic
       }))
 {
   // Do nothing
@@ -62,17 +59,15 @@ Task::Booking::Booking(
   ConstPriorityPtr priority,
   const std::string& requester,
   rmf_traffic::Time request_time,
-  bool automatic,
-  const std::vector<std::string>& labels)
+  bool automatic)
 : _pimpl(rmf_utils::make_impl<Implementation>(
       Implementation{
         std::move(id),
         earliest_start_time,
         std::move(priority),
         requester,
         std::move(request_time),
-        automatic,
-        labels,
+        automatic
       }))
 {
   // Do nothing
@@ -114,12 +109,6 @@ bool Task::Booking::automatic() const
   return _pimpl->automatic;
 }
 
-//==============================================================================
-std::vector<std::string> Task::Booking::labels() const
-{
-  return _pimpl->labels;
-}
-
 //==============================================================================
 class Task::Tag::Implementation
 {

rmf_task/src/rmf_task/requests/ChargeBattery.cpp

@@ -109,6 +109,9 @@ ChargeBattery::Model::estimate_finish(
     std::move(final_plan_start),
     initial_state.dedicated_charging_waypoint().value(),
     initial_state.battery_soc().value());
+  if (initial_state.dedicated_parking_waypoint()) {
+    state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value());
+  }
 
   double battery_soc = initial_state.battery_soc().value();
   rmf_traffic::Duration variant_duration(0);

rmf_task/src/rmf_task/requests/Clean.cpp

@@ -98,6 +98,9 @@ std::optional<rmf_task::Estimate> Clean::Model::estimate_finish(
     std::move(final_plan_start),
     initial_state.dedicated_charging_waypoint().value(),
     initial_state.battery_soc().value());
+  if (initial_state.dedicated_parking_waypoint()) {
+    state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value());
+  }
 
   rmf_traffic::Duration variant_duration(0);
   rmf_traffic::Duration end_duration(0);

rmf_task/src/rmf_task/requests/Delivery.cpp

@@ -118,6 +118,9 @@ std::optional<rmf_task::Estimate> Delivery::Model::estimate_finish(
     std::move(final_plan_start),
     initial_state.dedicated_charging_waypoint().value(),
     initial_state.battery_soc().value());
+  if (initial_state.dedicated_parking_waypoint()) {
+    state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value());
+  }
 
   rmf_traffic::Duration variant_duration(0);
 

rmf_task/src/rmf_task/requests/Loop.cpp

@@ -178,6 +178,9 @@ std::optional<rmf_task::Estimate> Loop::Model::estimate_finish(
     std::move(location),
     initial_state.dedicated_charging_waypoint().value(),
     battery_soc);
+  if (initial_state.dedicated_parking_waypoint()) {
+    finish_state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value());
+  }
 
   // Check if robot can return to its charger
   if (drain_battery)

rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp

@@ -77,10 +77,12 @@ ParkRobotFactory::ParkRobotFactory(
 ConstRequestPtr ParkRobotFactory::make_request(const State& state) const
 {
   std::string id = "ParkRobot" + generate_uuid();
-  const auto start_waypoint = state.waypoint().value();
-  const auto finish_waypoint = _pimpl->parking_waypoint.has_value() ?
-    _pimpl->parking_waypoint.value() :
-    state.dedicated_charging_waypoint().value();
+  const auto start_waypoint = state.waypoint().value_or(0);
+
+  const std::size_t finish_waypoint = 
+    _pimpl->parking_waypoint.has_value()? 
+    _pimpl->parking_waypoint.value() : 
+    state.dedicated_parking_waypoint().value_or(start_waypoint);
 
   if (_pimpl->requester.has_value() && _pimpl->time_now_cb)
   {