Provide an API for changing participant profile (#87)

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
open-rmf · Sep 16, 2022 · e60a256 · e60a256
1 parent 6594a26
commit e60a256
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 2 deletions.
diff --git a/rmf_traffic/CMakeLists.txt b/rmf_traffic/CMakeLists.txt
@@ -36,7 +36,7 @@ set(using_new_fcl true)
 set(FCL_LIBRARIES fcl)
 
 find_package(rmf_utils REQUIRED)
-find_package(Threads)
+find_package(Threads REQUIRED)
 
 find_package(eigen3_cmake_module QUIET)
 if (eigen3_cmake_module_FOUND)

diff --git a/rmf_traffic/include/rmf_traffic/schedule/Participant.hpp b/rmf_traffic/include/rmf_traffic/schedule/Participant.hpp
@@ -121,6 +121,9 @@ class Participant
   /// Get the current plan ID of the participant
   PlanId current_plan_id() const;
 
+  /// Change the profile of this participant
+  void change_profile(Profile new_profile);
+
   // This class supports moving but not copying
   Participant(Participant&&) = default;
   Participant& operator=(Participant&&) = default;

diff --git a/rmf_traffic/src/rmf_traffic/schedule/Participant.cpp b/rmf_traffic/src/rmf_traffic/schedule/Participant.cpp
@@ -20,6 +20,7 @@
 #include "internal_Rectifier.hpp"
 
 #include <iostream>
+#include <thread>
 
 using namespace std::chrono_literals;
 
@@ -359,6 +360,26 @@ ParticipantId Participant::Implementation::Shared::get_id() const
   return _id;
 }
 
+//==============================================================================
+void Participant::Implementation::Shared::change_profile(Profile new_profile)
+{
+  _description.profile(std::move(new_profile));
+
+  // The register_participant function does not return until it has received a
+  // response from the schedule. This will be a problem if a user calls the
+  // change_profile function from inside of a callback that inside a spinning
+  // executor, because the node will no longer be able to spin and therefore the
+  // response will never arrive and therefore the node will no longer be able to
+  // spin, creating a deadlock. We run the function in a detached thread so it
+  // can do its work without blocking. We don't care about the return value so
+  // we can just discard that without syncing.
+  std::thread t([w = _writer, d = _description]()
+    {
+      w->register_participant(d);
+    });
+  t.detach();
+}
+
 //==============================================================================
 void Participant::Implementation::Shared::correct_id(ParticipantId new_id)
 {
@@ -491,6 +512,12 @@ PlanId Participant::current_plan_id() const
   return _pimpl->_shared->_current_plan_id;
 }
 
+//==============================================================================
+void Participant::change_profile(Profile new_profile)
+{
+  _pimpl->_shared->change_profile(std::move(new_profile));
+}
+
 //==============================================================================
 Participant::Participant()
 {

diff --git a/rmf_traffic/src/rmf_traffic/schedule/internal_Participant.hpp b/rmf_traffic/src/rmf_traffic/schedule/internal_Participant.hpp
@@ -68,6 +68,8 @@ class Participant::Implementation
 
     ParticipantId get_id() const;
 
+    void change_profile(Profile new_profile);
+
     void correct_id(ParticipantId new_id);
 
     const ParticipantDescription& get_description() const;
@@ -81,7 +83,7 @@ class Participant::Implementation
 
     ParticipantId _id;
     ItineraryVersion _version;
-    const ParticipantDescription _description;
+    ParticipantDescription _description;
     std::shared_ptr<Writer> _writer;
     std::unique_ptr<RectificationRequester> _rectification;