Add override_status API

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -101,6 +101,12 @@ class RobotUpdateHandle
   /// charge as a fraction of its total charge capacity
   void update_battery_soc(const double battery_soc);
 
+  /// Use this function to override the robot status. The string provided must
+  /// be a valid enum as specified the in robot_state.json schema.
+  /// Pass std::nullopt to cancel the override and allow RMF to automatically
+  /// update the status. Default is std::nullopt.
+  void override_status(std::optional<std::string> status);
+
   /// Specify how high the delay of the current itinerary can become before it
   /// gets interrupted and replanned. A nullopt value will allow for an
   /// arbitrarily long delay to build up without being interrupted.

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -819,6 +819,9 @@ bool TaskManager::cancel_task_if_present(const std::string& task_id)
 //==============================================================================
 std::string TaskManager::robot_status() const
 {
+  if (_context->override_status().has_value())
+    return _context->override_status().value();
+
   if (!_active_task)
     return "idle";
 

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -395,6 +395,18 @@ uint32_t RobotContext::current_mode() const
   return _current_mode;
 }
 
+//==============================================================================
+void RobotContext::override_status(std::optional<std::string> status)
+{
+  _override_status = status;
+}
+
+//==============================================================================
+std::optional<std::string> RobotContext::override_status() const
+{
+  return _override_status;
+}
+
 //==============================================================================
 void RobotContext::action_executor(
   RobotUpdateHandle::ActionExecutor action_executor)
@@ -476,6 +488,7 @@ RobotContext::RobotContext(
   _battery_soc_obs = _battery_soc_publisher.get_observable();
 
   _current_mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE;
+  _override_status = std::nullopt;
 
   _action_executor = nullptr;
 }

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp

@@ -204,11 +204,20 @@ class RobotContext
 
   /// Set the current mode of the robot. This mode should correspond to a
   /// constant in the RobotMode message
+  [[deprecated]]
   void current_mode(uint32_t mode);
 
   /// Return the current mode of the robot
+  [[deprecated]]
   uint32_t current_mode() const;
 
+  /// Set the current mode of the robot.
+  /// Specify a valid string as specified in the robot_state.json schema
+  void override_status(std::optional<std::string> status);
+
+  /// Return the current mode of the robot
+  std::optional<std::string> override_status() const;
+
   /// Set the action executor for requesting this robot to execute a
   /// PerformAction activity
   void action_executor(RobotUpdateHandle::ActionExecutor action_executor);
@@ -284,6 +293,7 @@ class RobotContext
 
   // Mode value for RobotMode message
   uint32_t _current_mode;
+  std::optional<std::string> _override_status;
 
   RobotUpdateHandle::ActionExecutor _action_executor;
   Reporting _reporting;

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp

@@ -208,6 +208,72 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc)
   }
 }
 
+//==============================================================================
+void RobotUpdateHandle::override_status(std::optional<std::string> status)
+{
+
+  if (const auto context = _pimpl->get_context())
+  {
+
+    if (status.has_value())
+    {
+      // Here we capture [this] to avoid potential costly copy of
+      // schema_dictionary when more enties are inserted in the future.
+      // It is permissible here since the lambda will only be used within the
+      // scope of this function.
+      const auto loader =
+        [context, this](
+        const nlohmann::json_uri& id,
+        nlohmann::json& value)
+        {
+          const auto it = _pimpl->schema_dictionary.find(id.url());
+          if (it == _pimpl->schema_dictionary.end())
+          {
+            RCLCPP_ERROR(
+              context->node()->get_logger(),
+              "url: %s not found in schema dictionary. "
+              "Status for robot [%s] will not be overwritten.",
+              id.url().c_str(),
+              context->name().c_str());
+            return;
+          }
+
+          value = it->second;
+        };
+
+      try
+      {
+        static const auto validator =
+          nlohmann::json_schema::json_validator(
+          rmf_api_msgs::schemas::robot_state, loader);
+
+        nlohmann::json dummy_msg;
+        dummy_msg["status"] = status.value();
+        validator.validate(dummy_msg);
+
+      }
+      catch (const std::exception& e)
+      {
+        RCLCPP_ERROR(
+          context->node()->get_logger(),
+          "Encountered error: %s. Please ensure the override status is a "
+          "valid string as per the robot_state.json schema. The status for "
+          "robot [%s] will not over overwritten.",
+          e.what(),
+          context->name().c_str()
+        );
+        return;
+      }
+    }
+
+    context->worker().schedule(
+      [context, status](const auto&)
+      {
+        context->override_status(status);
+      });
+  }
+}
+
 //==============================================================================
 RobotUpdateHandle& RobotUpdateHandle::maximum_delay(
   rmf_utils::optional<rmf_traffic::Duration> value)

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp

@@ -21,6 +21,10 @@
 #include "RobotContext.hpp"
 #include <rmf_fleet_adapter/agv/RobotUpdateHandle.hpp>
 
+#include <nlohmann/json.hpp>
+#include <nlohmann/json-schema.hpp>
+#include <rmf_api_msgs/schemas/robot_state.hpp>
+
 namespace rmf_fleet_adapter {
 namespace agv {
 
@@ -73,6 +77,8 @@ class RobotUpdateHandle::Implementation
   std::string name;
   RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::Unstable();
   bool reported_loss = false;
+  std::unordered_map<std::string, nlohmann::json> schema_dictionary = {};
+
 
   static std::shared_ptr<RobotUpdateHandle> make(RobotContextPtr context)
   {
@@ -83,6 +89,18 @@ class RobotUpdateHandle::Implementation
       Implementation{std::move(context), std::move(name)});
     handle._pimpl->unstable._pimpl =
       &RobotUpdateHandle::Implementation::get(handle);
+
+    // Initialize schema dictionary
+    const std::vector<nlohmann::json> schemas = {
+      rmf_api_msgs::schemas::robot_state,
+    };
+
+    for (const auto& schema : schemas)
+    {
+      const auto json_uri = nlohmann::json_uri{schema["$id"]};
+      handle._pimpl->schema_dictionary.insert({json_uri.url(), schema});
+    }
+
     return std::make_shared<RobotUpdateHandle>(std::move(handle));
   }