diff --git a/bondcpp/include/bondcpp/bond.h b/bondcpp/include/bondcpp/bond.h index 40b860b7..60e5e615 100644 --- a/bondcpp/include/bondcpp/bond.h +++ b/bondcpp/include/bondcpp/bond.h @@ -48,13 +48,13 @@ #include #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries - #ifdef bondcpp_EXPORTS // we are building a shared lib/dll - #define BONDCPP_DECL ROS_HELPER_EXPORT - #else // we are using shared lib/dll - #define BONDCPP_DECL ROS_HELPER_IMPORT - #endif + #ifdef bondcpp_EXPORTS // we are building a shared lib/dll + #define BONDCPP_DECL ROS_HELPER_EXPORT + #else // we are using shared lib/dll + #define BONDCPP_DECL ROS_HELPER_IMPORT + #endif #else // ros is being built around static libraries - #define BONDCPP_DECL + #define BONDCPP_DECL #endif namespace bond { @@ -76,7 +76,8 @@ class BONDCPP_DECL Bond * \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, + Bond(const std::string& topic, + const std::string& id, boost::function on_broken = boost::function(), boost::function on_formed = boost::function()); @@ -93,7 +94,7 @@ class BONDCPP_DECL Bond double getHeartbeatPeriod() const { return heartbeat_period_; } void setHeartbeatPeriod(double dur); - void setCallbackQueue(ros::CallbackQueueInterface *queue); + void setCallbackQueue(ros::CallbackQueueInterface* queue); /** \brief Starts the bond and connects to the sister process. */ @@ -184,22 +185,21 @@ class BONDCPP_DECL Bond void onHeartbeatTimeout(); void onDisconnectTimeout(); - void bondStatusCB(const bond::Status::ConstPtr &msg); + void bondStatusCB(const bond::Status::ConstPtr& msg); - void doPublishing(const ros::SteadyTimerEvent &e); + void doPublishing(const ros::SteadyTimerEvent& e); void publishStatus(bool active); std::vector > pending_callbacks_; void flushPendingCallbacks(); }; -} // namespace bond - +} // namespace bond // Internal use only struct BondSM { - BondSM(bond::Bond *b_) : b(b_) {} + BondSM(bond::Bond* b_) : b(b_) {} void Connected(); void SisterDied(); void Death(); @@ -207,7 +207,7 @@ struct BondSM void StartDying(); private: - bond::Bond *b; + bond::Bond* b; }; -#endif // BONDCPP__BOND_H_ +#endif // BONDCPP__BOND_H_ diff --git a/bondcpp/include/bondcpp/timeout.h b/bondcpp/include/bondcpp/timeout.h index de2d4d4b..ac4a358b 100644 --- a/bondcpp/include/bondcpp/timeout.h +++ b/bondcpp/include/bondcpp/timeout.h @@ -38,16 +38,16 @@ class Timeout { public: Timeout( - const ros::Duration &d, - boost::function on_timeout = boost::function()); + const ros::Duration& d, + boost::function on_timeout = boost::function()); Timeout( - const ros::WallDuration &d, - boost::function on_timeout = boost::function()); + const ros::WallDuration& d, + boost::function on_timeout = boost::function()); ~Timeout(); // Undefined what these do to a running timeout - void setDuration(const ros::Duration &d); - void setDuration(const ros::WallDuration &d); + void setDuration(const ros::Duration& d); + void setDuration(const ros::WallDuration& d); void reset(); void cancel(); @@ -60,10 +60,9 @@ class Timeout ros::WallDuration duration_; boost::function on_timeout_; - void timerCallback(const ros::SteadyTimerEvent &e); + void timerCallback(const ros::SteadyTimerEvent& e); }; +} // namespace bond -} // namespace bond - -#endif // BONDCPP__TIMEOUT_H_ +#endif // BONDCPP__TIMEOUT_H_ diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 58e7901d..f75fc692 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -48,372 +48,405 @@ namespace bond { static std::string makeUUID() { #ifdef _WIN32 - UUID uuid; - UuidCreate(&uuid); - unsigned char *str; - UuidToStringA(&uuid, &str); - std::string return_string(reinterpret_cast(str)); - RpcStringFreeA(&str); - return return_string; + UUID uuid; + UuidCreate(&uuid); + unsigned char* str; + UuidToStringA(&uuid, &str); + std::string return_string(reinterpret_cast(str)); + RpcStringFreeA(&str); + return return_string; #else - uuid_t uuid; - uuid_generate_random(uuid); - char uuid_str[40]; - uuid_unparse(uuid, uuid_str); - return std::string(uuid_str); + uuid_t uuid; + uuid_generate_random(uuid); + char uuid_str[40]; + uuid_unparse(uuid, uuid_str); + return std::string(uuid_str); #endif } -Bond::Bond(const std::string &topic, const std::string &id, +Bond::Bond(const std::string& topic, + const std::string& id, boost::function on_broken, boost::function on_formed) - : - - bondsm_(new BondSM(this)), - sm_(*bondsm_), - topic_(topic), - id_(id), - instance_id_(makeUUID()), - on_broken_(on_broken), - on_formed_(on_formed), - sisterDiedFirst_(false), - started_(false), - - connect_timer_(ros::WallDuration(), boost::bind(&Bond::onConnectTimeout, this)), - heartbeat_timer_(ros::WallDuration(), boost::bind(&Bond::onHeartbeatTimeout, this)), - disconnect_timer_(ros::WallDuration(), boost::bind(&Bond::onDisconnectTimeout, this)) + : + bondsm_(new BondSM(this)), + sm_(*bondsm_), + topic_(topic), + id_(id), + instance_id_(makeUUID()), + on_broken_(on_broken), + on_formed_(on_formed), + sisterDiedFirst_(false), + started_(false), + + connect_timer_(ros::WallDuration(), boost::bind(&Bond::onConnectTimeout, this)), + heartbeat_timer_(ros::WallDuration(), boost::bind(&Bond::onHeartbeatTimeout, this)), + disconnect_timer_(ros::WallDuration(), boost::bind(&Bond::onDisconnectTimeout, this)) { - setConnectTimeout(bond::Constants::DEFAULT_CONNECT_TIMEOUT); - setDisconnectTimeout(bond::Constants::DEFAULT_DISCONNECT_TIMEOUT); - setHeartbeatTimeout(bond::Constants::DEFAULT_HEARTBEAT_TIMEOUT); - setHeartbeatPeriod(bond::Constants::DEFAULT_HEARTBEAT_PERIOD); + setConnectTimeout(bond::Constants::DEFAULT_CONNECT_TIMEOUT); + setDisconnectTimeout(bond::Constants::DEFAULT_DISCONNECT_TIMEOUT); + setHeartbeatTimeout(bond::Constants::DEFAULT_HEARTBEAT_TIMEOUT); + setHeartbeatPeriod(bond::Constants::DEFAULT_HEARTBEAT_PERIOD); } Bond::~Bond() { - if (!started_) { - return; - } - - breakBond(); - if (!waitUntilBroken(ros::Duration(1.0))) { - ROS_DEBUG("Bond failed to break on destruction %s (%s)", id_.c_str(), instance_id_.c_str()); - } - - // Must destroy the subscription before locking mutex_: shutdown() - // will block until the status callback completes, and the status - // callback locks mutex_ (in flushPendingCallbacks). - sub_.shutdown(); - - // Stops the timers before locking the mutex. Makes sure none of - // the callbacks are running when we aquire the mutex. - publishingTimer_.stop(); - connect_timer_.cancel(); - heartbeat_timer_.cancel(); - disconnect_timer_.cancel(); - - boost::mutex::scoped_lock lock(mutex_); - pub_.shutdown(); -} + if (!started_) + { + return; + } + + breakBond(); + if (!waitUntilBroken(ros::Duration(1.0))) + { + ROS_DEBUG("Bond failed to break on destruction %s (%s)", id_.c_str(), instance_id_.c_str()); + } + // Must destroy the subscription before locking mutex_: shutdown() + // will block until the status callback completes, and the status + // callback locks mutex_ (in flushPendingCallbacks). + sub_.shutdown(); + + // Stops the timers before locking the mutex. Makes sure none of + // the callbacks are running when we aquire the mutex. + publishingTimer_.stop(); + connect_timer_.cancel(); + heartbeat_timer_.cancel(); + disconnect_timer_.cancel(); + + boost::mutex::scoped_lock lock(mutex_); + pub_.shutdown(); +} void Bond::setConnectTimeout(double dur) { - if (started_) { - ROS_ERROR("Cannot set timeouts after calling start()"); - return; - } + if (started_) + { + ROS_ERROR("Cannot set timeouts after calling start()"); + return; + } - connect_timeout_ = dur; - connect_timer_.setDuration(ros::WallDuration(connect_timeout_)); + connect_timeout_ = dur; + connect_timer_.setDuration(ros::WallDuration(connect_timeout_)); } void Bond::setDisconnectTimeout(double dur) { - if (started_) { - ROS_ERROR("Cannot set timeouts after calling start()"); - return; - } + if (started_) + { + ROS_ERROR("Cannot set timeouts after calling start()"); + return; + } - disconnect_timeout_ = dur; - disconnect_timer_.setDuration(ros::WallDuration(disconnect_timeout_)); + disconnect_timeout_ = dur; + disconnect_timer_.setDuration(ros::WallDuration(disconnect_timeout_)); } void Bond::setHeartbeatTimeout(double dur) { - if (started_) { - ROS_ERROR("Cannot set timeouts after calling start()"); - return; - } + if (started_) + { + ROS_ERROR("Cannot set timeouts after calling start()"); + return; + } - heartbeat_timeout_ = dur; - heartbeat_timer_.setDuration(ros::WallDuration(heartbeat_timeout_)); + heartbeat_timeout_ = dur; + heartbeat_timer_.setDuration(ros::WallDuration(heartbeat_timeout_)); } void Bond::setHeartbeatPeriod(double dur) { - if (started_) { - ROS_ERROR("Cannot set timeouts after calling start()"); - return; - } + if (started_) + { + ROS_ERROR("Cannot set timeouts after calling start()"); + return; + } - heartbeat_period_ = dur; + heartbeat_period_ = dur; } -void Bond::setCallbackQueue(ros::CallbackQueueInterface *queue) +void Bond::setCallbackQueue(ros::CallbackQueueInterface* queue) { - if (started_) { - ROS_ERROR("Cannot set callback queue after calling start()"); - return; - } + if (started_) + { + ROS_ERROR("Cannot set callback queue after calling start()"); + return; + } - nh_.setCallbackQueue(queue); + nh_.setCallbackQueue(queue); } - void Bond::start() { - boost::mutex::scoped_lock lock(mutex_); - connect_timer_.reset(); - pub_ = nh_.advertise(topic_, 5); - sub_ = nh_.subscribe(topic_, 30, boost::bind(&Bond::bondStatusCB, this, _1)); - - publishingTimer_ = nh_.createSteadyTimer( - ros::WallDuration(heartbeat_period_), &Bond::doPublishing, this); - started_ = true; + boost::mutex::scoped_lock lock(mutex_); + connect_timer_.reset(); + pub_ = nh_.advertise(topic_, 5); + sub_ = nh_.subscribe(topic_, 30, boost::bind(&Bond::bondStatusCB, this, _1)); + + publishingTimer_ = nh_.createSteadyTimer(ros::WallDuration(heartbeat_period_), &Bond::doPublishing, this); + started_ = true; } void Bond::setFormedCallback(boost::function on_formed) { - boost::mutex::scoped_lock lock(mutex_); - on_formed_ = on_formed; + boost::mutex::scoped_lock lock(mutex_); + on_formed_ = on_formed; } void Bond::setBrokenCallback(boost::function on_broken) { - boost::mutex::scoped_lock lock(mutex_); - on_broken_ = on_broken; + boost::mutex::scoped_lock lock(mutex_); + on_broken_ = on_broken; } bool Bond::waitUntilFormed(ros::Duration timeout) { - return waitUntilFormed(ros::WallDuration(timeout.sec, timeout.nsec)); + return waitUntilFormed(ros::WallDuration(timeout.sec, timeout.nsec)); } bool Bond::waitUntilFormed(ros::WallDuration timeout) { - boost::mutex::scoped_lock lock(mutex_); - ros::SteadyTime deadline(ros::SteadyTime::now() + timeout); + boost::mutex::scoped_lock lock(mutex_); + ros::SteadyTime deadline(ros::SteadyTime::now() + timeout); - while (sm_.getState().getId() == SM::WaitingForSister.getId()) { - if (!ros::ok()) { - break; - } + while (sm_.getState().getId() == SM::WaitingForSister.getId()) + { + if (!ros::ok()) + { + break; + } - ros::WallDuration wait_time = ros::WallDuration(0.1); - if (timeout >= ros::WallDuration(0.0)) { - wait_time = std::min(wait_time, deadline - ros::SteadyTime::now()); - } + ros::WallDuration wait_time = ros::WallDuration(0.1); + if (timeout >= ros::WallDuration(0.0)) + { + wait_time = std::min(wait_time, deadline - ros::SteadyTime::now()); + } - if (wait_time <= ros::WallDuration(0.0)) { - break; // The deadline has expired - } + if (wait_time <= ros::WallDuration(0.0)) + { + break; // The deadline has expired + } - condition_.timed_wait(mutex_, boost::posix_time::milliseconds( - static_cast(wait_time.toSec() * 1000.0f))); - } - return sm_.getState().getId() != SM::WaitingForSister.getId(); + condition_.timed_wait(mutex_, boost::posix_time::milliseconds( + static_cast(wait_time.toSec() * 1000.0f))); + } + return sm_.getState().getId() != SM::WaitingForSister.getId(); } bool Bond::waitUntilBroken(ros::Duration timeout) { - return waitUntilBroken(ros::WallDuration(timeout.sec, timeout.nsec)); + return waitUntilBroken(ros::WallDuration(timeout.sec, timeout.nsec)); } bool Bond::waitUntilBroken(ros::WallDuration timeout) { - boost::mutex::scoped_lock lock(mutex_); - ros::SteadyTime deadline(ros::SteadyTime::now() + timeout); + boost::mutex::scoped_lock lock(mutex_); + ros::SteadyTime deadline(ros::SteadyTime::now() + timeout); - while (sm_.getState().getId() != SM::Dead.getId()) { - if (!ros::ok()) { - break; - } + while (sm_.getState().getId() != SM::Dead.getId()) + { + if (!ros::ok()) + { + break; + } - ros::WallDuration wait_time = ros::WallDuration(0.1); - if (timeout >= ros::WallDuration(0.0)) { - wait_time = std::min(wait_time, deadline - ros::SteadyTime::now()); - } + ros::WallDuration wait_time = ros::WallDuration(0.1); + if (timeout >= ros::WallDuration(0.0)) + { + wait_time = std::min(wait_time, deadline - ros::SteadyTime::now()); + } - if (wait_time <= ros::WallDuration(0.0)) { - break; // The deadline has expired - } + if (wait_time <= ros::WallDuration(0.0)) + { + break; // The deadline has expired + } - condition_.timed_wait(mutex_, boost::posix_time::milliseconds( - static_cast(wait_time.toSec() * 1000.0f))); - } - return sm_.getState().getId() == SM::Dead.getId(); + condition_.timed_wait(mutex_, boost::posix_time::milliseconds( + static_cast(wait_time.toSec() * 1000.0f))); + } + return sm_.getState().getId() == SM::Dead.getId(); } bool Bond::isBroken() { - boost::mutex::scoped_lock lock(mutex_); - return sm_.getState().getId() == SM::Dead.getId(); + boost::mutex::scoped_lock lock(mutex_); + return sm_.getState().getId() == SM::Dead.getId(); } void Bond::breakBond() { - { - boost::mutex::scoped_lock lock(mutex_); - if (sm_.getState().getId() != SM::Dead.getId()) { - sm_.Die(); - publishStatus(false); + { + boost::mutex::scoped_lock lock(mutex_); + if (sm_.getState().getId() != SM::Dead.getId()) + { + sm_.Die(); + publishStatus(false); + } } - } - flushPendingCallbacks(); + flushPendingCallbacks(); } - void Bond::onConnectTimeout() { - { - boost::mutex::scoped_lock lock(mutex_); - sm_.ConnectTimeout(); - } - flushPendingCallbacks(); + { + boost::mutex::scoped_lock lock(mutex_); + sm_.ConnectTimeout(); + } + flushPendingCallbacks(); } void Bond::onHeartbeatTimeout() { - // Checks that heartbeat timeouts haven't been disabled globally. - bool disable_heartbeat_timeout; - nh_.param(bond::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, disable_heartbeat_timeout, false); - if (disable_heartbeat_timeout) { - ROS_WARN("Heartbeat timeout is disabled. Not breaking bond (topic: %s, id: %s)", - topic_.c_str(), id_.c_str()); - return; - } - - { - boost::mutex::scoped_lock lock(mutex_); - sm_.HeartbeatTimeout(); - } - flushPendingCallbacks(); + // Checks that heartbeat timeouts haven't been disabled globally. + bool disable_heartbeat_timeout; + nh_.param(bond::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, disable_heartbeat_timeout, false); + if (disable_heartbeat_timeout) + { + ROS_WARN("Heartbeat timeout is disabled. Not breaking bond (topic: %s, id: %s)", + topic_.c_str(), id_.c_str()); + return; + } + + { + boost::mutex::scoped_lock lock(mutex_); + sm_.HeartbeatTimeout(); + } + flushPendingCallbacks(); } void Bond::onDisconnectTimeout() { - { - boost::mutex::scoped_lock lock(mutex_); - sm_.DisconnectTimeout(); - } - flushPendingCallbacks(); + { + boost::mutex::scoped_lock lock(mutex_); + sm_.DisconnectTimeout(); + } + flushPendingCallbacks(); } -void Bond::bondStatusCB(const bond::Status::ConstPtr &msg) +void Bond::bondStatusCB(const bond::Status::ConstPtr& msg) { - // Filters out messages from other bonds and messages from ourself - if (msg->id == id_ && msg->instance_id != instance_id_) { + if (msg->id != id_) + { + // filter out heartbeat from other bonds + return; + } + if (msg->instance_id == instance_id_) { - boost::mutex::scoped_lock lock(mutex_); - - if (sister_instance_id_.empty()) { - sister_instance_id_ = msg->instance_id; - } - if (sister_instance_id_ != msg->instance_id) { - ROS_ERROR( - "More than two locations are trying to use a single bond (topic: %s, id: %s). " - "You should only instantiate at most two bond instances for each (topic, id) pair.", - topic_.c_str(), id_.c_str()); + // filter out heartbeat from this bond return; - } + } - if (msg->active) { - sm_.SisterAlive(); - } else { - sm_.SisterDead(); + { + boost::mutex::scoped_lock lock(mutex_); - // Immediate ack for sister's death notification - if (sisterDiedFirst_) { - publishStatus(false); + if (sister_instance_id_.empty()) + { + sister_instance_id_ = msg->instance_id; + } + if (sister_instance_id_ != msg->instance_id) + { + ROS_ERROR( + "More than two locations are trying to use a single bond (topic: %s, id: %s)." + "You should only instantiate at most two bond instances for each (topic, id) pair.", + topic_.c_str(), id_.c_str()); + return; + } + + if (msg->active) + { + sm_.SisterAlive(); + } + else + { + sm_.SisterDead(); + + // Immediate ack for sister's death notification + if (sisterDiedFirst_) + { + publishStatus(false); + } } - } } flushPendingCallbacks(); - } } -void Bond::doPublishing(const ros::SteadyTimerEvent &) +void Bond::doPublishing(const ros::SteadyTimerEvent&) { - boost::mutex::scoped_lock lock(mutex_); - if (sm_.getState().getId() == SM::WaitingForSister.getId() || - sm_.getState().getId() == SM::Alive.getId()) { - publishStatus(true); - } else if (sm_.getState().getId() == SM::AwaitSisterDeath.getId()) { - publishStatus(false); - } else { - publishingTimer_.stop(); - } + boost::mutex::scoped_lock lock(mutex_); + if (sm_.getState().getId() == SM::WaitingForSister.getId() || + sm_.getState().getId() == SM::Alive.getId()) + { + publishStatus(true); + } + else if (sm_.getState().getId() == SM::AwaitSisterDeath.getId()) + { + publishStatus(false); + } + else + { + publishingTimer_.stop(); + } } void Bond::publishStatus(bool active) { - bond::Status::Ptr msg(new bond::Status); - msg->header.stamp = ros::Time::now(); - msg->id = id_; - msg->instance_id = instance_id_; - msg->active = active; - msg->heartbeat_timeout = heartbeat_timeout_; - msg->heartbeat_period = heartbeat_period_; - pub_.publish(msg); + bond::Status::Ptr msg(new bond::Status); + msg->header.stamp = ros::Time::now(); + msg->id = id_; + msg->instance_id = instance_id_; + msg->active = active; + msg->heartbeat_timeout = heartbeat_timeout_; + msg->heartbeat_period = heartbeat_period_; + pub_.publish(msg); } - void Bond::flushPendingCallbacks() { - std::vector > callbacks; - { - boost::mutex::scoped_lock lock(mutex_); - callbacks = pending_callbacks_; - pending_callbacks_.clear(); - } + std::vector > callbacks; + { + boost::mutex::scoped_lock lock(mutex_); + callbacks = pending_callbacks_; + pending_callbacks_.clear(); + } - for (size_t i = 0; i < callbacks.size(); ++i) { - callbacks[i](); - } + for (size_t i = 0; i < callbacks.size(); ++i) + { + callbacks[i](); + } } -} // namespace bond - +} // namespace bond void BondSM::Connected() { - b->connect_timer_.cancel(); - b->condition_.notify_all(); - if (b->on_formed_) { - b->pending_callbacks_.push_back(b->on_formed_); - } + b->connect_timer_.cancel(); + b->condition_.notify_all(); + if (b->on_formed_) + { + b->pending_callbacks_.push_back(b->on_formed_); + } } void BondSM::SisterDied() { - b->sisterDiedFirst_ = true; + b->sisterDiedFirst_ = true; } void BondSM::Death() { - b->condition_.notify_all(); - b->heartbeat_timer_.cancel(); - b->disconnect_timer_.cancel(); - if (b->on_broken_) { - b->pending_callbacks_.push_back(b->on_broken_); - } + b->condition_.notify_all(); + b->heartbeat_timer_.cancel(); + b->disconnect_timer_.cancel(); + if (b->on_broken_) + { + b->pending_callbacks_.push_back(b->on_broken_); + } } void BondSM::Heartbeat() { - b->heartbeat_timer_.reset(); + b->heartbeat_timer_.reset(); } void BondSM::StartDying() { - b->heartbeat_timer_.cancel(); - b->disconnect_timer_.reset(); - b->publishingTimer_.setPeriod(ros::WallDuration(bond::Constants::DEAD_PUBLISH_PERIOD)); + b->heartbeat_timer_.cancel(); + b->disconnect_timer_.reset(); + b->publishingTimer_.setPeriod(ros::WallDuration(bond::Constants::DEAD_PUBLISH_PERIOD)); } diff --git a/bondcpp/src/timeout.cpp b/bondcpp/src/timeout.cpp index e014ab38..28da7603 100644 --- a/bondcpp/src/timeout.cpp +++ b/bondcpp/src/timeout.cpp @@ -33,56 +33,56 @@ namespace bond { -Timeout::Timeout(const ros::Duration &d, +Timeout::Timeout(const ros::Duration& d, boost::function on_timeout) - : duration_(d.sec, d.nsec), on_timeout_(on_timeout) + : duration_(d.sec, d.nsec), on_timeout_(on_timeout) { } -Timeout::Timeout(const ros::WallDuration &d, +Timeout::Timeout(const ros::WallDuration& d, boost::function on_timeout) - : duration_(d), on_timeout_(on_timeout) + : duration_(d), on_timeout_(on_timeout) { } Timeout::~Timeout() { - timer_.stop(); + timer_.stop(); } -void Timeout::setDuration(const ros::Duration &d) +void Timeout::setDuration(const ros::Duration& d) { - duration_ = ros::WallDuration(d.sec, d.nsec); + duration_ = ros::WallDuration(d.sec, d.nsec); } -void Timeout::setDuration(const ros::WallDuration &d) +void Timeout::setDuration(const ros::WallDuration& d) { - duration_ = d; + duration_ = d; } - void Timeout::reset() { - timer_.stop(); - timer_ = nh_.createSteadyTimer(duration_, &Timeout::timerCallback, this, true); - deadline_ = ros::SteadyTime::now() + duration_; + timer_.stop(); + timer_ = nh_.createSteadyTimer(duration_, &Timeout::timerCallback, this, true); + deadline_ = ros::SteadyTime::now() + duration_; } void Timeout::cancel() { - timer_.stop(); + timer_.stop(); } ros::WallDuration Timeout::left() { - return std::max(ros::WallDuration(0.0), deadline_ - ros::SteadyTime::now()); + return std::max(ros::WallDuration(0.0), deadline_ - ros::SteadyTime::now()); } -void Timeout::timerCallback(const ros::SteadyTimerEvent &) +void Timeout::timerCallback(const ros::SteadyTimerEvent&) { - if (on_timeout_) { - on_timeout_(); - } + if (on_timeout_) + { + on_timeout_(); + } } -} // namespace bond +} // namespace bond