Allow system integrators to specify an update listener

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -328,6 +328,12 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   FleetUpdateHandle& fleet_state_update_period(
     std::optional<rmf_traffic::Duration> value);
 
+  /// Set a callback for listening to update messages (e.g. fleet states and
+  /// task updates). This will not receive any update messages that happened
+  /// before the listener was set.
+  FleetUpdateHandle& set_update_listener(
+    std::function<void(const nlohmann::json&)> listener);
+
   // Do not allow moving
   FleetUpdateHandle(FleetUpdateHandle&&) = delete;
   FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete;

rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp

@@ -156,6 +156,17 @@ std::shared_ptr<BroadcastClient> BroadcastClient::make(
 //==============================================================================
 void BroadcastClient::publish(const nlohmann::json& msg)
 {
+  {
+    const auto fleet = _fleet_handle.lock();
+    if (!fleet)
+      return;
+
+    auto& impl = agv::FleetUpdateHandle::Implementation::get(*fleet);
+    std::unique_lock<std::mutex> lock(*impl.update_callback_mutex);
+    if (impl.update_callback)
+      impl.update_callback(msg);
+  }
+
   std::lock_guard<std::mutex> lock(_queue_mutex);
   _queue.push(msg);
   _cv.notify_all();
@@ -164,6 +175,20 @@ void BroadcastClient::publish(const nlohmann::json& msg)
 //==============================================================================
 void BroadcastClient::publish(const std::vector<nlohmann::json>& msgs)
 {
+  {
+    const auto fleet = _fleet_handle.lock();
+    if (!fleet)
+      return;
+
+    auto& impl = agv::FleetUpdateHandle::Implementation::get(*fleet);
+    std::unique_lock<std::mutex> lock(*impl.update_callback_mutex);
+    if (impl.update_callback)
+    {
+      for (const auto& msg : msgs)
+        impl.update_callback(msg);
+    }
+  }
+
   std::lock_guard<std::mutex> lock(_queue_mutex);
   for (const auto& msg : msgs)
     _queue.push(msg);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -1789,6 +1789,15 @@ FleetUpdateHandle& FleetUpdateHandle::fleet_state_update_period(
   return *this;
 }
 
+//==============================================================================
+FleetUpdateHandle& FleetUpdateHandle::set_update_listener(
+  std::function<void(const nlohmann::json&)> listener)
+{
+  std::unique_lock<std::mutex> lock(*_pimpl->update_callback_mutex);
+  _pimpl->update_callback = std::move(listener);
+  return *this;
+}
+
 //==============================================================================
 bool FleetUpdateHandle::set_task_planner_params(
   std::shared_ptr<rmf_battery::agv::BatterySystem> battery_system,

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp

@@ -243,6 +243,10 @@ class FleetUpdateHandle::Implementation
   std::shared_ptr<rmf_traffic_ros2::schedule::Negotiation> negotiation;
   std::optional<std::string> server_uri;
 
+  std::shared_ptr<std::mutex> update_callback_mutex =
+    std::make_shared<std::mutex>();
+  std::function<void(const nlohmann::json&)> update_callback;
+
   TaskActivation activation = TaskActivation();
   TaskDeserialization deserialization = TaskDeserialization();
 

rmf_fleet_adapter_python/src/adapter.cpp

@@ -362,6 +362,10 @@ PYBIND11_MODULE(rmf_adapter, m) {
     "Specify a period for how often the fleet state message is published for\
      this fleet. Passing in None will disable the fleet state message\
      publishing. The default value is 1s")
+  .def("set_update_listener",
+    &agv::FleetUpdateHandle::set_update_listener,
+    py::arg("listener"),
+    "Provide a callback that will receive fleet state and task updates.")
   .def("consider_delivery_requests",
      [&](agv::FleetUpdateHandle& self,
          ModifiedConsiderRequest consider_pickup,