Add persistence to Traffic Schedule Participant IDs (#242)

Signed-off-by: Michael X. Grey <[email protected]>
Co-authored-by: Arjo Chakravarty <[email protected]>
Co-authored-by: Michael X. Grey <[email protected]>

Author: arjo129

rmf_fleet_adapter/rmf_rxcpp/examples/PlanPath.cpp

@@ -295,7 +295,7 @@ struct MetaPlannerAction
       rmf_traffic::schedule::ItineraryVersion v = 0;
       rmf_traffic::schedule::Version r = 0;
       for (const auto& route : alternative)
-        database.extend(p, {{r, route}}, v++);
+        database.extend(p.id(), {{r, route}}, v++);
 
       planner_actions.emplace_back(std::make_shared<PlannerAction>(
               std::move(database), config, start_B, goal_B, sync_failure));

rmf_traffic/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package rmf_traffic
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.3.0 (2021-XX-XX)
+------------------
+* Add persistence to Traffic Schedule Participant IDs: [#242](https://github.com/osrf/rmf_core/pull/242)
+
 1.2.0 (2021-01-05)
 ------------------
 * Improve planner performance scaling for large graphs: [#243](https://github.com/osrf/rmf_core/pull/243)

rmf_traffic/include/rmf_traffic/schedule/Database.hpp

@@ -81,7 +81,7 @@ class Database :
     ItineraryVersion version) final;
 
   // Documentation inherited from Writer
-  ParticipantId register_participant(
+  Registration register_participant(
     ParticipantDescription participant_info) final;
 
   /// Before calling this function on a Database, you should set the current
@@ -199,11 +199,16 @@ class Database :
   /// call this function for any other purpose.
   void set_current_time(Time time);
 
-  /// Get the curret itinerary version for the specified participant.
+  /// Get the current itinerary version for the specified participant.
   //
   // TODO(MXG): This function needs unit testing
   ItineraryVersion itinerary_version(ParticipantId participant) const;
 
+  /// Get the last Route ID used by this participant.
+  //
+  // TODO(MXG): This function needs unit testing
+  RouteId last_route_id(ParticipantId participant) const;
+
   class Implementation;
   class Debug;
 private:

rmf_traffic/include/rmf_traffic/schedule/Writer.hpp

@@ -122,8 +122,50 @@ class Writer
     const std::vector<RouteId>& routes,
     ItineraryVersion version) = 0;
 
-  // TODO(MXG): Consider saying "add" instead of "register" and "remove" instead
-  // of "unregister".
+  /// Information resulting from registering a participant
+  class Registration
+  {
+  public:
+
+    /// Constructor
+    ///
+    /// \param[in] id
+    ///   The ID for the registered participant
+    ///
+    /// \param[in] version
+    ///   The last itinerary version for the registered participant
+    ///
+    /// \param[in] route_id
+    ///   The last route_id for the registered participant
+    Registration(
+      ParticipantId id,
+      ItineraryVersion version,
+      RouteId route_id);
+
+    /// The ID of the registered participant
+    ParticipantId id() const;
+
+    /// The last itinerary version of the registered participant. New
+    /// Participants will begin by adding up from this version when issuing
+    /// schedule updates.
+    ///
+    /// This value might vary for systems that enforce participant uniqueness.
+    /// If this participant was registered in the past and is now being
+    /// re-registered, then the version number will pick up where it previously
+    /// left off.
+    ItineraryVersion last_itinerary_version() const;
+
+    /// The last Route ID of the registered participant. New Participants will
+    /// begin by adding up from this Route ID when issuing new schedule updates.
+    ///
+    /// Similar to last_itinerary_version, this value might vary for systems
+    /// that enforce participant uniqueness.
+    RouteId last_route_id() const;
+
+    class Implementation;
+  private:
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
 
   /// Register a new participant.
   ///
@@ -134,7 +176,7 @@ class Writer
   ///   The time at which the registration is being requested.
   ///
   /// \return result of registering the new participant.
-  virtual ParticipantId register_participant(
+  virtual Registration register_participant(
     ParticipantDescription participant_info) = 0;
 
   /// Unregister an existing participant.

rmf_traffic/src/rmf_traffic/schedule/Database.cpp

@@ -112,6 +112,7 @@ class Database::Implementation
     ParticipantStorage storage;
     const std::shared_ptr<const ParticipantDescription> description;
     const Version initial_schedule_version;
+    RouteId last_route_id = std::numeric_limits<RouteId>::max();
   };
   using ParticipantStates = std::unordered_map<ParticipantId, ParticipantState>;
   ParticipantStates states;
@@ -231,6 +232,9 @@ class Database::Implementation
     {
       const auto& item = input[i];
       const RouteId route_id = item.id;
+      if (rmf_utils::modular(state.last_route_id).less_than(route_id))
+        state.last_route_id = route_id;
+
       state.active_routes.insert(route_id);
 
       RouteStorage& entry_storage = *entries[i];
@@ -628,7 +632,7 @@ void Database::erase(
 }
 
 //==============================================================================
-ParticipantId Database::register_participant(
+Writer::Registration Database::register_participant(
   ParticipantDescription description)
 {
   const ParticipantId id = _pimpl->get_next_participant_id();
@@ -640,7 +644,7 @@ ParticipantId Database::register_participant(
   const auto description_ptr =
     std::make_shared<ParticipantDescription>(std::move(description));
 
-  _pimpl->states.insert(
+  const auto p_it = _pimpl->states.insert(
     std::make_pair(
       id,
       Implementation::ParticipantState{
@@ -649,12 +653,15 @@ ParticipantId Database::register_participant(
         {},
         description_ptr,
         version
-      }));
+      })).first;
 
   _pimpl->descriptions.insert({id, description_ptr});
 
   _pimpl->add_participant_version[version] = id;
-  return id;
+
+  const auto& state = p_it->second;
+  return Registration(
+    id, state.tracker->last_known_version(), state.last_route_id);
 }
 
 //==============================================================================
@@ -1245,5 +1252,19 @@ ItineraryVersion Database::itinerary_version(ParticipantId participant) const
   return p_it->second.tracker->last_known_version();
 }
 
+//==============================================================================
+RouteId Database::last_route_id(ParticipantId participant) const
+{
+  const auto p_it = _pimpl->states.find(participant);
+  if (p_it == _pimpl->states.end())
+  {
+    throw std::runtime_error(
+      "[Database::last_route_id] No participant with ID ["
+      + std::to_string(participant) + "]");
+  }
+
+  return p_it->second.last_route_id;
+}
+
 } // namespace schedule
 } // namespace rmf_traffic

rmf_traffic/src/rmf_traffic/schedule/Participant.cpp

@@ -19,6 +19,8 @@
 #include "debug_Participant.hpp"
 #include "RectifierInternal.hpp"
 
+#include <sstream>
+
 namespace rmf_traffic {
 namespace schedule {
 
@@ -34,17 +36,17 @@ Participant Participant::Implementation::make(
   std::shared_ptr<Writer> writer,
   std::shared_ptr<RectificationRequesterFactory> rectifier_factory)
 {
-  const ParticipantId id = writer->register_participant(description);
+  const auto registration = writer->register_participant(description);
 
   Participant participant;
   participant._pimpl = rmf_utils::make_unique_impl<Implementation>(
-    id, std::move(description), std::move(writer));
+    registration, std::move(description), std::move(writer));
 
   if (rectifier_factory)
   {
     participant._pimpl->_rectification =
       rectifier_factory->make(
-      Rectifier::Implementation::make(*participant._pimpl), id);
+      Rectifier::Implementation::make(*participant._pimpl), registration.id());
   }
 
   return participant;
@@ -55,6 +57,19 @@ void Participant::Implementation::retransmit(
   const std::vector<Rectifier::Range>& ranges,
   const ItineraryVersion last_known_version)
 {
+  if (rmf_utils::modular(current_version()).less_than(last_known_version))
+  {
+    std::stringstream str;
+    str << "[Participant::Implementation::retransmit] Remote database has a "
+        << "higher version number [" << last_known_version << "] than ["
+        << current_version() << "] the version number of the local "
+        << "participant [" << _id << ":" << _description.owner() << "/"
+        << _description.name() << "]. This indicates that the remote database "
+        << "is receiving participant updates from a conflicting source.";
+
+    throw std::runtime_error(str.str());
+  }
+
   for (const auto& range : ranges)
   {
     assert(rmf_utils::modular(range.lower).less_than_or_equal(range.upper));
@@ -97,10 +112,12 @@ ItineraryVersion Participant::Implementation::current_version() const
 
 //==============================================================================
 Participant::Implementation::Implementation(
-  ParticipantId id,
+  const Writer::Registration& registration,
   ParticipantDescription description,
   std::shared_ptr<Writer> writer)
-: _id(id),
+: _id(registration.id()),
+  _version(registration.last_itinerary_version()),
+  _last_route_id(registration.last_route_id()),
   _description(std::move(description)),
   _writer(writer)
 {

rmf_traffic/src/rmf_traffic/schedule/ParticipantInternal.hpp

@@ -46,7 +46,7 @@ class Participant::Implementation
   // Note: It would be better if this constructor were private, but we need to
   // make it public so it can be used by rmf_utils::make_unique_impl
   Implementation(
-    ParticipantId id,
+    const Writer::Registration& registration,
     ParticipantDescription description,
     std::shared_ptr<Writer> writer);
 
@@ -59,6 +59,8 @@ class Participant::Implementation
   ItineraryVersion get_next_version();
 
   const ParticipantId _id;
+  ItineraryVersion _version;
+  RouteId _last_route_id;
   const ParticipantDescription _description;
   std::shared_ptr<Writer> _writer;
   std::unique_ptr<RectificationRequester> _rectification;
@@ -69,8 +71,6 @@ class Participant::Implementation
   Writer::Input _current_itinerary;
 
   ChangeHistory _change_history;
-  RouteId _last_route_id = std::numeric_limits<RouteId>::max();
-  ItineraryVersion _version = std::numeric_limits<ItineraryVersion>::max();
   rmf_traffic::Duration _cumulative_delay = std::chrono::seconds(0);
 };
 

rmf_traffic/src/rmf_traffic/schedule/Writer.cpp

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <rmf_traffic/schedule/Writer.hpp>
+
+namespace rmf_traffic {
+namespace schedule {
+
+//==============================================================================
+class Writer::Registration::Implementation
+{
+public:
+
+  ParticipantId id;
+  ItineraryVersion version;
+  RouteId route_id;
+
+};
+
+//==============================================================================
+Writer::Registration::Registration(
+  ParticipantId id,
+  ItineraryVersion version,
+  RouteId route_id)
+: _pimpl(rmf_utils::make_impl<Implementation>(
+      Implementation{id, version, route_id}))
+{
+  // Do nothing
+}
+
+//==============================================================================
+ParticipantId Writer::Registration::id() const
+{
+  return _pimpl->id;
+}
+
+//==============================================================================
+ItineraryVersion Writer::Registration::last_itinerary_version() const
+{
+  return _pimpl->version;
+}
+
+//==============================================================================
+RouteId Writer::Registration::last_route_id() const
+{
+  return _pimpl->route_id;
+}
+
+} // namespace schedule
+} // namespace rmf_traffic

rmf_traffic/test/unit/agv/test_Planner.cpp

@@ -117,7 +117,7 @@ rmf_traffic::Trajectory test_with_obstacle(
   const rmf_traffic::Profile profile = create_test_profile(UnitCircle);
 
   rmf_traffic::Trajectory t_obs;
-  const rmf_traffic::schedule::ParticipantId p_obs =
+  const auto p_obs =
     database.register_participant(
     rmf_traffic::schedule::ParticipantDescription{
       "obstacle",
@@ -131,7 +131,7 @@ rmf_traffic::Trajectory test_with_obstacle(
   for (const auto& obstacle : obstacles)
   {
     const auto r = std::make_shared<rmf_traffic::Route>("test_map", obstacle);
-    database.extend(p_obs, {{rid++, r}}, iv++);
+    database.extend(p_obs.id(), {{rid++, r}}, iv++);
   }
 
   rmf_utils::optional<rmf_traffic::agv::Planner::Result> result;
@@ -2214,7 +2214,7 @@ SCENARIO("Test planner with various start conditions")
     profile};
 
   rmf_traffic::schedule::Database database;
-  const rmf_traffic::schedule::ParticipantId p_obs =
+  const auto p_obs =
     database.register_participant(
     rmf_traffic::schedule::ParticipantDescription{
       "obstacles",
@@ -2457,7 +2457,7 @@ SCENARIO("Test planner with various start conditions")
       {
         const auto r = std::make_shared<rmf_traffic::Route>(
           "test_map", obstacle);
-        database.extend(p_obs, {{ri_o++, r}}, iv_o++);
+        database.extend(p_obs.id(), {{ri_o++, r}}, iv_o++);
       }
 
       const auto result1 = plan.replan(start1);