From d1dc92a12bd819bc245fcf1587978ea2f60cc0bf Mon Sep 17 00:00:00 2001 From: James Xu Date: Thu, 11 Jul 2019 19:18:28 -0700 Subject: [PATCH 1/5] reformat with ISO CPP coding standards --- bondcpp/include/bondcpp/BondSM_sm.h | 75 ++--- bondcpp/include/bondcpp/bond.h | 22 +- bondcpp/include/bondcpp/timeout.h | 13 +- bondcpp/src/BondSM_sm.cpp | 8 +- bondcpp/src/bond.cpp | 505 +++++++++++++++------------- bondcpp/src/timeout.cpp | 20 +- 6 files changed, 330 insertions(+), 313 deletions(-) diff --git a/bondcpp/include/bondcpp/BondSM_sm.h b/bondcpp/include/bondcpp/BondSM_sm.h index fb673f0c..6171a79d 100644 --- a/bondcpp/include/bondcpp/BondSM_sm.h +++ b/bondcpp/include/bondcpp/BondSM_sm.h @@ -8,7 +8,6 @@ * from file : BondSM_sm.sm */ - #define SMC_USES_IOSTREAMS #include @@ -24,13 +23,13 @@ class BondSMState; class BondSMContext; class BondSM; -class BondSMState : - public statemap::State +class BondSMState : public statemap::State { public: BondSMState(const char *name, int stateId) - : statemap::State(name, stateId) - {}; + : statemap::State(name, stateId) + { + }; virtual void Entry(BondSMContext&) {} virtual void Exit(BondSMContext&) {} @@ -55,22 +54,22 @@ class SM static SM_Dead Dead; }; -class SM_Default : - public BondSMState +class SM_Default : public BondSMState { public: SM_Default(const char *name, int stateId) - : BondSMState(name, stateId) - {}; + : BondSMState(name, stateId) + { + }; }; -class SM_WaitingForSister : - public SM_Default +class SM_WaitingForSister : public SM_Default { public: SM_WaitingForSister(const char *name, int stateId) - : SM_Default(name, stateId) - {}; + : SM_Default(name, stateId) + { + }; void ConnectTimeout(BondSMContext& context); void Die(BondSMContext& context); @@ -78,13 +77,13 @@ class SM_WaitingForSister : void SisterDead(BondSMContext& context); }; -class SM_Alive : - public SM_Default +class SM_Alive : public SM_Default { public: SM_Alive(const char *name, int stateId) - : SM_Default(name, stateId) - {}; + : SM_Default(name, stateId) + { + }; void Die(BondSMContext& context); void HeartbeatTimeout(BondSMContext& context); @@ -92,13 +91,13 @@ class SM_Alive : void SisterDead(BondSMContext& context); }; -class SM_AwaitSisterDeath : - public SM_Default +class SM_AwaitSisterDeath : public SM_Default { public: SM_AwaitSisterDeath(const char *name, int stateId) - : SM_Default(name, stateId) - {}; + : SM_Default(name, stateId) + { + }; void Die(BondSMContext& context); void DisconnectTimeout(BondSMContext& context); @@ -107,13 +106,13 @@ class SM_AwaitSisterDeath : void SisterDead(BondSMContext& context); }; -class SM_Dead : - public SM_Default +class SM_Dead : public SM_Default { public: SM_Dead(const char *name, int stateId) - : SM_Default(name, stateId) - {}; + : SM_Default(name, stateId) + { + }; void ConnectTimeout(BondSMContext& context); void Die(BondSMContext& context); @@ -123,19 +122,18 @@ class SM_Dead : void SisterDead(BondSMContext& context); }; -class BondSMContext : - public statemap::FSMContext +class BondSMContext : public statemap::FSMContext { public: BondSMContext(BondSM& owner) - : FSMContext(SM::WaitingForSister), - _owner(owner) - {}; + : FSMContext(SM::WaitingForSister), _owner(owner) + { + }; BondSMContext(BondSM& owner, const statemap::State& state) - : FSMContext(state), - _owner(owner) - {}; + : FSMContext(state), _owner(owner) + { + }; virtual void enterStartState() { @@ -155,7 +153,7 @@ class BondSMContext : throw statemap::StateUndefinedException(); } - return (dynamic_cast(*_state)); + return dynamic_cast(*_state); }; void ConnectTimeout() @@ -192,11 +190,4 @@ class BondSMContext : BondSM& _owner; }; - -/* - * Local variables: - * buffer-read-only: t - * End: - */ - -#endif // BONDCPP__BONDSM_SM_H_ +#endif // BONDCPP__BONDSM_SM_H_ diff --git a/bondcpp/include/bondcpp/bond.h b/bondcpp/include/bondcpp/bond.h index 40b860b7..6a0dc089 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()); @@ -189,12 +190,11 @@ class BONDCPP_DECL Bond void doPublishing(const ros::SteadyTimerEvent &e); void publishStatus(bool active); - std::vector > pending_callbacks_; + std::vector> pending_callbacks_; void flushPendingCallbacks(); }; -} // namespace bond - +} // namespace bond // Internal use only struct BondSM @@ -210,4 +210,4 @@ struct BondSM 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..88e3fd64 100644 --- a/bondcpp/include/bondcpp/timeout.h +++ b/bondcpp/include/bondcpp/timeout.h @@ -38,11 +38,11 @@ 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 @@ -63,7 +63,6 @@ class Timeout void timerCallback(const ros::SteadyTimerEvent &e); }; +} // namespace bond -} // namespace bond - -#endif // BONDCPP__TIMEOUT_H_ +#endif // BONDCPP__TIMEOUT_H_ diff --git a/bondcpp/src/BondSM_sm.cpp b/bondcpp/src/BondSM_sm.cpp index 9879744d..80fa61b2 100644 --- a/bondcpp/src/BondSM_sm.cpp +++ b/bondcpp/src/BondSM_sm.cpp @@ -52,7 +52,7 @@ void BondSMState::SisterDead(BondSMContext& context) void BondSMState::Default(BondSMContext& context) { - throw ( + throw( statemap::TransitionUndefinedException( context.getState().getName(), context.getTransition())); @@ -353,9 +353,3 @@ void SM_Dead::SisterDead(BondSMContext& context) return; } - -/* - * Local variables: - * buffer-read-only: t - * End: - */ diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 58e7901d..e90a02d4 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) { - 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..4db8f3b1 100644 --- a/bondcpp/src/timeout.cpp +++ b/bondcpp/src/timeout.cpp @@ -33,15 +33,15 @@ 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) { } @@ -50,17 +50,16 @@ Timeout::~Timeout() timer_.stop(); } -void Timeout::setDuration(const ros::Duration &d) +void Timeout::setDuration(const ros::Duration& d) { duration_ = ros::WallDuration(d.sec, d.nsec); } -void Timeout::setDuration(const ros::WallDuration &d) +void Timeout::setDuration(const ros::WallDuration& d) { duration_ = d; } - void Timeout::reset() { timer_.stop(); @@ -78,11 +77,12 @@ ros::WallDuration Timeout::left() 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_) { + if (on_timeout_) + { on_timeout_(); } } -} // namespace bond +} // namespace bond From bc2ba6f240f1b92d84827f2c5843d93fc90cbba3 Mon Sep 17 00:00:00 2001 From: James Xu Date: Thu, 11 Jul 2019 19:23:49 -0700 Subject: [PATCH 2/5] fix condition_.timed_wait spacing --- bondcpp/src/bond.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index e90a02d4..6cba8a4f 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -227,7 +227,7 @@ bool Bond::waitUntilFormed(ros::WallDuration timeout) } condition_.timed_wait(mutex_, boost::posix_time::milliseconds( - static_cast(wait_time.toSec() * 1000.0f))); + static_cast(wait_time.toSec() * 1000.0f))); } return sm_.getState().getId() != SM::WaitingForSister.getId(); } @@ -260,7 +260,7 @@ bool Bond::waitUntilBroken(ros::WallDuration timeout) } condition_.timed_wait(mutex_, boost::posix_time::milliseconds( - static_cast(wait_time.toSec() * 1000.0f))); + static_cast(wait_time.toSec() * 1000.0f))); } return sm_.getState().getId() == SM::Dead.getId(); } From 3df1ba23972d774dd29807cdfd27f942ff10a7eb Mon Sep 17 00:00:00 2001 From: James Xu Date: Thu, 11 Jul 2019 19:53:44 -0700 Subject: [PATCH 3/5] fix missed files --- bondcpp/include/bondcpp/bond.h | 14 +++++++------- bondcpp/include/bondcpp/timeout.h | 10 +++++----- bondcpp/src/bond.cpp | 6 +++--- bondcpp/src/timeout.cpp | 24 ++++++++++++------------ 4 files changed, 27 insertions(+), 27 deletions(-) diff --git a/bondcpp/include/bondcpp/bond.h b/bondcpp/include/bondcpp/bond.h index 6a0dc089..74378085 100644 --- a/bondcpp/include/bondcpp/bond.h +++ b/bondcpp/include/bondcpp/bond.h @@ -76,8 +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()); @@ -94,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. */ @@ -185,9 +185,9 @@ 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_; @@ -199,7 +199,7 @@ class BONDCPP_DECL 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_ diff --git a/bondcpp/include/bondcpp/timeout.h b/bondcpp/include/bondcpp/timeout.h index 88e3fd64..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, + const ros::Duration& d, boost::function on_timeout = boost::function()); Timeout( - const ros::WallDuration &d, + 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,7 +60,7 @@ class Timeout ros::WallDuration duration_; boost::function on_timeout_; - void timerCallback(const ros::SteadyTimerEvent &e); + void timerCallback(const ros::SteadyTimerEvent& e); }; } // namespace bond diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 6cba8a4f..5f977210 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -50,9 +50,9 @@ static std::string makeUUID() #ifdef _WIN32 UUID uuid; UuidCreate(&uuid); - unsigned char *str; + unsigned char* str; UuidToStringA(&uuid, &str); - std::string return_string(reinterpret_cast(str)); + std::string return_string(reinterpret_cast(str)); RpcStringFreeA(&str); return return_string; #else @@ -165,7 +165,7 @@ void Bond::setHeartbeatPeriod(double dur) heartbeat_period_ = dur; } -void Bond::setCallbackQueue(ros::CallbackQueueInterface *queue) +void Bond::setCallbackQueue(ros::CallbackQueueInterface* queue) { if (started_) { diff --git a/bondcpp/src/timeout.cpp b/bondcpp/src/timeout.cpp index 4db8f3b1..28da7603 100644 --- a/bondcpp/src/timeout.cpp +++ b/bondcpp/src/timeout.cpp @@ -47,42 +47,42 @@ Timeout::Timeout(const ros::WallDuration& d, Timeout::~Timeout() { - timer_.stop(); + timer_.stop(); } 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) { - 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&) { - if (on_timeout_) - { - on_timeout_(); - } + if (on_timeout_) + { + on_timeout_(); + } } } // namespace bond From af1b69c47b1d8a57cfde56f15e5b91013d15804a Mon Sep 17 00:00:00 2001 From: James Xu Date: Thu, 11 Jul 2019 19:54:50 -0700 Subject: [PATCH 4/5] revert changes to auto-generated state machine code --- bondcpp/include/bondcpp/BondSM_sm.h | 75 ++++++++++++++++------------- bondcpp/src/BondSM_sm.cpp | 8 ++- 2 files changed, 49 insertions(+), 34 deletions(-) diff --git a/bondcpp/include/bondcpp/BondSM_sm.h b/bondcpp/include/bondcpp/BondSM_sm.h index 6171a79d..fb673f0c 100644 --- a/bondcpp/include/bondcpp/BondSM_sm.h +++ b/bondcpp/include/bondcpp/BondSM_sm.h @@ -8,6 +8,7 @@ * from file : BondSM_sm.sm */ + #define SMC_USES_IOSTREAMS #include @@ -23,13 +24,13 @@ class BondSMState; class BondSMContext; class BondSM; -class BondSMState : public statemap::State +class BondSMState : + public statemap::State { public: BondSMState(const char *name, int stateId) - : statemap::State(name, stateId) - { - }; + : statemap::State(name, stateId) + {}; virtual void Entry(BondSMContext&) {} virtual void Exit(BondSMContext&) {} @@ -54,22 +55,22 @@ class SM static SM_Dead Dead; }; -class SM_Default : public BondSMState +class SM_Default : + public BondSMState { public: SM_Default(const char *name, int stateId) - : BondSMState(name, stateId) - { - }; + : BondSMState(name, stateId) + {}; }; -class SM_WaitingForSister : public SM_Default +class SM_WaitingForSister : + public SM_Default { public: SM_WaitingForSister(const char *name, int stateId) - : SM_Default(name, stateId) - { - }; + : SM_Default(name, stateId) + {}; void ConnectTimeout(BondSMContext& context); void Die(BondSMContext& context); @@ -77,13 +78,13 @@ class SM_WaitingForSister : public SM_Default void SisterDead(BondSMContext& context); }; -class SM_Alive : public SM_Default +class SM_Alive : + public SM_Default { public: SM_Alive(const char *name, int stateId) - : SM_Default(name, stateId) - { - }; + : SM_Default(name, stateId) + {}; void Die(BondSMContext& context); void HeartbeatTimeout(BondSMContext& context); @@ -91,13 +92,13 @@ class SM_Alive : public SM_Default void SisterDead(BondSMContext& context); }; -class SM_AwaitSisterDeath : public SM_Default +class SM_AwaitSisterDeath : + public SM_Default { public: SM_AwaitSisterDeath(const char *name, int stateId) - : SM_Default(name, stateId) - { - }; + : SM_Default(name, stateId) + {}; void Die(BondSMContext& context); void DisconnectTimeout(BondSMContext& context); @@ -106,13 +107,13 @@ class SM_AwaitSisterDeath : public SM_Default void SisterDead(BondSMContext& context); }; -class SM_Dead : public SM_Default +class SM_Dead : + public SM_Default { public: SM_Dead(const char *name, int stateId) - : SM_Default(name, stateId) - { - }; + : SM_Default(name, stateId) + {}; void ConnectTimeout(BondSMContext& context); void Die(BondSMContext& context); @@ -122,18 +123,19 @@ class SM_Dead : public SM_Default void SisterDead(BondSMContext& context); }; -class BondSMContext : public statemap::FSMContext +class BondSMContext : + public statemap::FSMContext { public: BondSMContext(BondSM& owner) - : FSMContext(SM::WaitingForSister), _owner(owner) - { - }; + : FSMContext(SM::WaitingForSister), + _owner(owner) + {}; BondSMContext(BondSM& owner, const statemap::State& state) - : FSMContext(state), _owner(owner) - { - }; + : FSMContext(state), + _owner(owner) + {}; virtual void enterStartState() { @@ -153,7 +155,7 @@ class BondSMContext : public statemap::FSMContext throw statemap::StateUndefinedException(); } - return dynamic_cast(*_state); + return (dynamic_cast(*_state)); }; void ConnectTimeout() @@ -190,4 +192,11 @@ class BondSMContext : public statemap::FSMContext BondSM& _owner; }; -#endif // BONDCPP__BONDSM_SM_H_ + +/* + * Local variables: + * buffer-read-only: t + * End: + */ + +#endif // BONDCPP__BONDSM_SM_H_ diff --git a/bondcpp/src/BondSM_sm.cpp b/bondcpp/src/BondSM_sm.cpp index 80fa61b2..9879744d 100644 --- a/bondcpp/src/BondSM_sm.cpp +++ b/bondcpp/src/BondSM_sm.cpp @@ -52,7 +52,7 @@ void BondSMState::SisterDead(BondSMContext& context) void BondSMState::Default(BondSMContext& context) { - throw( + throw ( statemap::TransitionUndefinedException( context.getState().getName(), context.getTransition())); @@ -353,3 +353,9 @@ void SM_Dead::SisterDead(BondSMContext& context) return; } + +/* + * Local variables: + * buffer-read-only: t + * End: + */ From c2d6706ac978a1efcffa4bbe5d2771e435789160 Mon Sep 17 00:00:00 2001 From: James Xu Date: Thu, 11 Jul 2019 20:07:29 -0700 Subject: [PATCH 5/5] use > > over >> --- bondcpp/include/bondcpp/bond.h | 2 +- bondcpp/src/bond.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bondcpp/include/bondcpp/bond.h b/bondcpp/include/bondcpp/bond.h index 74378085..60e5e615 100644 --- a/bondcpp/include/bondcpp/bond.h +++ b/bondcpp/include/bondcpp/bond.h @@ -190,7 +190,7 @@ class BONDCPP_DECL Bond void doPublishing(const ros::SteadyTimerEvent& e); void publishStatus(bool active); - std::vector> pending_callbacks_; + std::vector > pending_callbacks_; void flushPendingCallbacks(); }; diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 5f977210..f75fc692 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -398,7 +398,7 @@ void Bond::publishStatus(bool active) void Bond::flushPendingCallbacks() { - std::vector> callbacks; + std::vector > callbacks; { boost::mutex::scoped_lock lock(mutex_); callbacks = pending_callbacks_;