From 5d49caace476a8d2a06777e1994227e66e5d2c1a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 11 Sep 2022 14:29:10 +0800 Subject: [PATCH 1/2] API for robot de/recommisioning Signed-off-by: Michael X. Grey --- .../agv/RobotUpdateHandle.hpp | 13 +++++++ .../agv/FleetUpdateHandle.cpp | 4 ++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 18 +++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 9 +++++ .../agv/RobotUpdateHandle.cpp | 37 +++++++++++++++++++ 5 files changed, 81 insertions(+) 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 79cadb5a3..65563abdd 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -323,6 +323,19 @@ class RobotUpdateHandle class Unstable { public: + /// True if this robot is allowed to accept new tasks. False if the robot + /// will not accept any new tasks. + bool is_commissioned() const; + + /// Stop this robot from accepting any new tasks. It will continue to + /// perform tasks that are already in its queue. To reassign those tasks, + /// you will need to use the task request API to cancel the tasks and + /// re-request them. + void decommission(); + + /// Allow this robot to resume accepting new tasks. + void recommission(); + /// Get the schedule participant of this robot rmf_traffic::schedule::Participant* get_participant(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index f56bfca16..098685cf1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1132,6 +1132,10 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const Expectations expect; for (const auto& t : task_managers) { + // Ignore any robots that are not currently commissioned. + if (!t.first->is_commissioned()) + continue; + expect.states.push_back(t.second->expected_finish_state()); const auto requests = t.second->requests(); expect.pending_requests.insert( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index c9f2ee3ac..35374046d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -435,6 +435,24 @@ std::shared_ptr RobotContext::task_manager() return _task_manager.lock(); } +//============================================================================== +bool RobotContext::is_commissioned() const +{ + return _commissioned; +} + +//============================================================================== +void RobotContext::decommission() +{ + _commissioned = false; +} + +//============================================================================== +void RobotContext::recommission() +{ + _commissioned = true; +} + //============================================================================== Reporting& RobotContext::reporting() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 9a4747544..ab1e53202 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -229,6 +229,14 @@ class RobotContext /// Get the task manager for this robot, if it exists. std::shared_ptr task_manager(); + /// Return true if this robot is currently commissioned (available to accept + /// new tasks). + bool is_commissioned() const; + + void decommission(); + + void recommission(); + Reporting& reporting(); const Reporting& reporting() const; @@ -290,6 +298,7 @@ class RobotContext RobotUpdateHandle::Unstable::Watchdog _lift_watchdog; rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0); + bool _commissioned = true; // Mode value for RobotMode message uint32_t _current_mode; 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 47f69cb21..db5c5250b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -625,6 +625,43 @@ const RobotUpdateHandle::Unstable& RobotUpdateHandle::unstable() const return _pimpl->unstable; } +//============================================================================== +bool RobotUpdateHandle::Unstable::is_commissioned() const +{ + if (const auto context = _pimpl->get_context()) + return context->is_commissioned(); + + return false; +} + +//============================================================================== +void RobotUpdateHandle::Unstable::decommission() +{ + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [w = context->weak_from_this()](const auto&) + { + if (const auto context = w.lock()) + context->decommission(); + }); + } +} + +//============================================================================== +void RobotUpdateHandle::Unstable::recommission() +{ + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [w = context->weak_from_this()](const auto&) + { + if (const auto context = w.lock()) + context->recommission(); + }); + } +} + //============================================================================== rmf_traffic::schedule::Participant* RobotUpdateHandle::Unstable::get_participant() From b702b9765fcc5df58f8c91e03355d6a71965e3bc Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 11 Sep 2022 14:48:38 +0800 Subject: [PATCH 2/2] Add Python API for de/recommissioning Signed-off-by: Michael X. Grey --- rmf_fleet_adapter_python/src/adapter.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 2a06ec3cb..b9bc2d814 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -138,6 +138,25 @@ PYBIND11_MODULE(rmf_adapter, m) { self.maximum_delay(duration); }, py::arg("seconds")) + .def("unstable_is_commissioned", + [&](const agv::RobotUpdateHandle& self) + { + return self.unstable().is_commissioned(); + }, + "Check if the robot is currently allowed to accept any new tasks") + .def("unstable_decommission", + [&](agv::RobotUpdateHandle& self) + { + return self.unstable().decommission(); + }, + "Stop this robot from accepting any new tasks. Use recommission to resume") + .def("unstable_recommission", + [&](agv::RobotUpdateHandle& self) + { + return self.unstable().recommission(); + }, + "Allow this robot to resume accepting new tasks if it was ever " + "decommissioned in the past") .def("get_unstable_participant", [&](agv::RobotUpdateHandle& self) {