Add mutex groups to lanes and break the supergraph at mutex group swi…

…tches

Signed-off-by: Michael X. Grey <[email protected]>

rmf_traffic/include/rmf_traffic/agv/Graph.hpp

@@ -162,6 +162,17 @@ class Graph
       const std::string& name_format = "%s",
       const std::string& index_format = "#%d") const;
 
+    /// Get the mutex group that this waypoint is associated with. An empty
+    /// string implies that it is not associated with any mutex group.
+    ///
+    /// Only one robot at a time is allowed to occupy any waypoint or lane
+    /// associated with a particular mutex group.
+    const std::string& in_mutex_group() const;
+
+    /// Set what mutex group this waypoint is associated with. Passing in an
+    /// empty string will disasscoiate the waypoint from any mutex group.
+    Waypoint& set_in_mutex_group(std::string group_name);
+
     class Implementation;
   private:
     Waypoint();
@@ -511,6 +522,7 @@ class Graph
 
       /// Construct a default set of properties
       /// * speed_limit: nullopt
+      /// * mutex_group: ""
       Properties();
 
       /// Get the speed limit along this lane. If a std::nullopt is returned,
@@ -521,6 +533,17 @@ class Graph
       /// indicates that there is no speed limit for the lane.
       Properties& speed_limit(std::optional<double> value);
 
+      /// Get the mutex group that this lane is associated with. An empty string
+      /// implies that it is not associated with any mutex group.
+      ///
+      /// Only one robot at a time is allowed to occupy any waypoint or lane
+      /// associated with a particular mutex group.
+      const std::string& in_mutex_group() const;
+
+      /// Set what mutex group this lane is associated with. Passing in an
+      /// empty string will disassociate the lane from any mutex group.
+      Properties& set_in_mutex_group(std::string group_name);
+
       class Implementation;
     private:
       rmf_utils::impl_ptr<Implementation> _pimpl;

rmf_traffic/src/rmf_traffic/agv/Graph.cpp

@@ -126,6 +126,8 @@ class Graph::Waypoint::Implementation
 
   LiftPropertiesPtr in_lift = nullptr;
 
+  std::string mutex_group = "";
+
   template<typename... Args>
   static Waypoint make(Args&& ... args)
   {
@@ -273,6 +275,19 @@ std::string Graph::Waypoint::name_or_index(
     + index_format.substr(it+2);
 }
 
+//==============================================================================
+const std::string& Graph::Waypoint::in_mutex_group() const
+{
+  return _pimpl->mutex_group;
+}
+
+//==============================================================================
+auto Graph::Waypoint::set_in_mutex_group(std::string group_name) -> Waypoint&
+{
+  _pimpl->mutex_group = std::move(group_name);
+  return *this;
+}
+
 //==============================================================================
 Graph::Waypoint::Waypoint()
 {
@@ -769,6 +784,7 @@ class Graph::Lane::Properties::Implementation
 public:
 
   std::optional<double> speed_limit;
+  std::string mutex_group;
 
 };
 
@@ -793,6 +809,20 @@ auto Graph::Lane::Properties::speed_limit(std::optional<double> value)
   return *this;
 }
 
+//==============================================================================
+const std::string& Graph::Lane::Properties::in_mutex_group() const
+{
+  return _pimpl->mutex_group;
+}
+
+//==============================================================================
+auto Graph::Lane::Properties::set_in_mutex_group(std::string group_name)
+-> Properties&
+{
+  _pimpl->mutex_group = std::move(group_name);
+  return *this;
+}
+
 //==============================================================================
 class Graph::Lane::Implementation
 {

rmf_traffic/src/rmf_traffic/agv/planning/Supergraph.cpp

@@ -80,6 +80,8 @@ struct TraversalNode
   Graph::Lane::EventPtr entry_event;
   Graph::Lane::EventPtr exit_event;
 
+  std::string mutex_group;
+
   // TODO(MXG): Replace this with a more intelligent way of handling speed limit
   // changes that may occur when traversing multiple consecutive lanes
   std::optional<double> lowest_speed_limit;
@@ -299,6 +301,7 @@ void perform_traversal(
   node.initial_waypoint_index = wp_index_0;
   node.finish_waypoint_index = wp_index_1;
   node.lowest_speed_limit = lane.properties().speed_limit();
+  node.mutex_group = lane.properties().in_mutex_group();
 
   if (parent)
   {
@@ -309,6 +312,31 @@ void perform_traversal(
       return;
     }
 
+    const auto& wp0_mutex = wp0.in_mutex_group();
+    if (!parent->mutex_group.empty() && wp0_mutex.empty())
+    {
+      // We are exiting a mutex group, so we should do a quick stop here to make
+      // sure that we release the mutex.
+      return;
+    }
+
+    if (!node.mutex_group.empty() && parent->mutex_group != node.mutex_group)
+    {
+      // The lane belongs to a different mutex group than the parent, so we
+      // cannot continuously traverse.
+      return;
+    }
+
+    const auto& wp1_mutex = wp1.in_mutex_group();
+    if (!wp1_mutex.empty() && wp1_mutex != node.mutex_group)
+    {
+      // The end waypoint of this lane belongs to a different mutex group than
+      // the lane leading up to it. The waypoint's mutex must be locked before
+      // traversing this lane, so we should stop before traversing this lane
+      // instead of continuing from a parent.
+      return;
+    }
+
     node.initial_lane_index = parent->initial_lane_index;
     node.initial_waypoint_index = parent->initial_waypoint_index;
     node.initial_p = parent->initial_p;