Bond cleanups

bondcpp/include/bondcpp/bond.hpp

@@ -33,7 +33,6 @@
 #define BONDCPP__BOND_HPP_
 
 #include <chrono>
-#include <condition_variable>
 #include <functional>
 #include <memory>
 #include <mutex>
@@ -59,6 +58,29 @@ namespace bond
 class Bond
 {
 public:
+  using EventCallback = std::function<void (void)>;
+
+  /** \brief Constructor to delegate common functionality
+   *
+   * \param topic The topic used to exchange the bond status messages.
+   * \param id The ID of the bond, which should match the ID used on
+   *           the sister's end.
+   * \param node_base base node interface
+   * \param node_logging logging node interface
+   * \param node_timers timers node interface
+   * \param on_broken callback that will be called when the bond is broken.
+   * \param on_formed callback that will be called when the bond is formed.
+   */
+  Bond(
+    const std::string & topic, const std::string & id,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params,
+    rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    EventCallback on_broken = EventCallback(),
+    EventCallback on_formed = EventCallback());
+
   /** \brief Constructs a bond, but does not connect
    *
    * \param topic The topic used to exchange the bond status messages.
@@ -71,8 +93,8 @@ class Bond
   Bond(
     const std::string & topic, const std::string & id,
     rclcpp_lifecycle::LifecycleNode::SharedPtr nh,
-    std::function<void(void)> on_broken = std::function<void(void)>(),
-    std::function<void(void)> on_formed = std::function<void(void)>());
+    EventCallback on_broken = EventCallback(),
+    EventCallback on_formed = EventCallback());
 
   /** \brief Constructs a bond, but does not connect
    *
@@ -86,46 +108,51 @@ class Bond
   Bond(
     const std::string & topic, const std::string & id,
     rclcpp::Node::SharedPtr nh,
-    std::function<void(void)> on_broken = std::function<void(void)>(),
-    std::function<void(void)> on_formed = std::function<void(void)>());
+    EventCallback on_broken = EventCallback(),
+    EventCallback on_formed = EventCallback());
 
   /** \brief Destructs the object, breaking the bond if it is still formed.
    */
   ~Bond();
 
   void setupConnections();
 
-  double getConnectTimeout() const {return connect_timeout_;}
+  double getConnectTimeout() const {return connect_timeout_.seconds();}
   void setConnectTimeout(double dur);
   void connectTimerReset();
   void connectTimerCancel();
-  double getDisconnectTimeout() const {return disconnect_timeout_;}
+
+  double getDisconnectTimeout() const {return disconnect_timeout_.seconds();}
   void setDisconnectTimeout(double dur);
   void disconnectTimerReset();
   void disconnectTimerCancel();
-  double getHeartbeatTimeout() const {return heartbeat_timeout_;}
+
+  double getHeartbeatTimeout() const {return heartbeat_timeout_.seconds();}
   void setHeartbeatTimeout(double dur);
   void heartbeatTimerReset();
   void heartbeatTimerCancel();
-  double getHeartbeatPeriod() const {return heartbeat_period_;}
+
+  double getHeartbeatPeriod() const {return heartbeat_period_.seconds();}
   void setHeartbeatPeriod(double dur);
   void publishingTimerReset();
   void publishingTimerCancel();
-  double getDeadPublishPeriod() const {return dead_publish_period_;}
+
+  double getDeadPublishPeriod() const {return dead_publish_period_.seconds();}
   void setDeadPublishPeriod(double dur);
   void deadpublishingTimerReset();
   void deadpublishingTimerCancel();
 
   /** \brief Starts the bond and connects to the sister process.
    */
   void start();
+
   /** \brief Sets the formed callback.
    */
-  void setFormedCallback(std::function<void(void)> on_formed);
+  void setFormedCallback(EventCallback on_formed);
 
   /** \brief Sets the broken callback
    */
-  void setBrokenCallback(std::function<void(void)> on_broken);
+  void setBrokenCallback(EventCallback on_broken);
 
   /** \brief Blocks until the bond is formed for at most 'duration'.
    *    Assumes the node to be spinning in the background
@@ -134,19 +161,23 @@ class Bond
    * \return true iff the bond has been formed.
    */
   bool waitUntilFormed(rclcpp::Duration timeout = rclcpp::Duration(std::chrono::seconds(-1)));
+
   /** \brief Blocks until the bond is broken for at most 'duration'.
    *    Assumes the node to be spinning in the background
    *
    * \param timeout Maximum duration to wait.  If -1 then this call will not timeout.
    * \return true iff the bond has been broken, even if it has never been formed.
    */
   bool waitUntilBroken(rclcpp::Duration timeout = rclcpp::Duration(std::chrono::seconds(-1)));
+
   /** \brief Indicates if the bond is broken.
    */
   bool isBroken();
+
   /** \brief Breaks the bond, notifying the other process.
    */
   void breakBond();
+
   std::string getTopic() {return topic_;}
   std::string getId() {return id_;}
   std::string getInstanceId() {return instance_id_;}
@@ -163,50 +194,50 @@ class Bond
   void doPublishing();
   void publishStatus(bool active);
 
-  std::vector<std::function<void(void)>> pending_callbacks_;
   void flushPendingCallbacks();
 
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
-  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
 
   rclcpp::TimerBase::SharedPtr connect_timer_;
   rclcpp::TimerBase::SharedPtr disconnect_timer_;
   rclcpp::TimerBase::SharedPtr heartbeat_timer_;
   rclcpp::TimerBase::SharedPtr publishing_timer_;
   rclcpp::TimerBase::SharedPtr deadpublishing_timer_;
 
+  std::mutex state_machine_mutex_;
   std::unique_ptr<BondSM> bondsm_;
   BondSMContext sm_;
 
   std::string topic_;
   std::string id_;
   std::string instance_id_;
   std::string sister_instance_id_;
-  std::function<void(void)> on_broken_;
-  std::function<void(void)> on_formed_;
-  bool sisterDiedFirst_;
-  bool started_;
-  bool connect_timer_reset_flag_;
-  bool disconnect_timer_reset_flag_;
-  bool deadpublishing_timer_reset_flag_;
-  bool disable_heartbeat_timeout_;
-  std::mutex mutex_;
-  std::condition_variable condition_;
-
-  double connect_timeout_;
-  double heartbeat_timeout_;
-  double disconnect_timeout_;
-  double heartbeat_period_;
-  double dead_publish_period_;
+
+  std::mutex callbacks_mutex_;
+  std::vector<EventCallback> pending_callbacks_;
+  EventCallback on_broken_;
+  EventCallback on_formed_;
+
+  bool sisterDiedFirst_ {false};
+  bool started_ {false};
+  bool connect_timer_reset_flag_ {false};
+  bool disconnect_timer_reset_flag_ {false};
+  bool deadpublishing_timer_reset_flag_ {false};
+  bool disable_heartbeat_timeout_ {false};
+
+  rclcpp::Duration connect_timeout_;
+  rclcpp::Duration disconnect_timeout_;
+  rclcpp::Duration heartbeat_timeout_;
+  rclcpp::Duration heartbeat_period_;
+  rclcpp::Duration dead_publish_period_;
 
   rclcpp::Subscription<bond::msg::Status>::SharedPtr sub_;
   rclcpp::Publisher<bond::msg::Status>::SharedPtr pub_;
 };
-
 }  // namespace bond
 
-
 // Internal use only
 struct BondSM
 {