Allow fleet adapters to change participant profiles

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -351,6 +351,11 @@ class RobotUpdateHandle
     /// Get the schedule participant of this robot
     rmf_traffic::schedule::Participant* get_participant();
 
+    /// Change the radius of the footprint and vicinity of this participant.
+    void change_participant_profile(
+      double footprint_radius,
+      double vicinity_radius);
+
     /// Override the schedule to say that the robot will be holding at a certain
     /// position. This should not be used while tasks with automatic schedule
     /// updating are running, or else the traffic schedule will have jumbled up

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp

@@ -18,6 +18,8 @@
 #include "internal_RobotUpdateHandle.hpp"
 #include "../TaskManager.hpp"
 
+#include <rmf_traffic/geometry/Circle.hpp>
+
 #include <rmf_traffic_ros2/Time.hpp>
 
 #include <iostream>
@@ -687,6 +689,40 @@ RobotUpdateHandle::Unstable::get_participant()
   return nullptr;
 }
 
+//==============================================================================
+void RobotUpdateHandle::Unstable::change_participant_profile(
+  double footprint_radius,
+  double vicinity_radius)
+{
+  const auto vicinity = [&]() -> rmf_traffic::geometry::FinalConvexShapePtr
+    {
+      if (vicinity_radius <= footprint_radius)
+        return nullptr;
+
+      return rmf_traffic::geometry::make_final_convex(
+        rmf_traffic::geometry::Circle(vicinity_radius));
+    } ();
+
+  const auto footprint = rmf_traffic::geometry::make_final_convex(
+    rmf_traffic::geometry::Circle(footprint_radius));
+
+  rmf_traffic::Profile profile(footprint, vicinity);
+  if (const auto context = _pimpl->get_context())
+  {
+    context->worker().schedule(
+      [
+        w = context->weak_from_this(),
+        profile = std::move(profile)
+      ](const auto&)
+      {
+        if (const auto context = w.lock())
+        {
+          context->itinerary().change_profile(profile);
+        }
+      });
+  }
+}
+
 //==============================================================================
 void RobotUpdateHandle::Unstable::declare_holding(
   std::string on_map,

rmf_fleet_adapter_python/src/adapter.cpp

@@ -177,6 +177,16 @@ PYBIND11_MODULE(rmf_adapter, m) {
     },
     py::return_value_policy::reference_internal,
     "Experimental API to access the schedule participant")
+  .def("unstable_change_participant_profile",
+    [&](agv::RobotUpdateHandle& self,
+    double footprint_radius,
+    double vicinity_radius)
+    {
+      self.unstable().change_participant_profile(
+        footprint_radius, vicinity_radius);
+    },
+    py::arg("footprint_radius"),
+    py::arg("vicinity_radius"))
   .def("unstable_declare_holding",
     [&](agv::RobotUpdateHandle& self,
     std::string on_map,