Load the parking waypoint into RobotContext; access it in adapter and FleetUpdateHandle

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -102,6 +102,11 @@ class RobotUpdateHandle
   /// property will be assumed as the charger for this robot.
   RobotUpdateHandle& set_charger_waypoint(const std::size_t charger_wp);
 
+  /// Set the waypoint where the parking spot for this robot is located.
+  /// If not specified, the nearest waypoint in the graph with the is_parking_spot()
+  /// property will be assumed as the parking spot for this robot.
+  RobotUpdateHandle& set_parking_waypoint(const std::size_t parking_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);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -788,6 +788,31 @@ get_nearest_charger(
   return nearest_charger;
 }
 
+//==============================================================================
+std::optional<std::size_t> FleetUpdateHandle::Implementation::
+get_nearest_parking_wp(
+  const rmf_traffic::agv::Planner::Start& start)
+{
+  if (parking_waypoints.empty())
+    return std::nullopt;
+
+  double min_cost = std::numeric_limits<double>::max();
+  std::optional<std::size_t> nearest_parking_wp = std::nullopt;
+  for (const auto& wp : parking_waypoints)
+  {
+    const rmf_traffic::agv::Planner::Goal goal{wp};
+    const auto& planner_result = (*planner)->setup(start, goal);
+    const auto ideal_cost = planner_result.ideal_cost();
+    if (ideal_cost.has_value() && ideal_cost.value() < min_cost)
+    {
+      min_cost = ideal_cost.value();
+      nearest_parking_wp = wp;
+    }
+  }
+
+  return nearest_parking_wp;
+}
+
 namespace {
 //==============================================================================
 std::optional<rmf_fleet_msgs::msg::Location> convert_location(
@@ -1319,6 +1344,7 @@ void FleetUpdateHandle::add_robot(
         return;
 
       const auto charger_wp = fleet->_pimpl->get_nearest_charger(start[0]);
+      const auto parking_wp = fleet->_pimpl->get_nearest_parking_wp(start[0]);
 
       if (!charger_wp.has_value())
       {
@@ -1330,8 +1356,19 @@ void FleetUpdateHandle::add_robot(
         // *INDENT-ON*
       }
 
+      if (!parking_wp.has_value())
+      {
+        // *INDENT-OFF*
+        throw std::runtime_error(
+          "[FleetUpdateHandle::add_robot] Unable to find nearest parking "
+          "waypoint. Adding a robot to a fleet requires at least one parking"
+          "waypoint to be present in its navigation graph.");
+        // *INDENT-ON*
+      }
+
       rmf_task::State state;
       state.load_basic(start[0], charger_wp.value(), 1.0);
+      state.load_parking_waypoint(parking_wp.value());
 
       auto context = std::make_shared<RobotContext>(
         RobotContext

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -275,7 +275,9 @@ std::function<rmf_task::State()> RobotContext::make_get_state()
         self->_location.front(),
         self->_charger_wp,
         self->_current_battery_soc);
-
+      state.load_parking_waypoint(
+        self->_parking_wp);
+
       state.insert<GetContext>(GetContext{self->shared_from_this()});
       return state;
     };
@@ -318,6 +320,12 @@ std::size_t RobotContext::dedicated_charger_wp() const
   return _charger_wp;
 }
 
+//==============================================================================
+std::size_t RobotContext::dedicated_parking_wp() const
+{
+  return _parking_wp;
+}
+
 //==============================================================================
 const rxcpp::observable<double>& RobotContext::observe_battery_soc() const
 {
@@ -491,6 +499,7 @@ RobotContext::RobotContext(
   _requester_id(
     _itinerary.description().owner() + "/" + _itinerary.description().name()),
   _charger_wp(state.dedicated_charging_waypoint().value()),
+  _parking_wp(state.dedicated_parking_waypoint().value()),
   _current_task_end_state(state),
   _current_task_id(std::nullopt),
   _task_planner(std::move(task_planner)),

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp

@@ -182,8 +182,12 @@ class RobotContext
   /// publishes the battery soc via _battery_soc_publisher.
   RobotContext& current_battery_soc(const double battery_soc);
 
+  /// Get the dedicated charging waypoint for the robot
   std::size_t dedicated_charger_wp() const;
 
+  /// Get the dedicated parking waypoint for the robot
+  std::size_t dedicated_parking_wp() const;
+
   // Get a reference to the battery soc observer of this robot.
   const rxcpp::observable<double>& observe_battery_soc() const;
 
@@ -289,6 +293,7 @@ class RobotContext
   /// Always call the current_battery_soc() setter to set a new value
   double _current_battery_soc = 1.0;
   std::size_t _charger_wp;
+  std::size_t _parking_wp;
   rxcpp::subjects::subject<double> _battery_soc_publisher;
   rxcpp::observable<double> _battery_soc_obs;
   rmf_task::State _current_task_end_state;

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp

@@ -207,6 +207,25 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint(
   return *this;
 }
 
+//==============================================================================
+RobotUpdateHandle& RobotUpdateHandle::set_parking_waypoint(
+  const std::size_t parking_wp)
+{
+  if (const auto context = _pimpl->get_context())
+  {
+    auto end_state = context->current_task_end_state();
+    end_state.dedicated_parking_waypoint(parking_wp);
+    context->current_task_end_state(end_state);
+    RCLCPP_INFO(
+      context->node()->get_logger(),
+      "Parking waypoint for robot [%s] set to index [%ld]",
+      context->name().c_str(),
+      parking_wp);
+  }
+
+  return *this;
+}
+
 //==============================================================================
 void RobotUpdateHandle::update_battery_soc(const double battery_soc)
 {

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp

@@ -295,6 +295,7 @@ class FleetUpdateHandle::Implementation
 
   // TODO Support for various charging configurations
   std::unordered_set<std::size_t> charging_waypoints = {};
+  std::unordered_set<std::size_t> parking_waypoints = {};
 
   std::shared_ptr<rmf_task_ros2::bidding::AsyncBidder> bidder = nullptr;
 
@@ -426,6 +427,13 @@ class FleetUpdateHandle::Implementation
         handle->_pimpl->charging_waypoints.insert(i);
     }
 
+    // Populate parking waypoints
+    for (std::size_t i = 0; i < graph.num_waypoints(); ++i)
+    {
+      if (graph.get_waypoint(i).is_parking_spot())
+        handle->_pimpl->parking_waypoints.insert(i);
+    }
+
     // Initialize schema dictionary
     const std::vector<nlohmann::json> schemas = {
       rmf_api_msgs::schemas::fleet_state_update,
@@ -554,6 +562,9 @@ class FleetUpdateHandle::Implementation
   std::optional<std::size_t> get_nearest_charger(
     const rmf_traffic::agv::Planner::Start& start);
 
+  std::optional<std::size_t> get_nearest_parking_wp(
+    const rmf_traffic::agv::Planner::Start& start);
+
   struct Expectations
   {
     std::vector<rmf_task::State> states;

rmf_fleet_adapter_python/src/adapter.cpp

@@ -115,6 +115,8 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("start_set"))
   .def("set_charger_waypoint", &agv::RobotUpdateHandle::set_charger_waypoint,
     py::arg("charger_wp"))
+  .def("set_parking_waypoint", &agv::RobotUpdateHandle::set_parking_waypoint,
+    py::arg("parking_wp"))
   .def("update_battery_soc", &agv::RobotUpdateHandle::update_battery_soc,
     py::arg("battery_soc"))
   .def("override_status", &agv::RobotUpdateHandle::override_status,