Add possibility to register multiple callbacks to ReverseInterface and TrajectoryPointInterface

include/ur_client_library/control/reverse_interface.h

@@ -38,6 +38,7 @@
 #include <cstring>
 #include <endian.h>
 #include <condition_variable>
+#include <list>
 
 namespace urcl
 {
@@ -156,9 +157,31 @@ class ReverseInterface
                "commands.")]] virtual void
   setKeepaliveCount(const uint32_t count);
 
-  void registerDisconnectionCallback(std::function<void(const int)> disconnection_fun)
+  /*!
+   * \brief Register a callback for the robot-based disconnection.
+   *
+   * The callback will be called when the robot disconnects from the reverse interface.
+   *
+   * \param disconnection_fun The function to be called on disconnection.
+   *
+   * \returns A unique handler ID for the registered callback. This can be used to unregister the
+   * callback later.
+   */
+  uint32_t registerDisconnectionCallback(std::function<void(const int)> disconnection_fun)
+  {
+    disconnect_callbacks_.push_back({ next_disconnect_callback_id_, disconnection_fun });
+    return next_disconnect_callback_id_++;
+  }
+
+  /*! \brief Unregisters a disconnection callback.
+   *
+   * \param handler_id The ID of the handler to be unregistered as obtained from
+   * registerDisconnectionCallback.
+   */
+  void unregisterDisconnectionCallback(const uint32_t handler_id)
   {
-    disconnection_callback_ = disconnection_fun;
+    disconnect_callbacks_.remove_if(
+        [handler_id](const HandlerFunction<void(const int)>& h) { return h.id == handler_id; });
   }
 
   /*!
@@ -178,7 +201,8 @@ class ReverseInterface
 
   virtual void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv);
 
-  std::function<void(const int)> disconnection_callback_ = nullptr;
+  std::list<HandlerFunction<void(const int)>> disconnect_callbacks_;
+  uint32_t next_disconnect_callback_id_ = 0;
   socket_t client_fd_;
   comm::TCPServer server_;
 

include/ur_client_library/control/trajectory_point_interface.h

@@ -29,6 +29,7 @@
 #ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
 #define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
 
+#include <list>
 #include <optional>
 
 #include "ur_client_library/control/motion_primitives.h"
@@ -133,10 +134,11 @@ class TrajectoryPointInterface : public ReverseInterface
    */
   bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive);
 
-  void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
-  {
-    handle_trajectory_end_ = callback;
-  }
+  void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback);
+
+  uint32_t addTrajectoryEndCallback(const std::function<void(TrajectoryResult)>& callback);
+
+  void removeTrajectoryEndCallback(const uint32_t callback_id);
 
 protected:
   virtual void connectionCallback(const socket_t filedescriptor) override;
@@ -146,7 +148,8 @@ class TrajectoryPointInterface : public ReverseInterface
   virtual void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv) override;
 
 private:
-  std::function<void(TrajectoryResult)> handle_trajectory_end_;
+  std::list<HandlerFunction<void(TrajectoryResult)>> trajectory_end_callbacks_;
+  uint32_t next_done_callback_id_ = 0;
 };
 
 }  // namespace control

include/ur_client_library/types.h

@@ -22,7 +22,9 @@
 
 #include <inttypes.h>
 #include <array>
+#include <functional>
 #include <iostream>
+#include "ur_client_library/log.h"
 
 namespace urcl
 {
@@ -81,4 +83,20 @@ constexpr typename std::underlying_type<E>::type toUnderlying(const E e) noexcep
 {
   return static_cast<typename std::underlying_type<E>::type>(e);
 }
-}  // namespace urcl
+
+template <typename FunctionT>
+struct HandlerFunction
+{
+  uint32_t id;
+  std::function<FunctionT> function;
+
+  HandlerFunction(uint32_t id, std::function<FunctionT> function) : id(id), function(function)
+  {
+  }
+
+  bool operator==(const HandlerFunction& other) const
+  {
+    return id == other.id;
+  }
+};
+}  // namespace urcl

include/ur_client_library/ur/instruction_executor.h

@@ -42,16 +42,16 @@ class InstructionExecutor
   InstructionExecutor() = delete;
   InstructionExecutor(std::shared_ptr<urcl::UrDriver> driver) : driver_(driver)
   {
-    driver_->registerTrajectoryDoneCallback(
+    traj_done_callback_handler_id_ = driver_->registerTrajectoryDoneCallback(
         std::bind(&InstructionExecutor::trajDoneCallback, this, std::placeholders::_1));
-    driver_->registerTrajectoryInterfaceDisconnectedCallback(
+    disconnected_handler_id_ = driver_->registerTrajectoryInterfaceDisconnectedCallback(
         std::bind(&InstructionExecutor::trajDisconnectCallback, this, std::placeholders::_1));
   }
 
   ~InstructionExecutor()
   {
-    driver_->registerTrajectoryDoneCallback(nullptr);
-    driver_->registerTrajectoryInterfaceDisconnectedCallback(nullptr);
+    driver_->unregisterTrajectoryDoneCallback(traj_done_callback_handler_id_);
+    driver_->unregisterTrajectoryInterfaceDisconnectedCallback(disconnected_handler_id_);
   }
 
   /**
@@ -187,10 +187,13 @@ class InstructionExecutor
     return trajectory_running_;
   }
 
-private:
+protected:
   void trajDoneCallback(const urcl::control::TrajectoryResult& result);
   void trajDisconnectCallback(const int filedescriptor);
 
+  uint32_t traj_done_callback_handler_id_;
+  uint32_t disconnected_handler_id_;
+
   std::shared_ptr<urcl::UrDriver> driver_;
   std::atomic<bool> trajectory_running_ = false;
   std::atomic<bool> cancel_requested_ = false;

include/ur_client_library/ur/ur_driver.h

@@ -823,10 +823,17 @@ class UrDriver
    *
    * \param trajectory_done_cb Callback function that will be triggered in the event of finishing
    * a trajectory execution
+   *
+   * \returns The ID of the callback that can be used to unregister the callback later.
    */
-  void registerTrajectoryDoneCallback(std::function<void(control::TrajectoryResult)> trajectory_done_cb)
+  uint32_t registerTrajectoryDoneCallback(std::function<void(control::TrajectoryResult)> trajectory_done_cb)
+  {
+    return trajectory_interface_->addTrajectoryEndCallback(trajectory_done_cb);
+  }
+
+  void unregisterTrajectoryDoneCallback(const uint32_t handler_id)
   {
-    trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb);
+    trajectory_interface_->removeTrajectoryEndCallback(handler_id);
   }
 
   /*!
@@ -887,9 +894,32 @@ class UrDriver
     primary_client_->stop();
   }
 
-  void registerTrajectoryInterfaceDisconnectedCallback(std::function<void(const int)> fun)
+  /*!
+   * \brief Register a callback for the trajectory interface disconnection.
+   *
+   * This callback will be called when the trajectory interface is disconnected.
+   *
+   * \param fun Callback function that will be triggered in the event of disconnection
+   *
+   * \returns The ID of the callback that can be used to unregister the callback later.
+   */
+  uint32_t registerTrajectoryInterfaceDisconnectedCallback(std::function<void(const int)> fun)
+  {
+    return trajectory_interface_->registerDisconnectionCallback(fun);
+  }
+
+  /*!
+   * \brief Unregister a callback for the trajectory interface disconnection.
+   *
+   * This will remove the callback that was registered with
+   * registerTrajectoryInterfaceDisconnectedCallback.
+   *
+   * \param handler_id The ID of the callback to be removed as obtained from
+   * registerTrajectoryInterfaceDisconnectedCallback.
+   */
+  void unregisterTrajectoryInterfaceDisconnectedCallback(const uint32_t handler_id)
   {
-    trajectory_interface_->registerDisconnectionCallback(fun);
+    trajectory_interface_->unregisterDisconnectionCallback(handler_id);
   }
 
   /*!

src/control/reverse_interface.cpp

@@ -238,6 +238,10 @@ void ReverseInterface::disconnectionCallback(const socket_t filedescriptor)
   URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor);
   client_fd_ = INVALID_SOCKET;
   handle_program_state_(false);
+  for (auto handler : disconnect_callbacks_)
+  {
+    handler.function(filedescriptor);
+  }
 }
 
 void ReverseInterface::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv)

src/control/trajectory_point_interface.cpp

@@ -275,9 +275,9 @@ void TrajectoryPointInterface::connectionCallback(const socket_t filedescriptor)
 void TrajectoryPointInterface::disconnectionCallback(const socket_t filedescriptor)
 {
   URCL_LOG_DEBUG("Connection to trajectory interface dropped.");
-  if (disconnection_callback_ != nullptr)
+  for (auto handler : disconnect_callbacks_)
   {
-    disconnection_callback_(filedescriptor);
+    handler.function(filedescriptor);
   }
   client_fd_ = INVALID_SOCKET;
 }
@@ -289,9 +289,12 @@ void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, ch
     int32_t* status = reinterpret_cast<int*>(buffer);
     URCL_LOG_DEBUG("Received message %d on TrajectoryPointInterface", be32toh(*status));
 
-    if (handle_trajectory_end_)
+    if (!trajectory_end_callbacks_.empty())
     {
-      handle_trajectory_end_(static_cast<TrajectoryResult>(be32toh(*status)));
+      for (auto handler : trajectory_end_callbacks_)
+      {
+        handler.function(static_cast<TrajectoryResult>(be32toh(*status)));
+      }
     }
     else
     {
@@ -304,5 +307,23 @@ void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, ch
                   nbytesrecv);
   }
 }
+
+void TrajectoryPointInterface::setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
+{
+  addTrajectoryEndCallback(callback);
+}
+
+uint32_t TrajectoryPointInterface::addTrajectoryEndCallback(const std::function<void(TrajectoryResult)>& callback)
+{
+  trajectory_end_callbacks_.push_back({ next_done_callback_id_, callback });
+  return next_done_callback_id_++;
+}
+
+void TrajectoryPointInterface::removeTrajectoryEndCallback(const uint32_t handler_id)
+{
+  trajectory_end_callbacks_.remove_if(
+      [handler_id](const HandlerFunction<void(TrajectoryResult)>& h) { return h.id == handler_id; });
+}
+
 }  // namespace control
 }  // namespace urcl

src/ur/instruction_executor.cpp

@@ -36,10 +36,13 @@
 void urcl::InstructionExecutor::trajDoneCallback(const urcl::control::TrajectoryResult& result)
 {
   URCL_LOG_DEBUG("Trajectory result received: %s", control::trajectoryResultToString(result).c_str());
-  std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
-  trajectory_done_cv_.notify_all();
-  trajectory_result_ = result;
-  trajectory_running_ = false;
+  if (trajectory_running_)
+  {
+    std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+    trajectory_done_cv_.notify_all();
+    trajectory_result_ = result;
+    trajectory_running_ = false;
+  }
 }
 void urcl::InstructionExecutor::trajDisconnectCallback(const int filedescriptor)
 {