pax_global_header00006660000000000000000000000064137222305310014510gustar00rootroot0000000000000052 comment=5a79fecb3e8b6f2c210d83669b5cb40d47d62c47 bond_core-1.8.6/000077500000000000000000000000001372223053100134565ustar00rootroot00000000000000bond_core-1.8.6/.gitignore000066400000000000000000000000061372223053100154420ustar00rootroot00000000000000*.pyc bond_core-1.8.6/bond/000077500000000000000000000000001372223053100144005ustar00rootroot00000000000000bond_core-1.8.6/bond/BondSM.sm000066400000000000000000000016351372223053100160700ustar00rootroot00000000000000// -*- tab-width: 4; -*- %start SM::WaitingForSister %class BondSM %header bondcpp/bond.h %map SM %% // Transition End State Action(s) WaitingForSister { SisterAlive Alive { Connected(); } SisterDead Dead { Connected(); SisterDied(); Death(); } ConnectTimeout Dead { Death(); } Die Dead { Death(); } } Alive { SisterAlive Alive { Heartbeat(); } SisterDead Dead { SisterDied(); Death(); } HeartbeatTimeout Dead { Death(); } Die AwaitSisterDeath { StartDying(); } } AwaitSisterDeath { SisterAlive AwaitSisterDeath {} HeartbeatTimeout AwaitSisterDeath {} Die AwaitSisterDeath {} SisterDead Dead { Death(); } DisconnectTimeout Dead { Death(); } } Dead { SisterAlive Dead {} SisterDead Dead {} DisconnectTimeout Dead {} HeartbeatTimeout Dead {} ConnectTimeout Dead {} Die Dead {} } %% bond_core-1.8.6/bond/CHANGELOG.rst000066400000000000000000000062071372223053100164260ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package bond ^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.8.6 (2020-08-28) ------------------ 1.8.5 (2020-05-14) ------------------ * Bump CMake minimum version to use CMP0048 (`#58 `_) * Contributors: Michael Carroll 1.8.4 (2020-02-24) ------------------ * Make Michael Carroll the maintainer (`#40 `_) * Contributors: Mikael Arguedas 1.8.3 (2018-08-17) ------------------ 1.8.2 (2018-04-27) ------------------ 1.8.1 (2017-10-27) ------------------ * fix package.xml to comply with schema (`#30 `_) * Contributors: Mikael Arguedas 1.8.0 (2017-07-27) ------------------ * switch to package format 2 (`#27 `_) * Contributors: Mikael Arguedas 1.7.19 (2017-03-27) ------------------- 1.7.18 (2016-10-24) ------------------- 1.7.17 (2016-03-15) ------------------- * update maintainer * Contributors: Mikael Arguedas 1.7.16 (2014-10-30) ------------------- 1.7.15 (2014-10-28) ------------------- 1.7.14 (2014-05-08) ------------------- * Export architecture_independent flag in package.xml `#4 `_ * Update maintainer field * Contributors: Esteve Fernandez, Scott K Logan, Vincent Rabaud 1.7.13 (2013-08-21) ------------------- 1.7.12 (2013-06-06) ------------------- 1.7.11 (2013-03-13) ------------------- 1.7.10 (2013-01-13) ------------------- 1.7.9 (2012-12-27) ------------------ * modified dep type of catkin * Contributors: Dirk Thomas 1.7.8 (2012-12-13) ------------------ * add missing downstream depend * switched from langs to message\_* packages * Contributors: Dirk Thomas 1.7.7 (2012-12-06) ------------------ * Updated url tags in package.xml's `#1 `_ * Contributors: William Woodall 1.7.6 (2012-10-30) ------------------ * fix catkin function order * Contributors: Dirk Thomas 1.7.5 (2012-10-27) ------------------ * clean up package.xml files * Contributors: Dirk Thomas 1.7.4 (2012-10-06) ------------------ 1.7.3 (2012-10-02 00:19) ------------------------ 1.7.2 (2012-10-02 00:06) ------------------------ * add the missing catkin dependency * Contributors: Vincent Rabaud 1.7.1 (2012-10-01 19:00) ------------------------ 1.7.0 (2012-10-01 16:51) ------------------------ * catkinize the package and bump to 1.7.0 even though it is not tagged yet * Modified bond's state machine to handle "alive" messages from the sibling when already dead. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036189 * Added global "bond_disable_heartbeat_timeout" parameter --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036106 * The bond state machine more gracefully handles excessive requests to die. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032653 * Moving bond into common --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032634 * Contributors: Vincent Rabaud, sglaser bond_core-1.8.6/bond/CMakeLists.txt000066400000000000000000000004321372223053100171370ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(bond) find_package(catkin REQUIRED message_generation std_msgs) add_message_files(DIRECTORY msg FILES Constants.msg Status.msg ) generate_messages(DEPENDENCIES std_msgs) catkin_package(CATKIN_DEPENDS message_runtime std_msgs) bond_core-1.8.6/bond/mainpage.dox000066400000000000000000000011061372223053100166730ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b link is ... \section codeapi Code API */ bond_core-1.8.6/bond/msg/000077500000000000000000000000001372223053100151665ustar00rootroot00000000000000bond_core-1.8.6/bond/msg/Constants.msg000066400000000000000000000004111372223053100176460ustar00rootroot00000000000000float32 DEAD_PUBLISH_PERIOD = 0.05 float32 DEFAULT_CONNECT_TIMEOUT = 10.0 float32 DEFAULT_HEARTBEAT_TIMEOUT = 4.0 float32 DEFAULT_DISCONNECT_TIMEOUT = 2.0 float32 DEFAULT_HEARTBEAT_PERIOD = 1.0 string DISABLE_HEARTBEAT_TIMEOUT_PARAM=/bond_disable_heartbeat_timeoutbond_core-1.8.6/bond/msg/Status.msg000066400000000000000000000004111372223053100171550ustar00rootroot00000000000000Header header string id # ID of the bond string instance_id # Unique ID for an individual in a bond bool active # Including the timeouts for the bond makes it easier to debug mis-matches # between the two sides. float32 heartbeat_timeout float32 heartbeat_periodbond_core-1.8.6/bond/package.xml000066400000000000000000000020701372223053100165140ustar00rootroot00000000000000 bond 1.8.6 A bond allows two processes, A and B, to know when the other has terminated, either cleanly or by crashing. The bond remains connected until it is either broken explicitly or until a heartbeat times out. Michael Carroll BSD http://www.ros.org/wiki/bond https://github.com/ros/bond_core/issues https://github.com/ros/bond_core Stuart Glaser catkin message_generation std_msgs message_runtime std_msgs bond_core-1.8.6/bond_core/000077500000000000000000000000001372223053100154105ustar00rootroot00000000000000bond_core-1.8.6/bond_core/CHANGELOG.rst000066400000000000000000000036501372223053100174350ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package bond_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.8.6 (2020-08-28) ------------------ 1.8.5 (2020-05-14) ------------------ * Bump CMake minimum version to use CMP0048 (`#58 `_) * Contributors: Michael Carroll 1.8.4 (2020-02-24) ------------------ * Make Michael Carroll the maintainer (`#40 `_) * Contributors: Mikael Arguedas 1.8.3 (2018-08-17) ------------------ 1.8.2 (2018-04-27) ------------------ 1.8.1 (2017-10-27) ------------------ 1.8.0 (2017-07-27) ------------------ * switch to package format 2 (`#27 `_) * Contributors: Mikael Arguedas 1.7.19 (2017-03-27) ------------------- 1.7.18 (2016-10-24) ------------------- 1.7.17 (2016-03-15) ------------------- * update maintainer * Contributors: Mikael Arguedas 1.7.16 (2014-10-30) ------------------- 1.7.15 (2014-10-28) ------------------- 1.7.14 (2014-05-08) ------------------- * Update maintainer field * Contributors: Esteve Fernandez 1.7.13 (2013-08-21) ------------------- 1.7.12 (2013-06-06) ------------------- 1.7.11 (2013-03-13) ------------------- * add CMakeLists.txt for metapackage * Contributors: Dirk Thomas 1.7.10 (2013-01-13) ------------------- 1.7.9 (2012-12-27) ------------------ * modified dep type of catkin * Contributors: Dirk Thomas 1.7.8 (2012-12-13) ------------------ 1.7.7 (2012-12-06) ------------------ 1.7.6 (2012-10-30) ------------------ 1.7.5 (2012-10-27) ------------------ * clean up package.xml files * Contributors: Dirk Thomas 1.7.4 (2012-10-06) ------------------ * adding bond_core metapackage to replace former stack * Contributors: Tully Foote 1.7.3 (2012-10-02 00:19) ------------------------ 1.7.2 (2012-10-02 00:06) ------------------------ 1.7.1 (2012-10-01 19:00) ------------------------ 1.7.0 (2012-10-01 16:51) ------------------------ bond_core-1.8.6/bond_core/CMakeLists.txt000066400000000000000000000001541372223053100201500ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(bond_core) find_package(catkin REQUIRED) catkin_metapackage() bond_core-1.8.6/bond_core/package.xml000066400000000000000000000020221372223053100175210ustar00rootroot00000000000000 bond_core 1.8.6 A bond allows two processes, A and B, to know when the other has terminated, either cleanly or by crashing. The bond remains connected until it is either broken explicitly or until a heartbeat times out. Michael Carroll BSD http://www.ros.org/wiki/bond_core https://github.com/ros/bond_core/issues https://github.com/ros/bond_core Stu Glaser catkin bond bondcpp bondpy smclib bond_core-1.8.6/bondcpp/000077500000000000000000000000001372223053100151035ustar00rootroot00000000000000bond_core-1.8.6/bondcpp/CHANGELOG.rst000066400000000000000000000161401372223053100171260ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package bondcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.8.6 (2020-08-28) ------------------ * narrow down required boost dependencies (`#68 `_) * Contributors: Mikael Arguedas 1.8.5 (2020-05-14) ------------------ * Bump CMake minimum version to use CMP0048 (`#58 `_) * Contributors: Michael Carroll 1.8.4 (2020-02-24) ------------------ * Solved static linking problems with bondcpp (`#44 `_) * Add BONDCPP_DECL for Bond class (`#2 `_) (`#45 `_) * Make Michael Carroll the maintainer (`#40 `_) * Contributors: James Xu, Mikael Arguedas, ivanpauno 1.8.3 (2018-08-17) ------------------ * Argument to Boost Milliseconds must be integral in Boost >= 1.67 (`#37 `_) * Argument to Boost milliseconds must be integral * Fix style * More consistent type * Contributors: Paul-Edouard Sarlin 1.8.2 (2018-04-27) ------------------ * uuid dependency fixup (`#36 `_) * dont export uuid dependency as this isnt anywhere in the public api * fixx uuid dependency in test_bond as well * Contributors: Mikael Arguedas 1.8.1 (2017-10-27) ------------------ * fix package.xml to comply with schema (`#30 `_) * Contributors: Mikael Arguedas 1.8.0 (2017-07-27) ------------------ * Use SteadyTime and SteadyTimer for bond timeouts (`#18 `_) * C++ style (`#28 `_) * switch to package format 2 (`#27 `_) * remove trailing whitespaces (`#26 `_) * Contributors: Felix Ruess, Mikael Arguedas 1.7.19 (2017-03-27) ------------------- * fix unused var warning * Contributors: Mikael Arguedas 1.7.18 (2016-10-24) ------------------- * fix -isystem /usr/include build breakage in gcc6 * Contributors: Mikael Arguedas 1.7.17 (2016-03-15) ------------------- * update maintainer * Contributors: Mikael Arguedas 1.7.16 (2014-10-30) ------------------- * Fix depedency version * Contributors: Esteve Fernandez 1.7.15 (2014-10-28) ------------------- * Added version dependency. * Removed redundant include_directories * Added cmake_modules in alphabetical order * Use FindUUID.cmake from cmake-modules to find the UUID libraries `#8 `_ * Contributors: Esteve Fernandez 1.7.14 (2014-05-08) ------------------- * Update maintainer field * Contributors: Esteve Fernandez, Vincent Rabaud 1.7.13 (2013-08-21) ------------------- * Use c++ style reinterpret_cast rather than c style cast * use rpc for uuid on windows * add missing archive/library/runtime destinations for library * Contributors: David Hodo, Dirk Thomas, William Woodall 1.7.12 (2013-06-06) ------------------- * fix dependency on exported targets if the variable is empty * use EXPORTED_TARGETS variable instead of explicit target names * Contributors: Dirk Thomas 1.7.11 (2013-03-13) ------------------- 1.7.10 (2013-01-13) ------------------- * add missing link library uuid `#6 `_ * Contributors: Dirk Thomas 1.7.9 (2012-12-27) ------------------ * modified dep type of catkin * Contributors: Dirk Thomas 1.7.8 (2012-12-13) ------------------ 1.7.7 (2012-12-06) ------------------ * Added missing link against catkin_LIBRARIES * Updated url tags in package.xml's `#1 `_ * updated catkin_package(DEPENDS) * Contributors: Dirk Thomas, William Woodall 1.7.6 (2012-10-30) ------------------ * fix catkin function order * Contributors: Dirk Thomas 1.7.5 (2012-10-27) ------------------ * clean up package.xml files * add missing target dependency to gencpp * Contributors: Dirk Thomas 1.7.4 (2012-10-06) ------------------ 1.7.3 (2012-10-02 00:19) ------------------------ * fix package building issues * Contributors: Vincent Rabaud 1.7.2 (2012-10-02 00:06) ------------------------ * add the missing catkin dependency * Contributors: Vincent Rabaud 1.7.1 (2012-10-01 19:00) ------------------------ * add missing dependencies * Contributors: Vincent Rabaud 1.7.0 (2012-10-01 16:51) ------------------------ * catkinize bond * catkinize the package and bump to 1.7.0 even though it is not tagged yet * add link flag for OSX * removed spurious reference to libroslib * bondcpp now explicitly links against the ros library. `#5334 `_ * Changed ros::Time/Duration to ros::WallTime/WallDuration so Bond still works when time stops. Fixes `#5035 `_ * Fixed destruction bug: doesn't destroy things if the bond was never started. * Can now set a bond's callback queue --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4037081 * Modified bond's state machine to handle "alive" messages from the sibling when already dead. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036189 * Added global "bond_disable_heartbeat_timeout" parameter --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036106 * typo --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035731 * rosdep and packages are not the same --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035730 * patch from stevenbellens for fedora uuid support `#4756 `_ --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035729 * Re-ordering locking in bondcpp's destructor --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035719 * In bond, wait_until_formed and wait_until_broken terminate when ROS shuts down. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035632 * Bond no longer warns on destructor when the other side disappeared. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035573 * removed wiki syntax from description --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035392 * Creating package descriptions for bondpy, bondcpp, and test_bond. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035354 * The bond state machine more gracefully handles excessive requests to die. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032653 * Moving bond into common --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032634 * Contributors: Brian Gerkey, Stuart Glaser, Vincent Rabaud, kwc, sglaser, tfoote bond_core-1.8.6/bondcpp/CMakeLists.txt000066400000000000000000000017751372223053100176550ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(bondcpp) find_package(Boost REQUIRED date_time thread) find_package(catkin REQUIRED bond cmake_modules roscpp smclib) find_package(UUID REQUIRED) include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${UUID_INCLUDE_DIRS} ) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS bond roscpp smclib DEPENDS Boost UUID ) add_library(${PROJECT_NAME} src/timeout.cpp src/bond.cpp src/BondSM_sm.cpp ) target_link_libraries(${PROJECT_NAME} ${UUID_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) if(catkin_EXPORTED_TARGETS) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) endif() # install stuff install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) bond_core-1.8.6/bondcpp/include/000077500000000000000000000000001372223053100165265ustar00rootroot00000000000000bond_core-1.8.6/bondcpp/include/bondcpp/000077500000000000000000000000001372223053100201535ustar00rootroot00000000000000bond_core-1.8.6/bondcpp/include/bondcpp/BondSM_sm.h000066400000000000000000000100031372223053100221370ustar00rootroot00000000000000#ifndef BONDCPP__BONDSM_SM_H_ #define BONDCPP__BONDSM_SM_H_ /* * ex: set ro: * DO NOT EDIT. * generated by smc (http://smc.sourceforge.net/) * from file : BondSM_sm.sm */ #define SMC_USES_IOSTREAMS #include // Forward declarations. class SM; class SM_WaitingForSister; class SM_Alive; class SM_AwaitSisterDeath; class SM_Dead; class SM_Default; class BondSMState; class BondSMContext; class BondSM; class BondSMState : public statemap::State { public: BondSMState(const char *name, int stateId) : statemap::State(name, stateId) {}; virtual void Entry(BondSMContext&) {} virtual void Exit(BondSMContext&) {} virtual void ConnectTimeout(BondSMContext& context); virtual void Die(BondSMContext& context); virtual void DisconnectTimeout(BondSMContext& context); virtual void HeartbeatTimeout(BondSMContext& context); virtual void SisterAlive(BondSMContext& context); virtual void SisterDead(BondSMContext& context); protected: virtual void Default(BondSMContext& context); }; class SM { public: static SM_WaitingForSister WaitingForSister; static SM_Alive Alive; static SM_AwaitSisterDeath AwaitSisterDeath; static SM_Dead Dead; }; class SM_Default : public BondSMState { public: SM_Default(const char *name, int stateId) : BondSMState(name, stateId) {}; }; class SM_WaitingForSister : public SM_Default { public: SM_WaitingForSister(const char *name, int stateId) : SM_Default(name, stateId) {}; void ConnectTimeout(BondSMContext& context); void Die(BondSMContext& context); void SisterAlive(BondSMContext& context); void SisterDead(BondSMContext& context); }; class SM_Alive : public SM_Default { public: SM_Alive(const char *name, int stateId) : SM_Default(name, stateId) {}; void Die(BondSMContext& context); void HeartbeatTimeout(BondSMContext& context); void SisterAlive(BondSMContext& context); void SisterDead(BondSMContext& context); }; class SM_AwaitSisterDeath : public SM_Default { public: SM_AwaitSisterDeath(const char *name, int stateId) : SM_Default(name, stateId) {}; void Die(BondSMContext& context); void DisconnectTimeout(BondSMContext& context); void HeartbeatTimeout(BondSMContext& context); void SisterAlive(BondSMContext& context); void SisterDead(BondSMContext& context); }; class SM_Dead : public SM_Default { public: SM_Dead(const char *name, int stateId) : SM_Default(name, stateId) {}; void ConnectTimeout(BondSMContext& context); void Die(BondSMContext& context); void DisconnectTimeout(BondSMContext& context); void HeartbeatTimeout(BondSMContext& context); void SisterAlive(BondSMContext& context); void SisterDead(BondSMContext& context); }; class BondSMContext : public statemap::FSMContext { public: BondSMContext(BondSM& owner) : FSMContext(SM::WaitingForSister), _owner(owner) {}; BondSMContext(BondSM& owner, const statemap::State& state) : FSMContext(state), _owner(owner) {}; virtual void enterStartState() { getState().Entry(*this); return; } BondSM& getOwner() const { return (_owner); }; BondSMState& getState() const { if (_state == NULL) { throw statemap::StateUndefinedException(); } return (dynamic_cast(*_state)); }; void ConnectTimeout() { (getState()).ConnectTimeout(*this); }; void Die() { (getState()).Die(*this); }; void DisconnectTimeout() { (getState()).DisconnectTimeout(*this); }; void HeartbeatTimeout() { (getState()).HeartbeatTimeout(*this); }; void SisterAlive() { (getState()).SisterAlive(*this); }; void SisterDead() { (getState()).SisterDead(*this); }; private: BondSM& _owner; }; /* * Local variables: * buffer-read-only: t * End: */ #endif // BONDCPP__BONDSM_SM_H_ bond_core-1.8.6/bondcpp/include/bondcpp/bond.h000066400000000000000000000154311372223053100212520ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Stuart Glaser */ #ifndef BONDCPP__BOND_H_ #define BONDCPP__BOND_H_ #include #include #include #include #include #include #include #include #include "BondSM_sm.h" #include #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 #else // ros is being built around static libraries #define BONDCPP_DECL #endif namespace bond { /** \brief Forms a bond to monitor another process. * * The bond::Bond class implements a bond, allowing you to monitor * another process and be notified when it dies. In turn, it will be * notified when you die. */ class BONDCPP_DECL Bond { public: /** \brief Constructs a bond, but does not connect * * \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 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, boost::function on_broken = boost::function(), boost::function on_formed = boost::function()); /** \brief Destructs the object, breaking the bond if it is still formed. */ ~Bond(); double getConnectTimeout() const { return connect_timeout_; } void setConnectTimeout(double dur); double getDisconnectTimeout() const { return disconnect_timeout_; } void setDisconnectTimeout(double dur); double getHeartbeatTimeout() const { return heartbeat_timeout_; } void setHeartbeatTimeout(double dur); double getHeartbeatPeriod() const { return heartbeat_period_; } void setHeartbeatPeriod(double dur); void setCallbackQueue(ros::CallbackQueueInterface *queue); /** \brief Starts the bond and connects to the sister process. */ void start(); /** \brief Sets the formed callback. */ void setFormedCallback(boost::function on_formed); /** \brief Sets the broken callback */ void setBrokenCallback(boost::function on_broken); /** \brief Blocks until the bond is formed for at most 'duration'. * * \param timeout Maximum duration to wait. If -1 then this call will not timeout. * \return true iff the bond has been formed. */ bool waitUntilFormed(ros::Duration timeout = ros::Duration(-1)); /** \brief Blocks until the bond is formed for at most 'duration'. * * \param timeout Maximum duration to wait. If -1 then this call will not timeout. * \return true iff the bond has been formed. */ bool waitUntilFormed(ros::WallDuration timeout = ros::WallDuration(-1)); /** \brief Blocks until the bond is broken for at most 'duration'. * * \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(ros::Duration timeout = ros::Duration(-1)); /** \brief Blocks until the bond is broken for at most 'duration'. * * \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(ros::WallDuration timeout = ros::WallDuration(-1)); /** \brief Indicates if the bond is broken * \return true iff the bond has been 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_; } private: friend class ::BondSM; ros::NodeHandle nh_; boost::scoped_ptr bondsm_; BondSMContext sm_; std::string topic_; std::string id_; std::string instance_id_; std::string sister_instance_id_; boost::function on_broken_; boost::function on_formed_; bool sisterDiedFirst_; bool started_; boost::mutex mutex_; boost::condition condition_; double connect_timeout_; double heartbeat_timeout_; double disconnect_timeout_; double heartbeat_period_; Timeout connect_timer_; Timeout heartbeat_timer_; Timeout disconnect_timer_; ros::Subscriber sub_; ros::Publisher pub_; ros::SteadyTimer publishingTimer_; void onConnectTimeout(); void onHeartbeatTimeout(); void onDisconnectTimeout(); void bondStatusCB(const bond::Status::ConstPtr &msg); void doPublishing(const ros::SteadyTimerEvent &e); void publishStatus(bool active); std::vector > pending_callbacks_; void flushPendingCallbacks(); }; } // namespace bond // Internal use only struct BondSM { BondSM(bond::Bond *b_) : b(b_) {} void Connected(); void SisterDied(); void Death(); void Heartbeat(); void StartDying(); private: bond::Bond *b; }; #endif // BONDCPP__BOND_H_ bond_core-1.8.6/bondcpp/include/bondcpp/timeout.h000066400000000000000000000046401372223053100220160ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef BONDCPP__TIMEOUT_H_ #define BONDCPP__TIMEOUT_H_ #include namespace bond { class Timeout { public: Timeout( const ros::Duration &d, boost::function on_timeout = boost::function()); Timeout( 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 reset(); void cancel(); ros::WallDuration left(); private: ros::NodeHandle nh_; ros::SteadyTimer timer_; ros::SteadyTime deadline_; ros::WallDuration duration_; boost::function on_timeout_; void timerCallback(const ros::SteadyTimerEvent &e); }; } // namespace bond #endif // BONDCPP__TIMEOUT_H_ bond_core-1.8.6/bondcpp/mainpage.dox000066400000000000000000000014361372223053100174040ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b bondcpp is the C++ implementation of a bond, which allows two processes to monitor each other and be informed on the death of the other. You can be informed either by a callback or by polling that the other process in the bond is no longer alive. \section codeapi Code API The main class is bond::Bond */ bond_core-1.8.6/bondcpp/package.xml000066400000000000000000000017641372223053100172300ustar00rootroot00000000000000 bondcpp 1.8.6 C++ implementation of bond, a mechanism for checking when another process has terminated. Michael Carroll BSD http://www.ros.org/wiki/bondcpp https://github.com/ros/bond_core/issues https://github.com/ros/bond_core Stuart Glaser catkin cmake_modules bond libboost-dev libboost-date-time-dev libboost-thread-dev roscpp smclib uuid bond_core-1.8.6/bondcpp/src/000077500000000000000000000000001372223053100156725ustar00rootroot00000000000000bond_core-1.8.6/bondcpp/src/BondSM_sm.cpp000066400000000000000000000153011372223053100202170ustar00rootroot00000000000000/* * ex: set ro: * DO NOT EDIT. * generated by smc (http://smc.sourceforge.net/) * from file : BondSM_sm.sm */ #include "bondcpp/bond.h" #include "bondcpp/BondSM_sm.h" // Static class declarations. SM_WaitingForSister SM::WaitingForSister("SM::WaitingForSister", 0); SM_Alive SM::Alive("SM::Alive", 1); SM_AwaitSisterDeath SM::AwaitSisterDeath("SM::AwaitSisterDeath", 2); SM_Dead SM::Dead("SM::Dead", 3); void BondSMState::ConnectTimeout(BondSMContext& context) { Default(context); return; } void BondSMState::Die(BondSMContext& context) { Default(context); return; } void BondSMState::DisconnectTimeout(BondSMContext& context) { Default(context); return; } void BondSMState::HeartbeatTimeout(BondSMContext& context) { Default(context); return; } void BondSMState::SisterAlive(BondSMContext& context) { Default(context); return; } void BondSMState::SisterDead(BondSMContext& context) { Default(context); return; } void BondSMState::Default(BondSMContext& context) { throw ( statemap::TransitionUndefinedException( context.getState().getName(), context.getTransition())); return; } void SM_WaitingForSister::ConnectTimeout(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_WaitingForSister::Die(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_WaitingForSister::SisterAlive(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Connected(); context.setState(SM::Alive); } catch (...) { context.setState(SM::Alive); throw; } (context.getState()).Entry(context); return; } void SM_WaitingForSister::SisterDead(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Connected(); ctxt.SisterDied(); ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_Alive::Die(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.StartDying(); context.setState(SM::AwaitSisterDeath); } catch (...) { context.setState(SM::AwaitSisterDeath); throw; } (context.getState()).Entry(context); return; } void SM_Alive::HeartbeatTimeout(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_Alive::SisterAlive(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Heartbeat(); context.setState(SM::Alive); } catch (...) { context.setState(SM::Alive); throw; } (context.getState()).Entry(context); return; } void SM_Alive::SisterDead(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.SisterDied(); ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_AwaitSisterDeath::Die(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::AwaitSisterDeath); (context.getState()).Entry(context); return; } void SM_AwaitSisterDeath::DisconnectTimeout(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_AwaitSisterDeath::HeartbeatTimeout(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::AwaitSisterDeath); (context.getState()).Entry(context); return; } void SM_AwaitSisterDeath::SisterAlive(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::AwaitSisterDeath); (context.getState()).Entry(context); return; } void SM_AwaitSisterDeath::SisterDead(BondSMContext& context) { BondSM& ctxt(context.getOwner()); (context.getState()).Exit(context); context.clearState(); try { ctxt.Death(); context.setState(SM::Dead); } catch (...) { context.setState(SM::Dead); throw; } (context.getState()).Entry(context); return; } void SM_Dead::ConnectTimeout(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::Dead); (context.getState()).Entry(context); return; } void SM_Dead::Die(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::Dead); (context.getState()).Entry(context); return; } void SM_Dead::DisconnectTimeout(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::Dead); (context.getState()).Entry(context); return; } void SM_Dead::HeartbeatTimeout(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::Dead); (context.getState()).Entry(context); return; } void SM_Dead::SisterAlive(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::Dead); (context.getState()).Entry(context); return; } void SM_Dead::SisterDead(BondSMContext& context) { (context.getState()).Exit(context); context.setState(SM::Dead); (context.getState()).Entry(context); return; } /* * Local variables: * buffer-read-only: t * End: */ bond_core-1.8.6/bondcpp/src/bond.cpp000066400000000000000000000256221372223053100173270ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ // Author: Stuart Glaser #include #include #include #ifdef _WIN32 #include #else #include #endif #include #include #include 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; #else 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, 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)) { 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(); } void Bond::setConnectTimeout(double dur) { if (started_) { ROS_ERROR("Cannot set timeouts after calling start()"); return; } 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; } 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; } 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; } heartbeat_period_ = dur; } void Bond::setCallbackQueue(ros::CallbackQueueInterface *queue) { if (started_) { ROS_ERROR("Cannot set callback queue after calling start()"); return; } 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; } void Bond::setFormedCallback(boost::function 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; } bool Bond::waitUntilFormed(ros::Duration timeout) { 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); 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()); } 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(); } bool Bond::waitUntilBroken(ros::Duration timeout) { 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); 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()); } 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(); } bool Bond::isBroken() { 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); } } flushPendingCallbacks(); } void Bond::onConnectTimeout() { { 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(); } void Bond::onDisconnectTimeout() { { boost::mutex::scoped_lock lock(mutex_); sm_.DisconnectTimeout(); } flushPendingCallbacks(); } 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_) { { 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()); 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 &) { 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); } void Bond::flushPendingCallbacks() { 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](); } } } // 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_); } } void BondSM::SisterDied() { 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_); } } void BondSM::Heartbeat() { 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)); } bond_core-1.8.6/bondcpp/src/timeout.cpp000066400000000000000000000051661372223053100200740ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "bondcpp/timeout.h" #include namespace bond { Timeout::Timeout(const ros::Duration &d, boost::function on_timeout) : duration_(d.sec, d.nsec), on_timeout_(on_timeout) { } Timeout::Timeout(const ros::WallDuration &d, boost::function on_timeout) : duration_(d), on_timeout_(on_timeout) { } Timeout::~Timeout() { timer_.stop(); } void Timeout::setDuration(const ros::Duration &d) { duration_ = ros::WallDuration(d.sec, d.nsec); } void Timeout::setDuration(const ros::WallDuration &d) { duration_ = d; } void Timeout::reset() { timer_.stop(); timer_ = nh_.createSteadyTimer(duration_, &Timeout::timerCallback, this, true); deadline_ = ros::SteadyTime::now() + duration_; } void Timeout::cancel() { timer_.stop(); } ros::WallDuration Timeout::left() { return std::max(ros::WallDuration(0.0), deadline_ - ros::SteadyTime::now()); } void Timeout::timerCallback(const ros::SteadyTimerEvent &) { if (on_timeout_) { on_timeout_(); } } } // namespace bond bond_core-1.8.6/bondpy/000077500000000000000000000000001372223053100147515ustar00rootroot00000000000000bond_core-1.8.6/bondpy/CHANGELOG.rst000066400000000000000000000143631372223053100170010ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package bondpy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.8.6 (2020-08-28) ------------------ * bondpy: Prevent exception in Bond.shutdown() (`#62 `_) * Contributors: Martin Pecka 1.8.5 (2020-05-14) ------------------ * Use setuptools instead of distutils (`#61 `_) * Bump CMake minimum version to use CMP0048 (`#58 `_) * Contributors: Alejandro Hernández Cordero, Michael Carroll 1.8.4 (2020-02-24) ------------------ * Fix import (`#48 `_) * Make Michael Carroll the maintainer (`#40 `_) * Contributors: Markus Grimm, Mikael Arguedas 1.8.3 (2018-08-17) ------------------ 1.8.2 (2018-04-27) ------------------ 1.8.1 (2017-10-27) ------------------ * fix package.xml to comply with schema (`#30 `_) * Contributors: Mikael Arguedas 1.8.0 (2017-07-27) ------------------ * switch to package format 2 (`#27 `_) * Closer to pep8 compliance (`#25 `_) * Python3 compatibility (`#24 `_) * Fixes problem with bondpy not exiting with the node (`#21 `_) * Contributors: Aaron Miller, Mikael Arguedas 1.7.19 (2017-03-27) ------------------- 1.7.18 (2016-10-24) ------------------- 1.7.17 (2016-03-15) ------------------- * Queue size for a publisher `#10 `_ Squash the warning. * update maintainer * Made code a bit more readable `#12 `_ * made local timer a member so we can cancel it during shutdown `#11 `_ * Contributors: Daniel Stonier, Esteve Fernandez, Hugo Boyer, Mikael Arguedas 1.7.16 (2014-10-30) ------------------- 1.7.15 (2014-10-28) ------------------- 1.7.14 (2014-05-08) ------------------- * Export architecture_independent flag in package.xml `#4 `_ * bondpy: Add catkin_package call to CMakeLists.txt `#3 `_ If catkin_package is not called, the package.xml is not installed * Update maintainer field * Contributors: Esteve Fernandez, Scott K Logan, Vincent Rabaud 1.7.13 (2013-08-21) ------------------- 1.7.12 (2013-06-06) ------------------- 1.7.11 (2013-03-13) ------------------- 1.7.10 (2013-01-13) ------------------- 1.7.9 (2012-12-27) ------------------ * fix wrong module for Duration * add missing dep on rospy * modified dep type of catkin * Contributors: Dirk Thomas 1.7.8 (2012-12-13) ------------------ * update setup() to use generate_distutils_setup * Contributors: Dirk Thomas 1.7.7 (2012-12-06) ------------------ * Updated url tags in package.xml's `#1 `_ * Contributors: William Woodall 1.7.6 (2012-10-30) ------------------ * fix catkin function order * Contributors: Dirk Thomas 1.7.5 (2012-10-27) ------------------ * clean up package.xml files * fixed python module import * updated setup.py files * Contributors: Dirk Thomas 1.7.4 (2012-10-06) ------------------ 1.7.3 (2012-10-02 00:19) ------------------------ * fix package building issues * Contributors: Vincent Rabaud 1.7.2 (2012-10-02 00:06) ------------------------ * add the missing catkin dependency * Contributors: Vincent Rabaud 1.7.1 (2012-10-01 19:00) ------------------------ 1.7.0 (2012-10-01 16:51) ------------------------ * catkinize the package and bump to 1.7.0 even though it is not tagged yet * Reverting all changes that were meant to debug test failures on the build farm. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036308 * More testing bond on the build farm: being careful to shutdown bond instances between tests. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036300 * bond tests: debugging around the message publishing. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036262 * Bond tests: debugging around state transition --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036259 * bump. Just making the build farm build again --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036258 * bond tests: typo --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036257 * Bond: debug info about status message. Still tracking down test errors on the build farm --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036256 * More debug info for tracking down test failures in the build farm. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036255 * Modified bond's state machine to handle "alive" messages from the sibling when already dead. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036189 * Added global "bond_disable_heartbeat_timeout" parameter --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036106 * In bond, wait_until_formed and wait_until_broken terminate when ROS shuts down. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035632 * removed wiki syntax from description --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035392 * Creating package descriptions for bondpy, bondcpp, and test_bond. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035354 * The bond state machine more gracefully handles excessive requests to die. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032653 * Moving bond into common --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032634 * Contributors: Vincent Rabaud, kwc, sglaser bond_core-1.8.6/bondpy/CMakeLists.txt000066400000000000000000000001761372223053100175150ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(bondpy) find_package(catkin REQUIRED) catkin_python_setup() catkin_package() bond_core-1.8.6/bondpy/mainpage.dox000066400000000000000000000011101372223053100172370ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b linkpy is ... \section codeapi Code API */ bond_core-1.8.6/bondpy/package.xml000066400000000000000000000022611372223053100170670ustar00rootroot00000000000000 bondpy 1.8.6 Python implementation of bond, a mechanism for checking when another process has terminated. Michael Carroll BSD http://www.ros.org/wiki/bondpy https://github.com/ros/bond_core/issues https://github.com/ros/bond_core Stuart Glaser catkin bond rospy smclib rospy smclib uuid python-setuptools python3-setuptools bond_core-1.8.6/bondpy/python/000077500000000000000000000000001372223053100162725ustar00rootroot00000000000000bond_core-1.8.6/bondpy/python/bondpy/000077500000000000000000000000001372223053100175655ustar00rootroot00000000000000bond_core-1.8.6/bondpy/python/bondpy/BondSM_sm.py000066400000000000000000000127601372223053100217660ustar00rootroot00000000000000# ex: set ro: # DO NOT EDIT. # generated by smc (http://smc.sourceforge.net/) # from file : BondSM_sm.sm from smclib import statemap class BondSMState(statemap.State): def Entry(self, fsm): pass def Exit(self, fsm): pass def ConnectTimeout(self, fsm): self.Default(fsm) def Die(self, fsm): self.Default(fsm) def DisconnectTimeout(self, fsm): self.Default(fsm) def HeartbeatTimeout(self, fsm): self.Default(fsm) def SisterAlive(self, fsm): self.Default(fsm) def SisterDead(self, fsm): self.Default(fsm) def Default(self, fsm): msg = "\n\tState: %s\n\tTransition: %s" % ( fsm.getState().getName(), fsm.getTransition()) raise statemap.TransitionUndefinedException(msg) class SM_Default(BondSMState): pass class SM_WaitingForSister(SM_Default): def ConnectTimeout(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def Die(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Connected() finally: fsm.setState(SM.Alive) fsm.getState().Entry(fsm) def SisterDead(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Connected() ctxt.SisterDied() ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM_Alive(SM_Default): def Die(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.StartDying() finally: fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def HeartbeatTimeout(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Heartbeat() finally: fsm.setState(SM.Alive) fsm.getState().Entry(fsm) def SisterDead(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.SisterDied() ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM_AwaitSisterDeath(SM_Default): def Die(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def DisconnectTimeout(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def HeartbeatTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def SisterDead(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM_Dead(SM_Default): def ConnectTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def Die(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def DisconnectTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def HeartbeatTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterDead(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM(object): WaitingForSister = SM_WaitingForSister('SM.WaitingForSister', 0) Alive = SM_Alive('SM.Alive', 1) AwaitSisterDeath = SM_AwaitSisterDeath('SM.AwaitSisterDeath', 2) Dead = SM_Dead('SM.Dead', 3) Default = SM_Default('SM.Default', -1) class BondSM_sm(statemap.FSMContext): def __init__(self, owner): statemap.FSMContext.__init__(self, SM.WaitingForSister) self._owner = owner def __getattr__(self, attrib): def trans_sm(*arglist): self._transition = attrib getattr(self.getState(), attrib)(self, *arglist) self._transition = None return trans_sm def enterStartState(self): self._state.Entry(self) def getOwner(self): return self._owner # Local variables: # buffer-read-only: t # End: bond_core-1.8.6/bondpy/python/bondpy/__init__.py000066400000000000000000000000001372223053100216640ustar00rootroot00000000000000bond_core-1.8.6/bondpy/python/bondpy/bondpy.py000066400000000000000000000323661372223053100214440ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. ## \author Stuart Glaser import threading import time import uuid import rospy from . import BondSM_sm from bond.msg import Constants, Status def as_ros_duration(d): if not isinstance(d, rospy.Duration): return rospy.Duration.from_sec(d) return d def as_float_duration(d): if isinstance(d, rospy.Duration): return d.to_sec() return d ## Internal use only class Timeout: def __init__(self, duration, on_timeout=None): self.duration = duration self.timer = threading.Timer(0, self._on_timer) self.timer.daemon = True self.deadline = time.time() self.on_timeout = on_timeout def reset(self): self.timer.cancel() self.timer = threading.Timer(self.duration.to_sec(), self._on_timer) self.timer.daemon = True self.timer.start() self.deadline = time.time() + self.duration.to_sec() return self def cancel(self): self.timer.cancel() def left(self): return max(rospy.Duration(0), rospy.Duration(self.deadline - time.time())) def _on_timer(self): if self.on_timeout: self.on_timeout() ## \brief Forms a bond to monitor another process. # # The Bond class implements a bond, allowing you to monitor another # process and be notified when it dies. In turn, it will be notified # when you die. class Bond(object): ## \brief Constructs a bond, but does not connect. # # \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 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. def __init__(self, topic, id, on_broken=None, on_formed=None): self.__started = False self.topic = topic self.id = id self.instance_id = str(uuid.uuid4()) self.sister_instance_id = None self.on_broken = on_broken self.on_formed = on_formed self.is_shutdown = False self.sister_died_first = False # Timeout for wait_until_formed self.deadline = None # Callbacks must be called outside of the locks and after the # state machine finishes transitioning. self.pending_callbacks = [] self.sm = BondSM_sm.BondSM_sm(self) # self.sm.setDebugFlag(True) self.lock = threading.RLock() self.condition = threading.Condition(self.lock) # Sets the default timeout values self.connect_timeout = Constants.DEFAULT_CONNECT_TIMEOUT self.heartbeat_timeout = Constants.DEFAULT_HEARTBEAT_TIMEOUT self.disconnect_timeout = Constants.DEFAULT_DISCONNECT_TIMEOUT self.heartbeat_period = Constants.DEFAULT_HEARTBEAT_PERIOD self.sub = None # queue_size 1 : avoid having a client receive backed up, potentially # late heartbearts, discussion@https://github.com/ros/bond_core/pull/10 self.pub = rospy.Publisher(self.topic, Status, queue_size=1) def get_connect_timeout(self): return self.__connect_timeout def set_connect_timeout(self, dur): assert not self.__started self.__connect_timeout = dur self.connect_timer = Timeout(as_ros_duration(dur), self._on_connect_timeout) connect_timeout = property(get_connect_timeout, set_connect_timeout) def get_heartbeat_timeout(self): return self.__heartbeat_timeout def set_heartbeat_timeout(self, dur): assert not self.__started self.__heartbeat_timeout = dur self.heartbeat_timer = Timeout(as_ros_duration(dur), self._on_heartbeat_timeout) heartbeat_timeout = property(get_heartbeat_timeout, set_heartbeat_timeout) def get_disconnect_timeout(self): return self.__disconnect_timeout def set_disconnect_timeout(self, dur): assert not self.__started self.__disconnect_timeout = dur self.disconnect_timer = Timeout(as_ros_duration(dur), self._on_disconnect_timeout) disconnect_timeout = property(get_disconnect_timeout, set_disconnect_timeout) def get_heartbeat_period(self): return self.__heartbeat_period def set_heartbeat_period(self, per): assert not self.__started self.__heartbeat_period = as_float_duration(per) heartbeat_period = property(get_heartbeat_period, set_heartbeat_period) ## \brief Starts the bond and connects to the sister process. def start(self): with self.lock: self.connect_timer.reset() self.sub = rospy.Subscriber(self.topic, Status, self._on_bond_status) self.thread = threading.Thread(target=self._publishing_thread) self.thread.daemon = True self.thread.start() self.__started = True def _on_connect_timeout(self): with self.lock: self.sm.ConnectTimeout() self._flush_pending_callbacks() def _on_heartbeat_timeout(self): # Checks that heartbeat timeouts haven't been disabled globally disable_heartbeat_timeout = rospy.get_param( Constants.DISABLE_HEARTBEAT_TIMEOUT_PARAM, False) if disable_heartbeat_timeout: rospy.logwarn( "Heartbeat timeout is disabled. Not breaking bond (topic: %s, id: %s)" % (self.topic, self.id)) return with self.lock: self.sm.HeartbeatTimeout() self._flush_pending_callbacks() def _on_disconnect_timeout(self): with self.lock: self.sm.DisconnectTimeout() self._flush_pending_callbacks() def __del__(self): self.shutdown() def shutdown(self): if not self.is_shutdown: if self.sub is not None: self.sub.unregister() with self.lock: self.is_shutdown = True if self.sm.getState().getName() != 'SM.Dead': self.break_bond() self.pub.unregister() self.condition.notify_all() self.connect_timer.cancel() if self.deadline: self.deadline.cancel() def _on_bond_status(self, msg): # Filters out messages from other bonds and messages from ourself if msg.id == self.id and msg.instance_id != self.instance_id: with self.lock: if not self.sister_instance_id: self.sister_instance_id = msg.instance_id if msg.instance_id != self.sister_instance_id: rospy.logerr( "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." % (self.topic, self.id)) return if msg.active: self.sm.SisterAlive() else: self.sm.SisterDead() # Immediate ack for sister's death notification if self.sister_died_first: self._publish(False) self._flush_pending_callbacks() def _publish(self, active): msg = Status() msg.header.stamp = rospy.Time.now() msg.id = self.id msg.instance_id = self.instance_id msg.active = active msg.heartbeat_timeout = self.heartbeat_timeout msg.heartbeat_period = self.heartbeat_period self.pub.publish(msg) def _publishing_thread(self): with self.lock: # Publishing ALIVE while not self.is_shutdown and self.sm.getState().getName() in ['SM.WaitingForSister', 'SM.Alive']: self._publish(True) self.condition.wait(self.heartbeat_period) # Publishing DEAD while not self.is_shutdown and self.sm.getState().getName() == 'SM.AwaitSisterDeath': self._publish(False) self.condition.wait(Constants.DEAD_PUBLISH_PERIOD) def _flush_pending_callbacks(self): callbacks = [] with self.lock: callbacks = self.pending_callbacks self.pending_callbacks = [] for c in callbacks: c() ## \brief INTERNAL def Connected(self): self.connect_timer.cancel() self.condition.notify_all() if self.on_formed: self.pending_callbacks.append(self.on_formed) ## \brief INTERNAL def Heartbeat(self): self.heartbeat_timer.reset() ## \brief INTERNAL def SisterDied(self): self.sister_died_first = True ## \brief INTERNAL def Death(self): self.condition.notify_all() self.heartbeat_timer.cancel() self.disconnect_timer.cancel() if self.on_broken: self.pending_callbacks.append(self.on_broken) ## \brief INTERNAL def StartDying(self): self.heartbeat_timer.cancel() self.disconnect_timer.reset() ## \brief Sets the formed callback def set_formed_callback(self, on_formed): with self.lock: self.on_formed = on_formed ## \brief Sets the broken callback def set_broken_callback(self, on_broken): with self.lock: self.on_broken = on_broken ## \brief Blocks until the bond is formed for at most 'duration'. # # \param timeout Maximum duration to wait. If None then this call will not timeout. # \return true iff the bond has been formed. def wait_until_formed(self, timeout=None): if self.deadline: self.deadline.cancel() self.deadline = None if timeout: self.deadline = Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() == 'SM.WaitingForSister': if rospy.is_shutdown(): break if self.deadline and self.deadline.left() == rospy.Duration(0): break wait_duration = 0.1 if self.deadline: wait_duration = min(wait_duration, self.deadline.left().to_sec()) self.condition.wait(wait_duration) return self.sm.getState().getName() != 'SM.WaitingForSister' ## \brief Blocks until the bond is broken for at most 'duration'. # # \param timeout Maximum duration to wait. If None then this call will not timeout. # \return true iff the bond has been broken, even if it has never been formed. def wait_until_broken(self, timeout=None): if self.deadline: self.deadline.cancel() self.deadline = None if timeout: self.deadline = Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() != 'SM.Dead': if rospy.is_shutdown(): break if self.deadline and self.deadline.left() == rospy.Duration(0): break wait_duration = 0.1 if self.deadline: wait_duration = min(wait_duration, self.deadline.left().to_sec()) self.condition.wait(wait_duration) return self.sm.getState().getName() == 'SM.Dead' ## \brief Indicates if the bond is broken # \return true iff the bond has been broken. def is_broken(self): with self.lock: return self.sm.getState().getName() == 'SM.Dead' ## \brief Breaks the bond, notifying the other process. def break_bond(self): with self.lock: self.sm.Die() self._publish(False) self._flush_pending_callbacks() def __repr__(self): return "[Bond %s, Instance %s (%s)]" % \ (self.id, self.instance_id, self.sm.getState().getName()) bond_core-1.8.6/bondpy/setup.py000066400000000000000000000003271372223053100164650ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['bondpy'], package_dir={'': 'python'} ) setup(**d) bond_core-1.8.6/smclib/000077500000000000000000000000001372223053100147275ustar00rootroot00000000000000bond_core-1.8.6/smclib/CHANGELOG.rst000066400000000000000000000062601372223053100167540ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package smclib ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.8.6 (2020-08-28) ------------------ 1.8.5 (2020-05-14) ------------------ * Use setuptools instead of distutils (`#61 `_) * Bump CMake minimum version to use CMP0048 (`#58 `_) * Contributors: Alejandro Hernández Cordero, Michael Carroll 1.8.4 (2020-02-24) ------------------ * remove windows.h to avoid namespace pollutions. (`#46 `_) * Make Michael Carroll the maintainer (`#40 `_) * Contributors: James Xu, Mikael Arguedas 1.8.3 (2018-08-17) ------------------ 1.8.2 (2018-04-27) ------------------ 1.8.1 (2017-10-27) ------------------ * fix package.xml to comply with schema (`#30 `_) * Contributors: Mikael Arguedas 1.8.0 (2017-07-27) ------------------ * C++ style (`#28 `_) * switch to package format 2 (`#27 `_) * remove trailing whitespaces (`#26 `_) * Closer to pep8 compliance (`#25 `_) * Contributors: Mikael Arguedas 1.7.19 (2017-03-27) ------------------- 1.7.18 (2016-10-24) ------------------- 1.7.17 (2016-03-15) ------------------- * update maintainer * Contributors: Mikael Arguedas 1.7.16 (2014-10-30) ------------------- 1.7.15 (2014-10-28) ------------------- 1.7.14 (2014-05-08) ------------------- * Export architecture_independent flag in package.xml `#4 `_ * Update maintainer field * Contributors: Esteve Fernandez, Scott K Logan, Vincent Rabaud 1.7.13 (2013-08-21) ------------------- 1.7.12 (2013-06-06) ------------------- 1.7.11 (2013-03-13) ------------------- 1.7.10 (2013-01-13) ------------------- 1.7.9 (2012-12-27) ------------------ * modified dep type of catkin * Contributors: Dirk Thomas 1.7.8 (2012-12-13) ------------------ * update setup() to use generate_distutils_setup * Contributors: Dirk Thomas 1.7.7 (2012-12-06) ------------------ * Updated url tags in package.xml's `#1 `_ * removed executable bit from header file * Contributors: Dirk Thomas, William Woodall 1.7.6 (2012-10-30) ------------------ * fix catkin function order * Contributors: Dirk Thomas 1.7.5 (2012-10-27) ------------------ * clean up package.xml files * updated setup.py files * Contributors: Dirk Thomas 1.7.4 (2012-10-06) ------------------ 1.7.3 (2012-10-02 00:19) ------------------------ 1.7.2 (2012-10-02 00:06) ------------------------ * add the missing catkin dependency * Contributors: Vincent Rabaud 1.7.1 (2012-10-01 19:00) ------------------------ 1.7.0 (2012-10-01 16:51) ------------------------ * catkinize bond * Downloading smclib from code.ros.org --HG-- extra : rebase_source : d17dff0350b44646e6a952fc2cc883156ba9f4a4 * Moving bond into common --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032634 * Contributors: Stuart Glaser, Vincent Rabaud, sglaser bond_core-1.8.6/smclib/CMakeLists.txt000066400000000000000000000004261372223053100174710ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(smclib) find_package(catkin REQUIRED) catkin_package(INCLUDE_DIRS include) catkin_python_setup() # install the include folder install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) bond_core-1.8.6/smclib/include/000077500000000000000000000000001372223053100163525ustar00rootroot00000000000000bond_core-1.8.6/smclib/include/smclib/000077500000000000000000000000001372223053100176235ustar00rootroot00000000000000bond_core-1.8.6/smclib/include/smclib/statemap.h000066400000000000000000000514721372223053100216230ustar00rootroot00000000000000// // The contents of this file are subject to the Mozilla Public // License Version 1.1 (the "License"); you may not use this file // except in compliance with the License. You may obtain a copy // of the License at http://www.mozilla.org/MPL/ // // Software distributed under the License is distributed on an // "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or // implied. See the License for the specific language governing // rights and limitations under the License. // // The Original Code is State Machine Compiler (SMC). // // The Initial Developer of the Original Code is Charles W. Rapp. // Portions created by Charles W. Rapp are // Copyright (C) 2000 - 2007. Charles W. Rapp. // All Rights Reserved. // // Contributor(s): // // Namespace // statemap // // Description // This namespace contains the finite state machine context // class. The user can derive FSM contexts from this class and // interface to them with the methods of this class. // // Notes // The finite state machine needs to be initialized to the // starting state of the FSM. This must be done manually in // the constructor of the derived class. // // Author // C. W. Rapp // // RCS ID // $Id: statemap.h,v 1.15 2009/11/24 20:42:39 cwrapp Exp $ // // CHANGE LOG // (See bottom of file) // #ifndef SMCLIB__STATEMAP_H_ #define SMCLIB__STATEMAP_H_ #if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) #include #if defined(SMC_NO_EXCEPTIONS) #include #endif // SMC_NO_EXCEPTIONS #include #elif defined(WIN32) #include #if defined(SMC_NO_EXCEPTIONS) #include #endif // SMC_NO_EXCEPTIONS #else #include #if defined(SMC_NO_EXCEPTIONS) #include #endif // SMC_NO_EXCEPTIONS #include #endif #if !defined(SMC_NO_EXCEPTIONS) #include #include #endif #include // Limit names to 100 ASCII characters. // Why 100? Because it is a round number. #define MAX_NAME_LEN 100 namespace statemap { //--------------------------------------------------------------- // Routines. // inline char* copyString(const char *s) { char *retval = NULL; if (s != NULL) { retval = new char[MAX_NAME_LEN + 1]; retval[MAX_NAME_LEN] = '\0'; (void) std::strncpy(retval, s, MAX_NAME_LEN); } return (retval); } //--------------------------------------------------------------- // Exception Classes. // #ifndef SMC_NO_EXCEPTIONS // Base class for all SMC exceptions. class SmcException : public std::runtime_error { //----------------------------------------------------------- // Member methods // public: // Destructor. virtual ~SmcException() throw() {}; protected: // Constructor. SmcException(const std::string& reason) : std::runtime_error(reason) {}; private: // Default construction not allowed. SmcException(); //----------------------------------------------------------- // Member data. // public: protected: private: }; // This class is thrown when a pop is issued on an empty // state stack. class PopOnEmptyStateStackException : public SmcException { //----------------------------------------------------------- // Member methods. // public: // Default constructor. PopOnEmptyStateStackException() : SmcException("no state to pop from state stack") {}; // Destructor. virtual ~PopOnEmptyStateStackException() throw() {}; protected: private: //----------------------------------------------------------- // Member data. // public: protected: private: }; // This class is thrown when a transition is issued // but there is no current state. This happens when // a transition is issued from within a transition // action. class StateUndefinedException : public SmcException { //----------------------------------------------------------- // Member methods. // public: // Default constructor. StateUndefinedException() : SmcException("transition invoked while in transition") {}; // Destructor. virtual ~StateUndefinedException() throw() {}; protected: private: //----------------------------------------------------------- // Member data. // public: protected: private: }; // This class is thrown when a transition is issued // but there is no code to handle it. class TransitionUndefinedException : public SmcException { //----------------------------------------------------------- // Member methods. // public: // Default constructor. TransitionUndefinedException() : SmcException("no such transition in current state"), _state(NULL), _transition(NULL) {}; // Construct an exception using the specified state // and transition. TransitionUndefinedException(const char *state, const char *transition) : SmcException("no such transition in current state"), _state(copyString(state)), _transition(copyString(transition)) {}; // Copy constructor. TransitionUndefinedException( const TransitionUndefinedException& ex) : SmcException("no such transition in current state"), _state(copyString(ex._state)), _transition(copyString(ex._transition)) {}; // Destructor. virtual ~TransitionUndefinedException() throw() { if (_state != NULL) { delete[] _state; _state = NULL; } if (_transition != NULL) { delete[] _transition; _transition = NULL; } }; // Assignment operator. const TransitionUndefinedException& operator=(const TransitionUndefinedException& ex) { // Don't do self assignment. if (this != &ex) { if (_state != NULL) { delete[] _state; _state = NULL; } if (_transition != NULL) { delete[] _transition; _transition = NULL; } _state = copyString(ex._state); _transition = copyString(ex._transition); } return (*this); }; // Returns the state. May be NULL. const char* getState() const { return(_state); }; // Returns the transition. May be NULL. const char* getTransition() const { return (_transition); }; protected: private: //----------------------------------------------------------- // Member data. // public: protected: private: char *_state; char *_transition; }; // This class is thrown when a state ID is either less than // the minimal value or greater than the maximal value. class IndexOutOfBoundsException : public SmcException { //----------------------------------------------------------- // Member methods. // public: // Default constructor. IndexOutOfBoundsException() : SmcException("index out of bounds"), _index(0), _minIndex(0), _maxIndex(0) {}; // Constructs an exception using the specified index, // minimum index and maximum index. IndexOutOfBoundsException(const int index, const int minIndex, const int maxIndex) : SmcException("index out of bounds"), _index(index), _minIndex(minIndex), _maxIndex(maxIndex) {}; // Copy constructor. IndexOutOfBoundsException( const IndexOutOfBoundsException& ex) : SmcException("index out of bounds"), _index(ex._index), _minIndex(ex._minIndex), _maxIndex(ex._maxIndex) {}; // Destructor. virtual ~IndexOutOfBoundsException() throw() {}; // Assignment operator. const IndexOutOfBoundsException& operator=(const IndexOutOfBoundsException& ex) { // Don't do self assignment. if (this != &ex) { _index = ex._index; _minIndex = ex._minIndex; _maxIndex = ex._maxIndex; } return (*this); }; // Returns the out-of-bounds index. int getIndex() const { return(_index); }; // Returns the minimum allowed index value. int getMinIndex() const { return (_minIndex); }; // Returns the maximum allowed index value. int getMaxIndex() const { return (_maxIndex); }; protected: private: //----------------------------------------------------------- // Member data. // public: protected: private: int _index; int _minIndex; int _maxIndex; }; #endif // !SMC_NO_EXCEPTIONS // // end of Exception Classes. //--------------------------------------------------------------- class State { //----------------------------------------------------------- // Member functions. // public: const char* getName() const { return (_name); }; int getId() const { return (_stateId); } protected: State(const char *name, int stateId) : _name(NULL), _stateId(stateId) { if (name != NULL) { _name = copyString(name); } else { _name = copyString("NAME NOT SET"); } }; virtual ~State() { if (_name != NULL) { delete[] _name; _name = NULL; } }; private: // Make the default and copy constructors private to // prevent their use. State() {} State(const State&) {} //----------------------------------------------------------- // Member data. // public: protected: // This state's printable name. char *_name; // This state's unique identifier. int _stateId; private: }; class FSMContext { //----------------------------------------------------------- // Nested classes. // public: protected: private: // Implements the state stack. class StateEntry { //------------------------------------------------------- // Member functions. // public: StateEntry(State *state, StateEntry *next) : _state(state), _next(next) {}; ~StateEntry() { _state = NULL; _next = NULL; }; State* getState() { return(_state); }; StateEntry* getNext() { return(_next); }; protected: private: //------------------------------------------------------- // Member data. // public: protected: private: State *_state; StateEntry *_next; //------------------------------------------------------- // Friends // friend class FSMContext; }; // end of class StateEntry //----------------------------------------------------------- // Member functions // public: // Destructor. virtual ~FSMContext() { StateEntry *state; if (_transition != NULL) { delete[] _transition; _transition = NULL; } // But we did allocate the state stack. while (_state_stack != NULL) { state = _state_stack; _state_stack = _state_stack->_next; delete state; } }; // Comparison and assignment operators // Assignment operator FSMContext& operator=(const FSMContext& fsm) { // Don't do the assignment if the left and right // hand sides are the same object. if (this != &fsm) { _state = fsm._state; } return(*this); }; // Starts the finite state machine running by executing // the initial state's entry actions. virtual void enterStartState() = 0; // Exact same object (is it me?) int same(const FSMContext& fsm) const { return(this == &fsm); }; // Returns the debug flag's current setting. bool getDebugFlag() { return(_debug_flag); }; // Sets the debug flag. A true value means debugging // is on and false means off. void setDebugFlag(bool flag) { _debug_flag = flag; return; }; #ifdef SMC_USES_IOSTREAMS // Returns the stream to which debug output is written. std::ostream& getDebugStream() { return (*_debug_stream); }; // Sets the debug output stream. void setDebugStream(std::ostream& debug_stream) { _debug_stream = &debug_stream; return; } #endif // SMC_USES_IOSTREAMS // Is this state machine already inside a transition? // Yes if state is null. bool isInTransition() const { return(_state == NULL ? true : false); }; // Returns the current transition's name. // Used only for debugging purposes. char* getTransition() { return (_transition); }; // Saves away the transition name only if debugging // is turned on. void setTransition(const char *transition) { if (_transition != NULL) { delete[] _transition; _transition = NULL; } _transition = copyString(transition); return; }; // Clears the current state. void clearState() { _previous_state = _state; _state = NULL; }; // Returns the state which a transition left. // May be NULL. State* getPreviousState() { return (_previous_state); } // Sets the current state to the specified state. void setState(const State& state) { _state = const_cast(&state); if (_debug_flag == true) { #ifdef SMC_USES_IOSTREAMS *_debug_stream << "ENTER STATE : " << _state->getName() << std::endl; #else TRACE("ENTER STATE : %s\n\r", _state->getName()); #endif // SMC_USES_IOSTREAMS } }; // Returns true if the state stack is empty and false // otherwise. bool isStateStackEmpty() const { return (_state_stack == NULL); } // Returns the state stack's depth. int getStateStackDepth() const { StateEntry *state_ptr; int retval; for (state_ptr = _state_stack, retval = 0; state_ptr != NULL; state_ptr = state_ptr->getNext(), ++retval) ; return (retval); } // Push the current state on top of the state stack // and make the specified state the current state. void pushState(const State& state) { StateEntry *new_entry; // Do the push only if there is a state to be pushed // on the stack. if (_state != NULL) { new_entry = new StateEntry(_state, _state_stack); _state_stack = new_entry; } _state = const_cast(&state); if (_debug_flag == true) { #ifdef SMC_USES_IOSTREAMS *_debug_stream << "PUSH TO STATE : " << _state->getName() << std::endl; #else TRACE("PUSH TO STATE : %s\n\r", _state->getName()); #endif // SMC_USES_IOSTREAMS } }; // Make the state on top of the state stack the // current state. void popState() { StateEntry *entry; // Popping when there was no previous push is an error. #ifdef SMC_NO_EXCEPTIONS assert(_state_stack != NULL); #else if (_state_stack == NULL) { throw PopOnEmptyStateStackException(); } #endif // SMC_NO_EXCEPTIONS _state = _state_stack->getState(); entry = _state_stack; _state_stack = _state_stack->getNext(); delete entry; if (_debug_flag == true) { #ifdef SMC_USES_IOSTREAMS *_debug_stream << "POP TO STATE : " << _state->getName() << std::endl; #else TRACE("POP TO STATE : %s\n\r", _state->getName()); #endif // SMC_USES_IOSTREAMS } }; // Remove all states from the state stack. void emptyStateStack() { StateEntry *state_ptr, *next_ptr; for (state_ptr = _state_stack; state_ptr != NULL; state_ptr = next_ptr) { next_ptr = state_ptr->getNext(); delete state_ptr; } _state_stack = NULL; }; protected: // Default constructor. FSMContext(const State& state) : _state(const_cast(&state)), _previous_state(NULL), _state_stack(NULL), _transition(NULL), #ifdef SMC_USES_IOSTREAMS _debug_flag(false), _debug_stream(&std::cerr) #else _debug_flag(false) #endif // SMC_USES_IOSTREAMS {}; private: // I don't believe that it makes sense to copy a // context. It may make sense to copy the application // class but the new object is *not* in the same // state as the old - the new object must start in // the FSM's initial state. Therefore, the copy // constructor is private in order to prevent it // being used. FSMContext(const FSMContext&) {}; //----------------------------------------------------------- // Member data // public: protected: // The current state of the finite state machine. State *_state; // Remember which state a transition left. State *_previous_state; // The stack of pushed states. StateEntry *_state_stack; // The current transition *name*. Use for debugging // purposes. char *_transition; private: // When this flag is set to true, this class will print // out debug messages. bool _debug_flag; // Include the following only if C++ iostreams are being used. #ifdef SMC_USES_IOSTREAMS // When FSM debugging is on, debug messages will be // written to this output stream. This stream is set to // standard error by default. std::ostream *_debug_stream; #endif // SMC_USES_IOSTREAMS }; // end of class FSMContext } // namespace statemap // // CHANGE LOG // $Log: statemap.h,v $ // Revision 1.15 2009/11/24 20:42:39 cwrapp // v. 6.0.1 update // // Revision 1.14 2009/03/01 18:20:40 cwrapp // Preliminary v. 6.0.0 commit. // // Revision 1.13 2008/05/20 18:31:12 cwrapp // ---------------------------------------------------------------------- // // Committing release 5.1.0. // // Modified Files: // Makefile README.txt smc.mk tar_list.txt bin/Smc.jar // examples/Ant/EX1/build.xml examples/Ant/EX2/build.xml // examples/Ant/EX3/build.xml examples/Ant/EX4/build.xml // examples/Ant/EX5/build.xml examples/Ant/EX6/build.xml // examples/Ant/EX7/build.xml examples/Ant/EX7/src/Telephone.java // examples/Java/EX1/Makefile examples/Java/EX4/Makefile // examples/Java/EX5/Makefile examples/Java/EX6/Makefile // examples/Java/EX7/Makefile examples/Ruby/EX1/Makefile // lib/statemap.jar lib/C++/statemap.h lib/Java/Makefile // lib/Php/statemap.php lib/Scala/Makefile // lib/Scala/statemap.scala net/sf/smc/CODE_README.txt // net/sf/smc/README.txt net/sf/smc/Smc.java // ---------------------------------------------------------------------- // // Revision 1.12 2007/12/28 12:34:40 cwrapp // Version 5.0.1 check-in. // // Revision 1.11 2007/08/05 12:58:54 cwrapp // Version 5.0.1 check-in. See net/sf/smc/CODE_README.txt for more information. // // Revision 1.10 2007/01/15 00:23:50 cwrapp // Release 4.4.0 initial commit. // // Revision 1.9 2006/07/11 18:28:22 cwrapp // Move SmcException::copyString() to a package-wide routine. // // Revision 1.8 2006/04/22 12:45:24 cwrapp // Version 4.3.1 // // Revision 1.7 2005/06/08 11:09:14 cwrapp // + Updated Python code generator to place "pass" in methods with empty // bodies. // + Corrected FSM errors in Python example 7. // + Removed unnecessary includes from C++ examples. // + Corrected errors in top-level makefile's distribution build. // // Revision 1.6 2005/05/28 18:44:13 cwrapp // Updated C++, Java and Tcl libraries, added CSharp, Python and VB. // // Revision 1.2 2005/02/21 19:01:42 charlesr // Changed State::_id to State::_stateId because of Object-C++ // reserved word conflict. // // Revision 1.1 2004/05/31 13:44:41 charlesr // Added support for non-iostreams output. // // Revision 1.0 2003/12/14 20:37:49 charlesr // Initial revision #endif // SMCLIB__STATEMAP_H_ bond_core-1.8.6/smclib/mainpage.dox000066400000000000000000000011101372223053100172150ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b smclib is ... \section codeapi Code API */ bond_core-1.8.6/smclib/package.xml000066400000000000000000000023141372223053100170440ustar00rootroot00000000000000 smclib 1.8.6 The State Machine Compiler (SMC) from http://smc.sourceforge.net/ converts a language-independent description of a state machine into the source code to support that state machine. This package contains the libraries that a compiled state machine depends on, but it does not contain the compiler itself. Michael Carroll Mozilla Public License Version 1.1 http://smc.sourceforge.net/ https://github.com/ros/bond_core/issues https://github.com/ros/bond_core Various catkin python-setuptools python3-setuptools bond_core-1.8.6/smclib/python/000077500000000000000000000000001372223053100162505ustar00rootroot00000000000000bond_core-1.8.6/smclib/python/smclib/000077500000000000000000000000001372223053100175215ustar00rootroot00000000000000bond_core-1.8.6/smclib/python/smclib/__init__.py000066400000000000000000000000001372223053100216200ustar00rootroot00000000000000bond_core-1.8.6/smclib/python/smclib/statemap.py000066400000000000000000000127111372223053100217130ustar00rootroot00000000000000# # The contents of this file are subject to the Mozilla Public # License Version 1.1 (the "License"); you may not use this file # except in compliance with the License. You may obtain a copy of # the License at http://www.mozilla.org/MPL/ # # Software distributed under the License is distributed on an "AS # IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or # implied. See the License for the specific language governing # rights and limitations under the License. # # The Original Code is State Machine Compiler (SMC). # # The Initial Developer of the Original Code is Charles W. Rapp. # Portions created by Charles W. Rapp are # Copyright (C) 2005. Charles W. Rapp. # All Rights Reserved. # # Port to Python by Francois Perrad, francois.perrad@gadz.org # Copyright 2004, Francois Perrad. # All Rights Reserved. # # Contributor(s): # # RCS ID # $Id: statemap.py,v 1.7 2009/11/24 20:42:39 cwrapp Exp $ # # See: http://smc.sourceforge.net/ # import sys class StateUndefinedException(Exception): """A StateUndefinedException is thrown by an SMC-generated state machine whenever a transition is taken and there is no state currently set. This occurs when a transition is issued from within a transition action.""" pass class TransitionUndefinedException(Exception): """A TransitionUndefinedException is thrown by an SMC-generated state machine whenever a transition is taken which: - Is not explicitly defined in the current state. - Is not explicitly defined in the current FSM's default state. - There is no Default transition in the current state.""" pass class State(object): """base State class""" def __init__(self, name, id): self._name = name self._id = id def getName(self): """Returns the state's printable name.""" return self._name def getId(self): """Returns the state's unique identifier.""" return self._id class FSMContext(object): """The user can derive FSM contexts from this class and interface to them with the methods of this class. The finite state machine needs to be initialized to the starting state of the FSM. This must be done manually in the constructor of the derived class. """ def __init__(self, state): self._state = state self._previous_state = None self._state_stack = [] self._transition = None self._debug_flag = False self._debug_stream = sys.stderr def getDebugFlag(self): """Returns the debug flag's current setting.""" return self._debug_flag def setDebugFlag(self, flag): """Sets the debug flag. A true value means debugging is on and false means off.""" self._debug_flag = flag def getDebugStream(self): """Returns the stream to which debug output is written.""" return self._debug_stream def setDebugStream(self, stream): """Sets the debug output stream.""" self._debug_stream = stream def getState(self): """Returns the current state.""" if self._state is None: raise StateUndefinedException return self._state def isInTransition(self): """Is this state machine already inside a transition? True if state is undefined.""" if self._state is None: return True else: return False def getTransition(self): """Returns the current transition's name. Used only for debugging purposes.""" return self._transition def clearState(self): """Clears the current state.""" self._previous_state = self._state self._state = None def getPreviousState(self): """Returns the state which a transition left. May be None""" return self._previous_state def setState(self, state): """Sets the current state to the specified state.""" if not isinstance(state, State): raise ValueError("state should be a statemap.State") self._state = state if self._debug_flag: self._debug_stream.write("NEW STATE : %s\n" % self._state.getName()) def isStateStackEmpty(self): """Returns True if the state stack is empty and False otherwise.""" return len(self._state_stack) == 0 def getStateStackDepth(self): """Returns the state stack's depth.""" return len(self._state_stack) def pushState(self, state): """Push the current state on top of the state stack and make the specified state the current state.""" if not isinstance(state, State): raise ValueError("state should be a statemap.State") if self._state is not None: self._state_stack.append(self._state) self._state = state if self._debug_flag: self._debug_stream.write("PUSH TO STATE : %s\n" % self._state.getName()) def popState(self): """Make the state on top of the state stack the current state.""" if len(self._state_stack) == 0: if self._debug_flag: self._debug_stream.write("POPPING ON EMPTY STATE STACK.\n") raise ValueError("empty state stack") else: self._state = self._state_stack.pop() if self._debug_flag: self._debug_stream.write("POP TO STATE : %s\n" % self._state.getName()) def emptyStateStack(self): """Remove all states from the state stack.""" self._state_stack = [] bond_core-1.8.6/smclib/setup.py000066400000000000000000000003271372223053100164430ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['smclib'], package_dir={'': 'python'} ) setup(**d) bond_core-1.8.6/test_bond/000077500000000000000000000000001372223053100154375ustar00rootroot00000000000000bond_core-1.8.6/test_bond/CHANGELOG.rst000066400000000000000000000144111372223053100174610ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package test_bond ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.8.6 (2020-08-28) ------------------ * bondpy: Prevent exception in Bond.shutdown() (`#62 `_) * Contributors: Martin Pecka 1.8.5 (2020-05-14) ------------------ * Bump CMake minimum version to use CMP0048 (`#58 `_) * Contributors: Michael Carroll 1.8.4 (2020-02-24) ------------------ * Make Michael Carroll the maintainer (`#40 `_) * Contributors: Mikael Arguedas 1.8.3 (2018-08-17) ------------------ * Fix build issue on Windows (`#38 `_) * Fix build issue on Windows * indent for better readability * Contributors: Johnson Shih 1.8.2 (2018-04-27) ------------------ * uuid dependency fixup (`#36 `_) * dont export uuid dependency as this isnt anywhere in the public api * fixx uuid dependency in test_bond as well * Contributors: Mikael Arguedas 1.8.1 (2017-10-27) ------------------ * fix package.xml to comply with schema (`#30 `_) * Contributors: Mikael Arguedas 1.8.0 (2017-07-27) ------------------ * C++ style (`#28 `_) * switch to package format 2 (`#27 `_) * Closer to pep8 compliance (`#25 `_) * Python3 compatibility (`#24 `_) * Contributors: Mikael Arguedas 1.7.19 (2017-03-27) ------------------- 1.7.18 (2016-10-24) ------------------- * fix -isystem /usr/include build breakage in gcc6 * Contributors: Mikael Arguedas 1.7.17 (2016-03-15) ------------------- * update maintainer * Made code a bit more readable `#12 `_ * Contributors: Esteve Fernandez, Mikael Arguedas 1.7.16 (2014-10-30) ------------------- 1.7.15 (2014-10-28) ------------------- 1.7.14 (2014-05-08) ------------------- * Update maintainer field * Contributors: Esteve Fernandez, Vincent Rabaud 1.7.13 (2013-08-21) ------------------- * update check for CATKIN_ENABLE_TESTING to work with isolated built * check for CATKIN_ENABLE_TESTING * Contributors: Dirk Thomas 1.7.12 (2013-06-06) ------------------- * fix dependency on exported targets if the variable is empty for the test package * use EXPORTED_TARGETS variable instead of explicit target names * Contributors: Dirk Thomas 1.7.11 (2013-03-13) ------------------- 1.7.10 (2013-01-13) ------------------- 1.7.9 (2012-12-27) ------------------ * fix wrong import of bondpy * increase timeout for test * modified dep type of catkin * Contributors: Dirk Thomas 1.7.8 (2012-12-13) ------------------ 1.7.7 (2012-12-06) ------------------ * Updated url tags in package.xml's `#1 `_ * Contributors: William Woodall 1.7.6 (2012-10-30) ------------------ * fix catkin function order * Contributors: Dirk Thomas 1.7.5 (2012-10-27) ------------------ * clean up package.xml files * fixed dep to rostest * fixed python module import * fixed test registration in cmake * fixed compiling tests * Contributors: Dirk Thomas 1.7.4 (2012-10-06) ------------------ 1.7.3 (2012-10-02 00:19) ------------------------ 1.7.2 (2012-10-02 00:06) ------------------------ * add the missing catkin dependency * Contributors: Vincent Rabaud 1.7.1 (2012-10-01 19:00) ------------------------ 1.7.0 (2012-10-01 16:51) ------------------------ * catkinize the package and bump to 1.7.0 even though it is not tagged yet * bondpy tests now cleanly shutdown any bonds that they create. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036309 * Reverting all changes that were meant to debug test failures on the build farm. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036308 * More testing bond on the build farm: being careful to shutdown bond instances between tests. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036300 * Bond: debug info about status message. Still tracking down test errors on the build farm --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036256 * More debug info for tracking down test failures in the build farm. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036255 * Changed exercise_bond.py to print more information on failure in order to debug test failures that only occur on the build machines. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036253 * Bond tester was spewing warning messages even when successful --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036126 * Added global "bond_disable_heartbeat_timeout" parameter --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4036106 * Creating package descriptions for bondpy, bondcpp, and test_bond. --HG-- extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4035354 * Renamed bondtest to test_bond (`#4435 `_) --HG-- rename : bondtest/CMakeLists.txt => test_bond/CMakeLists.txt rename : bondtest/Makefile => test_bond/Makefile rename : bondtest/mainpage.dox => test_bond/mainpage.dox rename : bondtest/manifest.xml => test_bond/manifest.xml rename : bondtest/scripts/BondSM_sm.py => test_bond/scripts/BondSM_sm.py rename : bondtest/scripts/tester.py => test_bond/scripts/tester.py rename : bondtest/srv/TestBond.srv => test_bond/srv/TestBond.srv rename : bondtest/test/exercise_bond.cpp => test_bond/test/exercise_bond.cpp rename : bondtest/test/exercise_bond.py => test_bond/test/exercise_bond.py rename : bondtest/test/test_callbacks_cpp.cpp => test_bond/test/test_callbacks_cpp.cpp rename : bondtest/test/test_callbacks_py.py => test_bond/test/test_callbacks_py.py extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk%4032810 * Contributors: Vincent Rabaud, sglaser bond_core-1.8.6/test_bond/CMakeLists.txt000066400000000000000000000025161372223053100202030ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(test_bond) find_package(catkin REQUIRED COMPONENTS cmake_modules bondcpp message_generation rostest) if(CATKIN_ENABLE_TESTING) include_directories(${GTEST_INCLUDE_DIRS}) include_directories(include ${bondcpp_INCLUDE_DIRS}) # create the service add_service_files(DIRECTORY srv FILES TestBond.srv) generate_messages() endif() catkin_package() if(CATKIN_ENABLE_TESTING) # add more tests find_package(UUID REQUIRED) include_directories(${UUID_INCLUDE_DIRS}) add_executable(exercise_bond EXCLUDE_FROM_ALL test/exercise_bond.cpp) target_link_libraries(exercise_bond ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${UUID_LIBRARIES}) if(test_bond_EXPORTED_TARGETS) add_dependencies(exercise_bond ${test_bond_EXPORTED_TARGETS}) endif() add_dependencies(tests exercise_bond) add_rostest(test/test_cpp.launch) add_rostest(test/test_python.launch) add_executable(test_callbacks_cpp EXCLUDE_FROM_ALL test/test_callbacks_cpp.cpp) target_link_libraries(test_callbacks_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${UUID_LIBRARIES}) if(test_bond_EXPORTED_TARGETS) add_dependencies(test_callbacks_cpp ${test_bond_EXPORTED_TARGETS}) endif() add_dependencies(tests test_callbacks_cpp) add_rostest(test/test_callbacks_cpp.launch) add_rostest(test/test_callbacks_py.launch) endif() bond_core-1.8.6/test_bond/mainpage.dox000066400000000000000000000011141372223053100177310ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b test_bonds is ... \section codeapi Code API */ bond_core-1.8.6/test_bond/package.xml000066400000000000000000000017321372223053100175570ustar00rootroot00000000000000 test_bond 1.8.6 Contains tests for [[bond]], including tests for [[bondpy]] and [[bondcpp]]. Michael Carroll BSD http://www.ros.org/wiki/test_bond https://github.com/ros/bond_core/issues https://github.com/ros/bond_core Stuart Glaser catkin bondcpp cmake_modules message_generation rostest bondpy uuid bond_core-1.8.6/test_bond/scripts/000077500000000000000000000000001372223053100171265ustar00rootroot00000000000000bond_core-1.8.6/test_bond/scripts/BondSM_sm.py000066400000000000000000000116661372223053100213330ustar00rootroot00000000000000# ex: set ro: # DO NOT EDIT. # generated by smc (http://smc.sourceforge.net/) # from file : BondSM_sm.sm from smclib import statemap class BondSMState(statemap.State): def Entry(self, fsm): pass def Exit(self, fsm): pass def ConnectTimeout(self, fsm): self.Default(fsm) def Die(self, fsm): self.Default(fsm) def DisconnectTimeout(self, fsm): self.Default(fsm) def HeartbeatTimeout(self, fsm): self.Default(fsm) def SisterAlive(self, fsm): self.Default(fsm) def SisterDead(self, fsm): self.Default(fsm) def Default(self, fsm): msg = "\n\tState: %s\n\tTransition: %s" % ( fsm.getState().getName(), fsm.getTransition()) raise statemap.TransitionUndefinedException(msg) class SM_Default(BondSMState): pass class SM_WaitingForSister(SM_Default): def ConnectTimeout(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def Die(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Connected() finally: fsm.setState(SM.Alive) fsm.getState().Entry(fsm) class SM_Alive(SM_Default): def Die(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.StartDying() finally: fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def HeartbeatTimeout(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Heartbeat() finally: fsm.setState(SM.Alive) fsm.getState().Entry(fsm) def SisterDead(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.SisterDied() ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM_AwaitSisterDeath(SM_Default): def DisconnectTimeout(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def HeartbeatTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def SisterAlive(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.AwaitSisterDeath) fsm.getState().Entry(fsm) def SisterDead(self, fsm): ctxt = fsm.getOwner() fsm.getState().Exit(fsm) fsm.clearState() try: ctxt.Death() finally: fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM_Dead(SM_Default): def ConnectTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def Die(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def DisconnectTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def HeartbeatTimeout(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) def SisterDead(self, fsm): fsm.getState().Exit(fsm) fsm.setState(SM.Dead) fsm.getState().Entry(fsm) class SM(object): WaitingForSister = SM_WaitingForSister('SM.WaitingForSister', 0) Alive = SM_Alive('SM.Alive', 1) AwaitSisterDeath = SM_AwaitSisterDeath('SM.AwaitSisterDeath', 2) Dead = SM_Dead('SM.Dead', 3) Default = SM_Default('SM.Default', -1) class BondSM_sm(statemap.FSMContext): def __init__(self, owner): statemap.FSMContext.__init__(self, SM.WaitingForSister) self._owner = owner def __getattr__(self, attrib): def trans_sm(*arglist): self._transition = attrib getattr(self.getState(), attrib)(self, *arglist) self._transition = None return trans_sm def enterStartState(self): self._state.Entry(self) def getOwner(self): return self._owner # Local variables: # buffer-read-only: t # End: bond_core-1.8.6/test_bond/scripts/tester.py000077500000000000000000000221561372223053100210170ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Stuart Glaser # This is largely copied from bondpy, with hooks that "break" it for testing purposes. from __future__ import print_function import threading import time import uuid import roslib; roslib.load_manifest('test_bond') import rospy from bond.msg import Constants, Status from test_bond.srv import TestBond, TestBondResponse import atexit atexit.register(rospy.signal_shutdown, 'exit') import BondSM_sm class Timeout: def __init__(self, duration, on_timeout=None): self.duration = duration self.timer = threading.Timer(0, self._on_timer) self.deadline = rospy.Time.now() self.on_timeout = on_timeout def reset(self): self.timer.cancel() self.timer = threading.Timer(self.duration.to_sec(), self._on_timer) self.timer.start() self.deadline = rospy.Time.now() + self.duration return self def cancel(self): self.timer.cancel() def left(self): return max(rospy.Duration(0), self.deadline - rospy.Time.now()) def _on_timer(self): if self.on_timeout: self.on_timeout() class BondTester: def __init__(self, req): self.req = req self.topic = req.topic self.id = req.id self.instance_id = str(uuid.uuid4()) self.on_death = None self.on_life = None self.is_shutdown = False self.sister_died_first = False self.death_started = None self.sm = BondSM_sm.BondSM_sm(self) # self.sm.setDebugFlag(True) self.lock = threading.RLock() self.condition = threading.Condition(self.lock) self.connect_timeout = Timeout( rospy.Duration(Constants.DEFAULT_CONNECT_TIMEOUT), self._on_connect_timeout) self.heartbeat_timeout = Timeout( rospy.Duration(Constants.DEFAULT_HEARTBEAT_TIMEOUT), self._on_heartbeat_timeout) self.disconnect_timeout = Timeout( rospy.Duration(Constants.DEFAULT_DISCONNECT_TIMEOUT), self._on_disconnect_timeout) self.connect_timeout.reset() self.sub = rospy.Subscriber(self.topic, Status, self._on_bond_status) self.pub = rospy.Publisher(self.topic, Status) self.thread = threading.Thread(target=self._publishing_thread) self.thread.daemon = True self.thread.start() if req.delay_death >= rospy.Duration(0.0): self.death_timeout = Timeout(req.delay_death, self.die).reset() self.deadline = None def _on_connect_timeout(self): with self.lock: self.sm.ConnectTimeout() def _on_heartbeat_timeout(self): if self.req.inhibit_death: return with self.lock: self.sm.HeartbeatTimeout() def _on_disconnect_timeout(self): with self.lock: self.sm.DisconnectTimeout() def __del__(self): self.shutdown() def shutdown(self): if not self.is_shutdown: with self.lock: self.is_shutdown = True if self.sm.getState().getName() != 'SM.Dead': print("I'm not dead yet:%s in %s" % (self.id, self.sm.getState().getName())) self.die() self.sub.unregister() self.pub.unregister() self.condition.notify_all() print("Unregistered") def _on_bond_status(self, msg): # Filters out messages from other bonds and messages from ourselves if msg.id == self.id and msg.instance_id != self.instance_id: with self.lock: if msg.active: if not self.is_dead(): self.sm.SisterAlive() else: if self.req.inhibit_death: return self.sm.SisterDead() # Immediate ack for sister's death notification if self.sister_died_first: self._publish(False) def _publish(self, active): msg = Status() msg.header.stamp = rospy.Time.now() msg.id = self.id msg.instance_id = self.instance_id msg.active = active if not msg.active and self.req.inhibit_death_message: pass else: self.pub.publish(msg) def _publishing_thread(self): time.sleep(self.req.delay_connect.to_sec()) with self.lock: # Publishing ALIVE while not self.is_shutdown and self.sm.getState().getName() in [ 'SM.WaitingForSister', 'SM.Alive']: self._publish(True) self.condition.wait(Constants.DEFAULT_HEARTBEAT_PERIOD) # Publishing DEAD while not self.is_shutdown and self.sm.getState().getName() == 'SM.AwaitSisterDeath': self._publish(False) self.condition.wait(Constants.DEAD_PUBLISH_PERIOD) def Connected(self): self.connect_timeout.cancel() self.condition.notify_all() if self.on_life: self.on_life() def Heartbeat(self): self.heartbeat_timeout.reset() def SisterDied(self): self.sister_died_first = True def Death(self): self.condition.notify_all() self.heartbeat_timeout.cancel() self.disconnect_timeout.cancel() if not self.death_started: self.death_started = rospy.Time.now() def StartDying(self): self.heartbeat_timeout.cancel() self.disconnect_timeout.reset() if not self.death_started: self.death_started = rospy.Time.now() def wait_for_life(self, timeout=None): if self.deadline: self.deadline.cancel() self.deadline = None if timeout: self.deadline = Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() == 'SM.WaitingForSister': if self.deadline and self.deadline.left() == rospy.Duration(0): break self.condition.wait(self.deadline and self.deadline.left().to_sec()) return self.sm.getState().getName() != 'SM.WaitingForSister' def wait_for_death(self, timeout=None): if self.deadline: self.deadline.cancel() self.deadline = None if timeout: self.deadline = Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() != 'SM.Dead': if self.deadline and self.deadline.left() == rospy.Duration(0): break self.condition.wait(self.deadline and self.deadline.left().to_sec()) return self.sm.getState().getName() == 'SM.Dead' def is_dead(self): with self.lock: return self.sm.getState().getName() == 'SM.Dead' def die(self): with self.lock: self.sm.Die() self._publish(False) def __repr__(self): return "[Bond %s, Instance %s (%s)]" % ( self.id, self.instance_id, self.sm.getState().getName()) class Tester: def __init__(self): self.bond_tester = None self.service = rospy.Service('test_bond', TestBond, self._test_bond) def _test_bond(self, req): print("TEST") if self.bond_tester: self.bond_tester.shutdown() self.bond_tester = BondTester(req) print("Test bond instance id: %s" % self.bond_tester.instance_id) return TestBondResponse() def main(): rospy.init_node('bond_tester', anonymous=True, disable_signals=True) tester = Tester() # noqa rospy.spin() if __name__ == '__main__': main() bond_core-1.8.6/test_bond/srv/000077500000000000000000000000001372223053100162515ustar00rootroot00000000000000bond_core-1.8.6/test_bond/srv/TestBond.srv000066400000000000000000000001651372223053100205310ustar00rootroot00000000000000string topic string id duration delay_connect duration delay_death bool inhibit_death bool inhibit_death_message --- bond_core-1.8.6/test_bond/test/000077500000000000000000000000001372223053100164165ustar00rootroot00000000000000bond_core-1.8.6/test_bond/test/exercise_bond.cpp000066400000000000000000000127211372223053100217360ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #ifndef _WIN32 # include #else # include #endif #include #include #include const char TOPIC[] = "test_bond_topic"; std::string genId() { #ifndef _WIN32 uuid_t uuid; uuid_generate_random(uuid); char uuid_str[40]; uuid_unparse(uuid, uuid_str); return std::string(uuid_str); #else UUID uuid; UuidCreate(&uuid); RPC_CSTR str; UuidToStringA(&uuid, &str); std::string return_string(reinterpret_cast(str)); RpcStringFreeA(&str); return return_string; #endif } ros::ServiceClient getService() { ros::NodeHandle nh; ros::ServiceClient cli = nh.serviceClient("test_bond"); EXPECT_TRUE(cli); EXPECT_TRUE(cli.waitForExistence(ros::Duration(5.0))); return cli; } TEST(ExerciseBondCpp, normal) { std::string id = genId(); bond::Bond bond(TOPIC, id); ros::ServiceClient cli = getService(); EXPECT_TRUE(cli.isValid()); EXPECT_TRUE(cli.exists()); test_bond::TestBond::Request req; test_bond::TestBond::Response resp; req.topic = TOPIC; req.id = id; req.delay_death = ros::Duration(2.0); ASSERT_TRUE(cli.call(req, resp)); bond.start(); EXPECT_TRUE(bond.waitUntilFormed(ros::Duration(2.0))); EXPECT_TRUE(bond.waitUntilBroken(ros::Duration(5.0))); } TEST(ExerciseBondCpp, remoteNeverConnects) { std::string id = genId(); // Never starts the other side of the bond bond::Bond bond(TOPIC, id); bond.start(); EXPECT_FALSE(bond.waitUntilFormed(ros::Duration(1.0))); EXPECT_TRUE(bond.waitUntilBroken(ros::Duration(20.0))); } TEST(ExerciseBondCpp, heartbeatTimeout) { std::string id = genId(); bond::Bond bond(TOPIC, id); ros::ServiceClient cli = getService(); test_bond::TestBond::Request req; test_bond::TestBond::Response resp; req.topic = TOPIC; req.id = id; req.delay_death = ros::Duration(2.0); req.inhibit_death_message = true; ASSERT_TRUE(cli.call(req, resp)); bond.start(); EXPECT_TRUE(bond.waitUntilFormed(ros::Duration(2.0))); EXPECT_TRUE(bond.waitUntilBroken(ros::Duration(10.0))); } TEST(ExerciseBondCpp, cleanLocalDeath) { std::string id = genId(); bond::Bond bond(TOPIC, id); ros::ServiceClient cli = getService(); test_bond::TestBond::Request req; test_bond::TestBond::Response resp; req.topic = TOPIC; req.id = id; req.delay_death = ros::Duration(-1); ASSERT_TRUE(cli.call(req, resp)); bond.start(); EXPECT_TRUE(bond.waitUntilFormed(ros::Duration(2.0))); bond.breakBond(); EXPECT_TRUE(bond.waitUntilBroken(ros::Duration(5.0))); } TEST(ExerciseBondCpp, localDeathNoAck) { std::string id = genId(); bond::Bond bond(TOPIC, id); ros::ServiceClient cli = getService(); test_bond::TestBond::Request req; test_bond::TestBond::Response resp; req.topic = TOPIC; req.id = id; req.delay_death = ros::Duration(-1); req.inhibit_death_message = true; ASSERT_TRUE(cli.call(req, resp)); bond.start(); EXPECT_TRUE(bond.waitUntilFormed(ros::Duration(2.0))); bond.breakBond(); EXPECT_TRUE(bond.waitUntilBroken(ros::Duration(5.0))); } TEST(ExerciseBondCpp, remoteIgnoresLocalDeath) { std::string id = genId(); bond::Bond bond(TOPIC, id); ros::ServiceClient cli = getService(); test_bond::TestBond::Request req; test_bond::TestBond::Response resp; req.topic = TOPIC; req.id = id; req.delay_death = ros::Duration(-1); req.inhibit_death = true; ASSERT_TRUE(cli.call(req, resp)); bond.start(); EXPECT_TRUE(bond.waitUntilFormed(ros::Duration(2.0))); bond.breakBond(); EXPECT_FALSE(bond.waitUntilBroken(ros::Duration(1.0))); EXPECT_TRUE(bond.waitUntilBroken(ros::Duration(10.0))); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "exercise_bondcpp", true); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle nh; int ret = RUN_ALL_TESTS(); spinner.stop(); return ret; }; bond_core-1.8.6/test_bond/test/exercise_bond.py000077500000000000000000000137201372223053100216070ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Stuart Glaser import sys import time import uuid PKG = 'test_bond' import roslib; roslib.load_manifest(PKG) import rospy from bondpy import bondpy from test_bond.srv import TestBond import unittest import rostest import atexit atexit.register(rospy.signal_shutdown, 'exit') def gen_id(): return "test_" + str(uuid.uuid4()) TOPIC = "test_bond_topic" test_bond = rospy.ServiceProxy('test_bond', TestBond) class Exerciser(unittest.TestCase): # Bond formed, bond broken remotely def test_normal(self): id = gen_id() test_bond.wait_for_service() test_bond(id=id, topic=TOPIC, delay_death=rospy.Duration(2.0)) rospy.logerr("test_normal: test_bond service call just returned") self.bond = bond = bondpy.Bond(TOPIC, id) bond.start() bond_start_time = time.time() formed = bond.wait_until_formed(rospy.Duration(2.0)) if not formed: s = '' s += "BONDFAIL\n" s += "wait_until_formed returned %s\n" % formed s += "Bond details:\n" s += " Started = %s\n" % bond._Bond__started s += " sister_instance_id = %s\n" % bond.sister_instance_id s += " is_shutdown = %s\n" % bond.is_shutdown s += " num connections = %d\n" % bond.pub.get_num_connections() formed_later = bond.wait_until_formed(rospy.Duration(20.0)) s += "Formed later: %s after %.3f seconds\n" % ( formed_later, time.time() - bond_start_time) rospy.logerr(s) self.fail(s) # self.assertTrue(bond.wait_until_formed(rospy.Duration(2.0))) self.assertTrue(bond.wait_until_broken(rospy.Duration(5.0))) # Remote never connects def test_no_connect(self): id = gen_id() # Don't start the other side of the bond self.bond = bond = bondpy.Bond(TOPIC, id) bond.start() self.assertFalse(bond.wait_until_formed(rospy.Duration(1.0))) self.assertTrue(bond.wait_until_broken(rospy.Duration(20.0))) # Remote dies (but doesn't report it) def test_heartbeat_timeout(self): id = gen_id() test_bond.wait_for_service() test_bond(id=id, topic=TOPIC, delay_death=rospy.Duration(2.0), inhibit_death_message=True) self.bond = bond = bondpy.Bond(TOPIC, id) bond.start() self.assertTrue(bond.wait_until_formed(rospy.Duration(2.0))) self.assertTrue(bond.wait_until_broken(rospy.Duration(10.0))) # Local dies, remote acks normally def test_i_die(self): id = gen_id() test_bond.wait_for_service() test_bond(id=id, topic=TOPIC, delay_death=rospy.Duration(-1)) self.bond = bond = bondpy.Bond(TOPIC, id) bond.start() self.assertTrue(bond.wait_until_formed(rospy.Duration(2.0))) bond.break_bond() self.assertTrue(bond.wait_until_broken(rospy.Duration(2.0))) # Local dies, remote goes silent def test_die_no_ack(self): id = gen_id() test_bond.wait_for_service() test_bond(id=id, topic=TOPIC, delay_death=rospy.Duration(-1), inhibit_death_message=True) self.bond = bond = bondpy.Bond(TOPIC, id) bond.start() self.assertTrue(bond.wait_until_formed(rospy.Duration(2.0))) bond.break_bond() self.assertTrue(bond.wait_until_broken(rospy.Duration(5.0))) # Local dies, remote stays alive def test_die_ignore_death(self): id = gen_id() test_bond.wait_for_service() test_bond(id=id, topic=TOPIC, delay_death=rospy.Duration(-1), inhibit_death=True) self.bond = bond = bondpy.Bond(TOPIC, id) bond.start() self.assertTrue(bond.wait_until_formed(rospy.Duration(2.0))) bond.break_bond() self.assertFalse(bond.wait_until_broken(rospy.Duration(1.0))) self.assertTrue(bond.wait_until_broken(rospy.Duration(10.0))) # Shutdown() before start() def test_shutdown_before_start(self): id = gen_id() self.bond = bondpy.Bond(TOPIC, id) self.bond.shutdown() self.bond = None def setUp(self): self.bond = None def tearDown(self): if self.bond: self.bond.shutdown() self.bond = None def main(): rospy.init_node('exercise_bondpy', anonymous=True, disable_signals=True) rostest.run(PKG, 'exercise_bondpy', Exerciser, sys.argv) if __name__ == '__main__': main() bond_core-1.8.6/test_bond/test/test_callbacks_cpp.cpp000066400000000000000000000054041372223053100227450ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #ifndef _WIN32 # include #else # include #endif #include #include #include const char TOPIC[] = "test_bond_topic"; std::string genId() { #ifndef _WIN32 uuid_t uuid; uuid_generate_random(uuid); char uuid_str[40]; uuid_unparse(uuid, uuid_str); return std::string(uuid_str); #else UUID uuid; UuidCreate(&uuid); RPC_CSTR str; UuidToStringA(&uuid, &str); std::string return_string(reinterpret_cast(str)); RpcStringFreeA(&str); return return_string; #endif } TEST(TestCallbacksCpp, dieInLifeCallback) { std::string id = genId(); bond::Bond a(TOPIC, id); bond::Bond b(TOPIC, id); a.setFormedCallback(boost::bind(&bond::Bond::breakBond, &a)); a.start(); b.start(); EXPECT_TRUE(a.waitUntilFormed(ros::Duration(5.0))); EXPECT_TRUE(b.waitUntilBroken(ros::Duration(3.0))); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_callbacks_cpp", true); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle nh; int ret = RUN_ALL_TESTS(); spinner.stop(); return ret; }; bond_core-1.8.6/test_bond/test/test_callbacks_cpp.launch000066400000000000000000000001461372223053100234330ustar00rootroot00000000000000 bond_core-1.8.6/test_bond/test/test_callbacks_py.launch000066400000000000000000000001471372223053100233020ustar00rootroot00000000000000 bond_core-1.8.6/test_bond/test/test_callbacks_py.py000077500000000000000000000047551372223053100224740ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Stuart Glaser import sys import uuid PKG = 'test_bond' import roslib; roslib.load_manifest(PKG) import rospy from bondpy import bondpy import unittest import rostest import atexit atexit.register(rospy.signal_shutdown, 'exit') TOPIC = "test_bond_topic" def gen_id(): return "test_" + str(uuid.uuid4()) class CallbackTests(unittest.TestCase): def test_die_in_life_callback(self): id = gen_id() a = bondpy.Bond(TOPIC, id) b = bondpy.Bond(TOPIC, id) a.set_formed_callback(a.break_bond) a.start() b.start() self.assertTrue(a.wait_until_formed(rospy.Duration(5.0))) self.assertTrue(b.wait_until_broken(rospy.Duration(3.0))) del a, b def main(): rospy.init_node('test_callbacks_python', anonymous=True, disable_signals=True) rostest.run(PKG, 'test_callbacks_python', CallbackTests, sys.argv) if __name__ == '__main__': main() bond_core-1.8.6/test_bond/test/test_cpp.launch000066400000000000000000000002371372223053100214350ustar00rootroot00000000000000 bond_core-1.8.6/test_bond/test/test_python.launch000066400000000000000000000002451372223053100221730ustar00rootroot00000000000000