Add API for reporting status of perform_action (#190)

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
open-rmf · Apr 8, 2022 · e9743b9 · e9743b9
1 parent 4cf5d18
commit e9743b9
Show file tree

Hide file tree

Showing 5 changed files with 72 additions and 8 deletions.
diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp
@@ -125,13 +125,27 @@ class RobotUpdateHandle
   class ActionExecution
   {
   public:
-    // Update the amount of time remaining for this action
+    /// Update the amount of time remaining for this action
     void update_remaining_time(rmf_traffic::Duration remaining_time_estimate);
 
-    // Trigger this when the action is finished
+    /// Set task status to underway and optionally log a message (info tier)
+    void underway(std::optional<std::string> text);
+
+    /// Set task status to error and optionally log a message (error tier)
+    void error(std::optional<std::string> text);
+
+    /// Set the task status to delayed and optionally log a message
+    /// (warning tier)
+    void delayed(std::optional<std::string> text);
+
+    /// Set the task status to blocked and optionally log a message
+    /// (warning tier)
+    void blocked(std::optional<std::string> text);
+
+    /// Trigger this when the action is successfully finished
     void finished();
 
-    // Returns false if the Action has been killed or cancelled
+    /// Returns false if the Action has been killed or cancelled
     bool okay() const;
 
     class Implementation;

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp
@@ -583,6 +583,42 @@ void RobotUpdateHandle::ActionExecution::update_remaining_time(
   _pimpl->data->remaining_time = remaining_time_estimate;
 }
 
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::underway(
+  std::optional<std::string> text)
+{
+  _pimpl->data->state->update_status(rmf_task::Event::Status::Underway);
+  if (text.has_value())
+    _pimpl->data->state->update_log().info(*text);
+}
+
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::error(
+  std::optional<std::string> text)
+{
+  _pimpl->data->state->update_status(rmf_task::Event::Status::Error);
+  if (text.has_value())
+    _pimpl->data->state->update_log().error(*text);
+}
+
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::delayed(
+  std::optional<std::string> text)
+{
+  _pimpl->data->state->update_status(rmf_task::Event::Status::Delayed);
+  if (text.has_value())
+    _pimpl->data->state->update_log().warn(*text);
+}
+
+//==============================================================================
+void RobotUpdateHandle::ActionExecution::blocked(
+  std::optional<std::string> text)
+{
+  _pimpl->data->state->update_status(rmf_task::Event::Status::Blocked);
+  if (text.has_value())
+    _pimpl->data->state->update_log().warn(*text);
+}
+
 //==============================================================================
 void RobotUpdateHandle::ActionExecution::finished()
 {

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp
@@ -20,6 +20,7 @@
 
 #include "RobotContext.hpp"
 #include <rmf_fleet_adapter/agv/RobotUpdateHandle.hpp>
+#include <rmf_task/events/SimpleEventState.hpp>
 
 #include <nlohmann/json.hpp>
 #include <nlohmann/json-schema.hpp>
@@ -36,17 +37,19 @@ class RobotUpdateHandle::ActionExecution::Implementation
 
   struct Data
   {
-
     std::function<void()> finished;
+    std::shared_ptr<rmf_task::events::SimpleEventState> state;
     std::optional<rmf_traffic::Duration> remaining_time;
     bool okay;
     // TODO: Consider adding a mutex to lock read/write
 
     Data(
       std::function<void()> finished_,
+      std::shared_ptr<rmf_task::events::SimpleEventState> state_,
       std::optional<rmf_traffic::Duration> remaining_time_ = std::nullopt)
     {
       finished = std::move(finished_);
+      state = std::move(state_);
       remaining_time = remaining_time_;
       okay = true;
     }

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp
@@ -253,7 +253,14 @@ void PerformAction::Active::_execute_action()
     return;
   }
 
-  auto data = std::make_shared<ExecutionData>(_finished, std::nullopt);
+  auto finished = [state = _state, cb = _finished]()
+    {
+      state->update_status(Status::Completed);
+      cb();
+    };
+
+  auto data = std::make_shared<ExecutionData>(
+    std::move(finished), _state, std::nullopt);
   _execution_data = data;
 
   auto action_execution =

diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp
@@ -187,11 +187,15 @@ PYBIND11_MODULE(rmf_adapter, m) {
 
   py::class_<ActionExecution>(
     m_robot_update_handle, "ActionExecution")
-  .def("finished", &ActionExecution::finished)
-  .def("okay", &ActionExecution::okay)
   .def("update_remaining_time",
     &ActionExecution::update_remaining_time,
-    py::arg("remaining_time_estimate"));
+    py::arg("remaining_time_estimate"))
+  .def("underway", &ActionExecution::underway, py::arg("text"))
+  .def("error", &ActionExecution::error, py::arg("text"))
+  .def("delayed", &ActionExecution::delayed, py::arg("text"))
+  .def("blocked", &ActionExecution::blocked, py::arg("text"))
+  .def("finished", &ActionExecution::finished)
+  .def("okay", &ActionExecution::okay);
 
   // ROBOT INTERRUPTION   ====================================================
   py::class_<RobotInterruption>(