From 906018d5309017d1b0085d48cf03f7443a1f8e4d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 5 Apr 2022 20:40:55 +0800 Subject: [PATCH] Add API for reporting status of perform_action Signed-off-by: Michael X. Grey --- .../agv/RobotUpdateHandle.hpp | 20 +++++++++-- .../agv/RobotUpdateHandle.cpp | 36 +++++++++++++++++++ .../agv/internal_RobotUpdateHandle.hpp | 5 ++- .../events/PerformAction.cpp | 9 ++++- rmf_fleet_adapter_python/src/adapter.cpp | 10 ++++-- 5 files changed, 72 insertions(+), 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 index dd631ed35..b79091202 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -119,13 +119,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 text); + + /// Set task status to error and optionally log a message (error tier) + void error(std::optional text); + + /// Set the task status to delayed and optionally log a message + /// (warning tier) + void delayed(std::optional text); + + /// Set the task status to blocked and optionally log a message + /// (warning tier) + void blocked(std::optional 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 index faffd3a96..61fed1172 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -517,6 +517,42 @@ void RobotUpdateHandle::ActionExecution::update_remaining_time( _pimpl->data->remaining_time = remaining_time_estimate; } +//============================================================================== +void RobotUpdateHandle::ActionExecution::underway( + std::optional 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 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 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 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 index 3e180d3c7..bfa4075c4 100644 --- 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 +#include namespace rmf_fleet_adapter { namespace agv { @@ -31,17 +32,19 @@ class RobotUpdateHandle::ActionExecution::Implementation struct Data { - std::function finished; + std::shared_ptr state; std::optional remaining_time; bool okay; // TODO: Consider adding a mutex to lock read/write Data( std::function finished_, + std::shared_ptr state_, std::optional 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 index b2da11748..1b053a205 100644 --- 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(_finished, std::nullopt); + auto finished = [state = _state, cb = _finished]() + { + state->update_status(Status::Completed); + cb(); + }; + + auto data = std::make_shared( + 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 index 8b11a2dbb..4f8a9bfed 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -185,11 +185,15 @@ PYBIND11_MODULE(rmf_adapter, m) { py::class_( 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_(