Add alarm emergency response (#162)

rmf_fleet_adapter/CHANGELOG.rst

@@ -9,6 +9,7 @@ Forthcoming
 ------------------
 * Traffic Light API: [#147](https://github.com/osrf/rmf_core/pull/147)
 * Allow fleet adapters to adjust the maximum delay: [#148](https://github.com/osrf/rmf_core/pull/148)
+* Full Control Fleet Adapters respond to emergency alarm topic: [#162](https://github.com/osrf/rmf_core/pull/162)
 
 1.0.2 (2020-07-27)
 ------------------

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -19,6 +19,28 @@
 
 namespace rmf_fleet_adapter {
 
+//==============================================================================
+TaskManagerPtr TaskManager::make(agv::RobotContextPtr context)
+{
+  auto mgr = TaskManagerPtr(new TaskManager(std::move(context)));
+  mgr->_emergency_sub = mgr->_context->node()->emergency_notice()
+    .observe_on(rxcpp::identity_same_worker(mgr->_context->worker()))
+    .subscribe(
+      [w = mgr->weak_from_this()](const auto& msg)
+    {
+      if (auto mgr = w.lock())
+      {
+        if (auto task = mgr->_active_task)
+        {
+          if (auto phase = task->current_phase())
+            phase->emergency_alarm(msg->data);
+        }
+      }
+    });
+
+  return mgr;
+}
+
 //==============================================================================
 TaskManager::TaskManager(agv::RobotContextPtr context)
   : _context(std::move(context))

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp

@@ -31,11 +31,11 @@ namespace rmf_fleet_adapter {
 /// optimal. A better task manager would queue tasks across the whole fleet
 /// instead of queuing tasks for each robot individually. We will attempt that
 /// in a later implementation.
-class TaskManager
+class TaskManager : public std::enable_shared_from_this<TaskManager>
 {
 public:
 
-  TaskManager(agv::RobotContextPtr context);
+  static std::shared_ptr<TaskManager> make(agv::RobotContextPtr context);
 
   using Start = rmf_traffic::agv::Plan::Start;
   using StartSet = rmf_traffic::agv::Plan::StartSet;
@@ -52,15 +52,21 @@ class TaskManager
   agv::ConstRobotContextPtr context() const;
 
 private:
+
+  TaskManager(agv::RobotContextPtr context);
+
   agv::RobotContextPtr _context;
   std::shared_ptr<Task> _active_task;
   std::vector<std::shared_ptr<Task>> _queue;
   rmf_utils::optional<Start> _expected_finish_location;
   rxcpp::subscription _task_sub;
+  rxcpp::subscription _emergency_sub;
 
   void _begin_next_task();
 };
 
+using TaskManagerPtr = std::shared_ptr<TaskManager>;
+
 } // namespace rmf_fleet_adapter
 
 #endif // SRC__RMF_FLEET_ADAPTER__TASKMANAGER_HPP

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -83,7 +83,7 @@ auto FleetUpdateHandle::Implementation::estimate_delivery(
   DeliveryEstimate best;
   for (const auto& element : task_managers)
   {
-    const auto& mgr = element.second;
+    const auto& mgr = *element.second;
     auto start = mgr.expected_finish_location();
     const auto pickup_plan = planner->plan(start, pickup_goal);
     if (!pickup_plan)
@@ -132,7 +132,7 @@ void FleetUpdateHandle::Implementation::perform_delivery(
     const rmf_task_msgs::msg::Delivery& request,
     const DeliveryEstimate& estimate)
 {
-  auto& mgr = task_managers.at(estimate.robot);
+  auto& mgr = *task_managers.at(estimate.robot);
   mgr.queue_task(
         tasks::make_delivery(
           request,
@@ -175,7 +175,7 @@ auto FleetUpdateHandle::Implementation::estimate_loop(
     LoopEstimate estimate;
     estimate.robot = element.first;
 
-    const auto& mgr = element.second;
+    const auto& mgr = *element.second;
     auto start = mgr.expected_finish_location();
     const auto loop_init_plan = planner->plan(start, loop_start_goal);
     if (!loop_init_plan)
@@ -264,7 +264,7 @@ void FleetUpdateHandle::Implementation::perform_loop(
     const LoopEstimate& estimate)
 {
   auto& mgr = task_managers.at(estimate.robot);
-  mgr.queue_task(
+  mgr->queue_task(
         tasks::make_loop(
           request,
           estimate.robot,
@@ -333,7 +333,7 @@ void FleetUpdateHandle::add_robot(
         "Added a robot named [%s] with participant ID [%d]",
         context->name().c_str(), context->itinerary().id());
 
-      fleet->_pimpl->task_managers.insert({context, context});
+      fleet->_pimpl->task_managers.insert({context, TaskManager::make(context)});
       if (handle_cb)
       {
         handle_cb(RobotUpdateHandle::Implementation::make(std::move(context)));

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp

@@ -18,6 +18,7 @@
 #include "Node.hpp"
 
 #include <rmf_fleet_adapter/StandardNames.hpp>
+#include <rmf_traffic_ros2/StandardNames.hpp>
 
 namespace rmf_fleet_adapter {
 namespace agv {
@@ -50,6 +51,8 @@ std::shared_ptr<Node> Node::make(
         DispenserResultTopicName, default_qos);
   node->_dispenser_state_obs = node->create_observable<DispenserState>(
         DispenserStateTopicName, default_qos);
+  node->_emergency_notice_obs = node->create_observable<EmergencyNotice>(
+        rmf_traffic_ros2::EmergencyTopicName, default_qos);
 
   return node;
 }
@@ -118,5 +121,11 @@ auto Node::dispenser_state() const -> const DispenserStateObs&
   return _dispenser_state_obs;
 }
 
+//==============================================================================
+auto Node::emergency_notice() const -> const EmergencyNoticeObs&
+{
+  return _emergency_notice_obs;
+}
+
 } // namespace agv
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp

@@ -29,6 +29,7 @@
 #include <rmf_lift_msgs/msg/lift_request.hpp>
 #include <rmf_lift_msgs/msg/lift_state.hpp>
 #include <rmf_task_msgs/msg/task_summary.hpp>
+#include <std_msgs/msg/bool.hpp>
 
 namespace rmf_fleet_adapter {
 namespace agv {
@@ -79,6 +80,10 @@ class Node : public rmf_rxcpp::Transport
   using DispenserStateObs = rxcpp::observable<DispenserState::SharedPtr>;
   const DispenserStateObs& dispenser_state() const;
 
+  using EmergencyNotice = std_msgs::msg::Bool;
+  using EmergencyNoticeObs = rxcpp::observable<EmergencyNotice::SharedPtr>;
+  const EmergencyNoticeObs& emergency_notice() const;
+
 private:
 
   Node(
@@ -95,6 +100,7 @@ class Node : public rmf_rxcpp::Transport
   DispenserRequestPub _dispenser_request_pub;
   DispenserResultObs _dispenser_result_obs;
   DispenserStateObs _dispenser_state_obs;
+  EmergencyNoticeObs _emergency_notice_obs;
 };
 
 } // namespace agv

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp

@@ -121,7 +121,7 @@ class FleetUpdateHandle::Implementation
       std::chrono::nanoseconds(std::chrono::seconds(10));
 
   AcceptDeliveryRequest accept_delivery = nullptr;
-  std::unordered_map<RobotContextPtr, TaskManager> task_managers = {};
+  std::unordered_map<RobotContextPtr, std::shared_ptr<TaskManager>> task_managers = {};
 
   template<typename... Args>
   static std::shared_ptr<FleetUpdateHandle> make(Args&&... args)

rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp

@@ -55,12 +55,12 @@ rmf_traffic::Duration GoToPlace::Active::estimate_remaining_time() const
 }
 
 //==============================================================================
-void GoToPlace::Active::emergency_alarm(const bool on)
+void GoToPlace::Active::emergency_alarm(const bool value)
 {
-  if (_emergency_active == on)
+  if (_emergency_active == value)
     return;
 
-  _emergency_active = on;
+  _emergency_active = value;
   if (_emergency_active)
   {
     cancel();