pax_global_header00006660000000000000000000000064125330520660014514gustar00rootroot0000000000000052 comment=89fa35d7bf7d2e71c1247e03e93945e5b56823c9 ros-bond-core-1.7.16/000077500000000000000000000000001253305206600142615ustar00rootroot00000000000000ros-bond-core-1.7.16/.gitignore000066400000000000000000000000061253305206600162450ustar00rootroot00000000000000*.pyc ros-bond-core-1.7.16/bond/000077500000000000000000000000001253305206600152035ustar00rootroot00000000000000ros-bond-core-1.7.16/bond/BondSM.sm000066400000000000000000000016351253305206600166730ustar00rootroot00000000000000// -*- 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 {} } %% ros-bond-core-1.7.16/bond/CMakeLists.txt000066400000000000000000000004321253305206600177420ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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) ros-bond-core-1.7.16/bond/mainpage.dox000066400000000000000000000011061253305206600174760ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b link is ... \section codeapi Code API */ ros-bond-core-1.7.16/bond/msg/000077500000000000000000000000001253305206600157715ustar00rootroot00000000000000ros-bond-core-1.7.16/bond/msg/Constants.msg000066400000000000000000000004111253305206600204510ustar00rootroot00000000000000float32 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_timeoutros-bond-core-1.7.16/bond/msg/Status.msg000066400000000000000000000004111253305206600177600ustar00rootroot00000000000000Header 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_periodros-bond-core-1.7.16/bond/package.xml000066400000000000000000000016341253305206600173240ustar00rootroot00000000000000 bond 1.7.16 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. Stuart Glaser Esteve Fernandez BSD http://www.ros.org/wiki/bond https://github.com/ros/bond_core/issues https://github.com/ros/bond_core catkin message_generation std_msgs message_runtime std_msgs ros-bond-core-1.7.16/bond_core/000077500000000000000000000000001253305206600162135ustar00rootroot00000000000000ros-bond-core-1.7.16/bond_core/CMakeLists.txt000066400000000000000000000001541253305206600207530ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(bond_core) find_package(catkin REQUIRED) catkin_metapackage() ros-bond-core-1.7.16/bond_core/package.xml000066400000000000000000000016101253305206600203260ustar00rootroot00000000000000 bond_core 1.7.16 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. Esteve Fernandez 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 ros-bond-core-1.7.16/bondcpp/000077500000000000000000000000001253305206600157065ustar00rootroot00000000000000ros-bond-core-1.7.16/bondcpp/CMakeLists.txt000066400000000000000000000017661253305206600204600ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(bondcpp) find_package(Boost REQUIRED) find_package(catkin REQUIRED bond cmake_modules roscpp smclib) find_package(UUID REQUIRED) include_directories(SYSTEM ${BOOST_INCLUDE_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ${UUID_LIBRARIES} CATKIN_DEPENDS bond roscpp smclib DEPENDS Boost ) 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} ) ros-bond-core-1.7.16/bondcpp/include/000077500000000000000000000000001253305206600173315ustar00rootroot00000000000000ros-bond-core-1.7.16/bondcpp/include/bondcpp/000077500000000000000000000000001253305206600207565ustar00rootroot00000000000000ros-bond-core-1.7.16/bondcpp/include/bondcpp/BondSM_sm.h000066400000000000000000000077601253305206600227620ustar00rootroot00000000000000#ifndef _H_BONDSM_SM #define _H_BONDSM_SM /* * 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 // _H_BONDSM_SM ros-bond-core-1.7.16/bondcpp/include/bondcpp/bond.h000066400000000000000000000145121253305206600220540ustar00rootroot00000000000000/* * 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 "BondSM_sm.h" 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 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::WallTimer publishingTimer_; void onConnectTimeout(); void onHeartbeatTimeout(); void onDisconnectTimeout(); void bondStatusCB(const bond::Status::ConstPtr &msg); void doPublishing(const ros::WallTimerEvent &e); void publishStatus(bool active); std::vector > pending_callbacks_; void flushPendingCallbacks(); }; }// namespace // 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 ros-bond-core-1.7.16/bondcpp/include/bondcpp/timeout.h000066400000000000000000000045461253305206600226260ustar00rootroot00000000000000/* * 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::WallTimer timer_; ros::WallTime deadline_; ros::WallDuration duration_; boost::function on_timeout_; void timerCallback(const ros::WallTimerEvent &e); }; } // namespace #endif ros-bond-core-1.7.16/bondcpp/mainpage.dox000066400000000000000000000014361253305206600202070ustar00rootroot00000000000000/** \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 */ ros-bond-core-1.7.16/bondcpp/package.xml000066400000000000000000000017451253305206600200320ustar00rootroot00000000000000 bondcpp 1.7.16 C++ implementation of bond, a mechanism for checking when another process has terminated. Stuart Glaser Esteve Fernandez BSD http://www.ros.org/wiki/bondcpp https://github.com/ros/bond_core/issues https://github.com/ros/bond_core catkin bond boost cmake_modules roscpp smclib uuid bond boost roscpp smclib uuid ros-bond-core-1.7.16/bondcpp/src/000077500000000000000000000000001253305206600164755ustar00rootroot00000000000000ros-bond-core-1.7.16/bondcpp/src/BondSM_sm.cpp000066400000000000000000000153331253305206600210270ustar00rootroot00000000000000/* * 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" using namespace statemap; // 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 ( 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: */ ros-bond-core-1.7.16/bondcpp/src/bond.cpp000066400000000000000000000253331253305206600201310ustar00rootroot00000000000000/* * 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 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_.createWallTimer(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::WallTime deadline(ros::WallTime::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::WallTime::now()); if (wait_time <= ros::WallDuration(0.0)) break; // The deadline has expired condition_.timed_wait(mutex_, boost::posix_time::milliseconds(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::WallTime deadline(ros::WallTime::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::WallTime::now()); if (wait_time <= ros::WallDuration(0.0)) break; // The deadline has expired condition_.timed_wait(mutex_, boost::posix_time::milliseconds(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::WallTimerEvent &e) { 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 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)); } ros-bond-core-1.7.16/bondcpp/src/timeout.cpp000066400000000000000000000051001253305206600206630ustar00rootroot00000000000000/* * 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 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_.createWallTimer(duration_, &Timeout::timerCallback, this, true); deadline_ = ros::WallTime::now() + duration_; } void Timeout::cancel() { timer_.stop(); } ros::WallDuration Timeout::left() { return std::max(ros::WallDuration(0.0), deadline_ - ros::WallTime::now()); } void Timeout::timerCallback(const ros::WallTimerEvent &e) { if (on_timeout_) on_timeout_(); } } ros-bond-core-1.7.16/bondpy/000077500000000000000000000000001253305206600155545ustar00rootroot00000000000000ros-bond-core-1.7.16/bondpy/CMakeLists.txt000066400000000000000000000001761253305206600203200ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(bondpy) find_package(catkin REQUIRED) catkin_python_setup() catkin_package() ros-bond-core-1.7.16/bondpy/mainpage.dox000066400000000000000000000011101253305206600200420ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b linkpy is ... \section codeapi Code API */ ros-bond-core-1.7.16/bondpy/package.xml000066400000000000000000000015211253305206600176700ustar00rootroot00000000000000 bondpy 1.7.16 Python implementation of bond, a mechanism for checking when another process has terminated. Stuart Glaser Esteve Fernandez BSD http://www.ros.org/wiki/bondpy https://github.com/ros/bond_core/issues https://github.com/ros/bond_core catkin bond rospy smclib rospy smclib uuid ros-bond-core-1.7.16/bondpy/python/000077500000000000000000000000001253305206600170755ustar00rootroot00000000000000ros-bond-core-1.7.16/bondpy/python/bondpy/000077500000000000000000000000001253305206600203705ustar00rootroot00000000000000ros-bond-core-1.7.16/bondpy/python/bondpy/BondSM_sm.py000066400000000000000000000127511253305206600225710ustar00rootroot00000000000000# 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: ros-bond-core-1.7.16/bondpy/python/bondpy/__init__.py000066400000000000000000000000001253305206600224670ustar00rootroot00000000000000ros-bond-core-1.7.16/bondpy/python/bondpy/bondpy.py000066400000000000000000000307331253305206600222430ustar00rootroot00000000000000# 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 threading import time import uuid import rospy from bond.msg import * import BondSM_sm 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.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.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 # 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.pub = rospy.Publisher(self.topic, Status) 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: 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() 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): deadline = timeout and Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() == 'SM.WaitingForSister': if rospy.is_shutdown(): break if deadline and deadline.left() == rospy.Duration(0): break wait_duration = 0.1 if deadline: wait_duration = min(wait_duration, 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): deadline = timeout and Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() != 'SM.Dead': if rospy.is_shutdown(): break if deadline and deadline.left() == rospy.Duration(0): break wait_duration = 0.1 if deadline: wait_duration = min(wait_duration, 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()) ros-bond-core-1.7.16/bondpy/setup.py000066400000000000000000000003321253305206600172640ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['bondpy'], package_dir={'': 'python'} ) setup(**d) ros-bond-core-1.7.16/smclib/000077500000000000000000000000001253305206600155325ustar00rootroot00000000000000ros-bond-core-1.7.16/smclib/CMakeLists.txt000066400000000000000000000004261253305206600202740ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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} ) ros-bond-core-1.7.16/smclib/include/000077500000000000000000000000001253305206600171555ustar00rootroot00000000000000ros-bond-core-1.7.16/smclib/include/smclib/000077500000000000000000000000001253305206600204265ustar00rootroot00000000000000ros-bond-core-1.7.16/smclib/include/smclib/statemap.h000066400000000000000000000543211253305206600224220ustar00rootroot00000000000000#ifndef _H_STATEMAP #define _H_STATEMAP // // 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) // #if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) #include #if defined(SMC_NO_EXCEPTIONS) #include #endif // SMC_NO_EXCEPTIONS #include #elif defined(WIN32) #include #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 // 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 } // // 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 ros-bond-core-1.7.16/smclib/mainpage.dox000066400000000000000000000011101253305206600200200ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b smclib is ... \section codeapi Code API */ ros-bond-core-1.7.16/smclib/package.xml000066400000000000000000000015631253305206600176540ustar00rootroot00000000000000 smclib 1.7.16 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. Various Esteve Fernandez Mozilla Public License Version 1.1 http://smc.sourceforge.net/ https://github.com/ros/bond_core/issues https://github.com/ros/bond_core catkin ros-bond-core-1.7.16/smclib/python/000077500000000000000000000000001253305206600170535ustar00rootroot00000000000000ros-bond-core-1.7.16/smclib/python/smclib/000077500000000000000000000000001253305206600203245ustar00rootroot00000000000000ros-bond-core-1.7.16/smclib/python/smclib/__init__.py000066400000000000000000000000001253305206600224230ustar00rootroot00000000000000ros-bond-core-1.7.16/smclib/python/smclib/statemap.py000066400000000000000000000116001253305206600225120ustar00rootroot00000000000000# # 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 == 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 == 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 != 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 = [] ros-bond-core-1.7.16/smclib/setup.py000066400000000000000000000003321253305206600172420ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['smclib'], package_dir={'': 'python'} ) setup(**d) ros-bond-core-1.7.16/test_bond/000077500000000000000000000000001253305206600162425ustar00rootroot00000000000000ros-bond-core-1.7.16/test_bond/CMakeLists.txt000066400000000000000000000023051253305206600210020ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(test_bond) find_package(catkin REQUIRED COMPONENTS bondcpp genmsg rostest) if(CATKIN_ENABLE_TESTING) include_directories(${GTEST_INCLUDE_DIRS}) include_directories(SYSTEM ${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 add_executable(exercise_bond EXCLUDE_FROM_ALL test/exercise_bond.cpp) target_link_libraries(exercise_bond ${catkin_LIBRARIES} ${GTEST_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}) 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() ros-bond-core-1.7.16/test_bond/mainpage.dox000066400000000000000000000011141253305206600205340ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b test_bonds is ... \section codeapi Code API */ ros-bond-core-1.7.16/test_bond/package.xml000066400000000000000000000013471253305206600203640ustar00rootroot00000000000000 test_bond 1.7.16 Contains tests for [[bond]], including tests for [[bondpy]] and [[bondcpp]]. Stuart Glaser Esteve Fernandez BSD http://www.ros.org/wiki/test_bond https://github.com/ros/bond_core/issues https://github.com/ros/bond_core catkin bondcpp genmsg rostest bondpy ros-bond-core-1.7.16/test_bond/scripts/000077500000000000000000000000001253305206600177315ustar00rootroot00000000000000ros-bond-core-1.7.16/test_bond/scripts/BondSM_sm.py000066400000000000000000000116571253305206600221360ustar00rootroot00000000000000# 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: ros-bond-core-1.7.16/test_bond/scripts/tester.py000077500000000000000000000212371253305206600216210ustar00rootroot00000000000000#! /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. import threading import time import uuid import roslib; roslib.load_manifest('test_bond') import rospy from bond.msg import * from test_bond.srv import * 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() 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:", self.id, " in ", 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): deadline = timeout and Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() == 'SM.WaitingForSister': if deadline and deadline.left() == rospy.Duration(0): break self.condition.wait(deadline and deadline.left().to_sec()) return self.sm.getState().getName() != 'SM.WaitingForSister' def wait_for_death(self, timeout = None): deadline = timeout and Timeout(timeout).reset() with self.lock: while self.sm.getState().getName() != 'SM.Dead': if deadline and deadline.left() == rospy.Duration(0): break self.condition.wait(deadline and 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() rospy.spin() if __name__ == '__main__': main() ros-bond-core-1.7.16/test_bond/srv/000077500000000000000000000000001253305206600170545ustar00rootroot00000000000000ros-bond-core-1.7.16/test_bond/srv/TestBond.srv000066400000000000000000000001651253305206600213340ustar00rootroot00000000000000string topic string id duration delay_connect duration delay_death bool inhibit_death bool inhibit_death_message --- ros-bond-core-1.7.16/test_bond/test/000077500000000000000000000000001253305206600172215ustar00rootroot00000000000000ros-bond-core-1.7.16/test_bond/test/exercise_bond.cpp000066400000000000000000000122721253305206600225420ustar00rootroot00000000000000/* * 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 #include #include #include const std::string TOPIC = "test_bond_topic"; std::string genId() { uuid_t uuid; uuid_generate_random(uuid); char uuid_str[40]; uuid_unparse(uuid, uuid_str); return std::string(uuid_str); } 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; }; ros-bond-core-1.7.16/test_bond/test/exercise_bond.py000077500000000000000000000134171253305206600224150ustar00rootroot00000000000000#! /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 threading import time import uuid PKG = 'test_bond' import roslib; roslib.load_manifest(PKG) import rospy from bondpy import bondpy from test_bond.srv import * 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))) 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() ros-bond-core-1.7.16/test_bond/test/test_callbacks_cpp.cpp000066400000000000000000000047551253305206600235600ustar00rootroot00000000000000/* * 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 #include #include #include const std::string TOPIC = "test_bond_topic"; std::string genId() { uuid_t uuid; uuid_generate_random(uuid); char uuid_str[40]; uuid_unparse(uuid, uuid_str); return std::string(uuid_str); } 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; }; ros-bond-core-1.7.16/test_bond/test/test_callbacks_cpp.launch000066400000000000000000000001461253305206600242360ustar00rootroot00000000000000 ros-bond-core-1.7.16/test_bond/test/test_callbacks_py.launch000066400000000000000000000001471253305206600241050ustar00rootroot00000000000000 ros-bond-core-1.7.16/test_bond/test/test_callbacks_py.py000077500000000000000000000050351253305206600232670ustar00rootroot00000000000000#! /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 threading import time import uuid PKG = 'test_bond' import roslib; roslib.load_manifest(PKG) import rospy from bondpy import bondpy from test_bond.srv import * 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() ros-bond-core-1.7.16/test_bond/test/test_cpp.launch000066400000000000000000000002371253305206600222400ustar00rootroot00000000000000 ros-bond-core-1.7.16/test_bond/test/test_python.launch000066400000000000000000000002451253305206600227760ustar00rootroot00000000000000