diff --git a/bondcpp/include/bondcpp/bond.hpp b/bondcpp/include/bondcpp/bond.hpp index 873dc4c..f8a53b4 100644 --- a/bondcpp/include/bondcpp/bond.hpp +++ b/bondcpp/include/bondcpp/bond.hpp @@ -58,6 +58,7 @@ namespace bond class Bond { public: + using EventCallback = std::function; /** \brief Constructs a bond, but does not connect * * \param topic The topic used to exchange the bond status messages. @@ -70,8 +71,8 @@ class Bond Bond( const std::string & topic, const std::string & id, rclcpp_lifecycle::LifecycleNode::SharedPtr nh, - std::function on_broken = std::function(), - std::function on_formed = std::function()); + EventCallback on_broken = EventCallback(), + EventCallback on_formed = EventCallback()); /** \brief Constructs a bond, but does not connect * @@ -85,8 +86,8 @@ class Bond Bond( const std::string & topic, const std::string & id, rclcpp::Node::SharedPtr nh, - std::function on_broken = std::function(), - std::function on_formed = std::function()); + EventCallback on_broken = EventCallback(), + EventCallback on_formed = EventCallback()); /** \brief Destructs the object, breaking the bond if it is still formed. */ @@ -120,11 +121,11 @@ class Bond void start(); /** \brief Sets the formed callback. */ - void setFormedCallback(std::function on_formed); + void setFormedCallback(EventCallback on_formed); /** \brief Sets the broken callback */ - void setBrokenCallback(std::function 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 @@ -162,7 +163,7 @@ class Bond void doPublishing(); void publishStatus(bool active); - std::vector> pending_callbacks_; + std::vector pending_callbacks_; void flushPendingCallbacks(); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; @@ -182,8 +183,9 @@ class Bond std::string id_; std::string instance_id_; std::string sister_instance_id_; - std::function on_broken_; - std::function on_formed_; + EventCallback on_broken_; + EventCallback on_formed_; + bool sisterDiedFirst_; bool started_; bool connect_timer_reset_flag_; @@ -201,10 +203,8 @@ class Bond rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; }; - } // namespace bond - // Internal use only struct BondSM { diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 4ff139a..66681b4 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -71,8 +71,8 @@ static std::string makeUUID() Bond::Bond( const std::string & topic, const std::string & id, rclcpp_lifecycle::LifecycleNode::SharedPtr nh, - std::function on_broken, - std::function on_formed) + EventCallback on_broken, + EventCallback on_formed) : bondsm_(new BondSM(this)), sm_(*bondsm_), topic_(topic), @@ -108,8 +108,8 @@ Bond::Bond( Bond::Bond( const std::string & topic, const std::string & id, rclcpp::Node::SharedPtr nh, - std::function on_broken, - std::function on_formed) + EventCallback on_broken, + EventCallback on_formed) : bondsm_(new BondSM(this)), sm_(*bondsm_), topic_(topic), @@ -380,13 +380,13 @@ void Bond::start() started_ = true; } -void Bond::setFormedCallback(std::function on_formed) +void Bond::setFormedCallback(EventCallback on_formed) { std::unique_lock lock(mutex_); on_formed_ = on_formed; } -void Bond::setBrokenCallback(std::function on_broken) +void Bond::setBrokenCallback(EventCallback on_broken) { std::unique_lock lock(mutex_); on_broken_ = on_broken; @@ -545,7 +545,7 @@ void Bond::publishStatus(bool active) void Bond::flushPendingCallbacks() { - std::vector> callbacks; + std::vector callbacks; { std::unique_lock lock(mutex_); callbacks = pending_callbacks_;