Update Traffic API to provide deadlock blockers (#180)

* Update Traffic API to provide deadlock blockers * Update CHANGELOG
osrf · Sep 28, 2020 · 70e3aa0 · 70e3aa0
1 parent 1ca2c4d
commit 70e3aa0
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 4 deletions.
diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst
@@ -7,7 +7,7 @@ Forthcoming
 
 1.1.0 (2020-09-XX)
 ------------------
-* Traffic Light API: [#147](https://github.com/osrf/rmf_core/pull/147) [#176](https://github.com/osrf/rmf_core/pull/176)
+* Traffic Light API: [#147](https://github.com/osrf/rmf_core/pull/147) [#176](https://github.com/osrf/rmf_core/pull/176) [#180](https://github.com/osrf/rmf_core/pull/180)
 * 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)
 * Migrating to ROS2 Foxy: [#133](https://github.com/osrf/rmf_core/pull/133)

diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/TrafficLight.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/TrafficLight.hpp
@@ -19,6 +19,7 @@
 #define RMF_FLEET_ADAPTER__AGV__TRAFFICLIGHT_HPP
 
 #include <rmf_fleet_adapter/agv/Waypoint.hpp>
+#include <rmf_traffic/schedule/ParticipantDescription.hpp>
 
 #include <memory>
 #include <vector>
@@ -133,11 +134,28 @@ class TrafficLight
         std::vector<Checkpoint> checkpoints,
         OnStandby on_standby) = 0;
 
+    /// This class is given to the deadlock() function to describe the
+    /// participants that are blocking the robot and creating the deadlock.
+    class Blocker
+    {
+    public:
+
+      /// Get the schedule participant ID of the blocker.
+      rmf_traffic::schedule::ParticipantId participand_id() const;
+
+      /// Get the description of the blocker.
+      const rmf_traffic::schedule::ParticipantDescription& description() const;
+
+      class Implementation;
+    private:
+      rmf_utils::impl_ptr<Implementation> _pimpl;
+    };
+
     /// This function will be called when deadlock has occurred due to an
     /// unresolvable conflict. Human intervention may be required at this point,
     /// because the RMF traffic negotiation system does not have a high enough
     /// level of control over the conflicting participants to resolve it.
-    virtual void deadlock() = 0;
+    virtual void deadlock(std::vector<Blocker> blockers) = 0;
 
     virtual ~CommandHandle() = default;
   };

diff --git a/rmf_fleet_adapter/src/mock_traffic_light/main.cpp b/rmf_fleet_adapter/src/mock_traffic_light/main.cpp
@@ -118,7 +118,7 @@ class MockTrafficLightCommandHandle
     _moving = true;
   }
 
-  void deadlock() final
+  void deadlock(std::vector<Blocker>) final
   {
     _deadlock = true;
     _stop_request.path.clear();

diff --git a/rmf_fleet_adapter/test/mock/MockTrafficLightCommand.hpp b/rmf_fleet_adapter/test/mock/MockTrafficLightCommand.hpp
@@ -45,7 +45,7 @@ class MockTrafficLightCommand
     cv.notify_all();
   }
 
-  void deadlock() final
+  void deadlock(std::vector<Blocker>) final
   {
     // Do nothing
   }