diff --git a/include/iri_base_driver/iri_base_driver.h b/include/iri_base_driver/iri_base_driver.h index 2250327bcd55317257344b77b92eea27a3bd3854..02bbc4b2a9642732f1ea1f016bf95d6a91696f4d 100644 --- a/include/iri_base_driver/iri_base_driver.h +++ b/include/iri_base_driver/iri_base_driver.h @@ -73,6 +73,10 @@ class IriBaseDriver boost::function< void() > preCloseHook; + boost::function< void() > postStartHook; + + boost::function< void() > preStopHook; + /** * \brief driver unique identifier * @@ -200,6 +204,10 @@ class IriBaseDriver void setPreCloseHook(boost::function< void() > function); + void setPostStartHook(boost::function< void() > function); + + void setPreStopHook(boost::function< void() > function); + /** * \brief Destructor * diff --git a/include/iri_base_driver/iri_base_driver_node.h b/include/iri_base_driver/iri_base_driver_node.h index 1baef5b345709cbdacc8c95cfa052d2b6c96f8d0..ca0677753c0463fc4a60ef053a677b1d07c98d57 100644 --- a/include/iri_base_driver/iri_base_driver_node.h +++ b/include/iri_base_driver/iri_base_driver_node.h @@ -37,20 +37,6 @@ namespace iri_base_driver { -/** - * \brief Abstract Class for Ctrl+C management - * - */ -class AbstractDriverNode -{ - protected: - - static void hupCalled(int sig) - { - ROS_WARN("Unexpected SIGHUP caught. Ignoring it."); - } -}; - /** * \brief default main thread frequency * @@ -88,7 +74,7 @@ class AbstractDriverNode * from the IRI GitLab respository (See http://wiki.iri.upc.edu). */ template <class Driver> -class IriBaseNodeDriver : public AbstractDriverNode +class IriBaseNodeDriver { public: /** @@ -149,6 +135,18 @@ class IriBaseNodeDriver : public AbstractDriverNode */ void preCloseHook(void); + /** + * \brief main node post start hook + * + */ + void postStartHook(void); + + /** + * \brief main node pre stop hook + * + */ + void preStopHook(void); + /** * \brief dynamic reconfigure server callback * @@ -235,6 +233,18 @@ class IriBaseNodeDriver : public AbstractDriverNode */ virtual void preNodeCloseHook(void); + /** + * \brief node implementation post start hook + * + */ + virtual void postNodeStartHook(void); + + /** + * \brief node implementation pre stop hook + * + */ + virtual void preNodeStopHook(void); + /** * \brief dynamic reconfigure server callback * @@ -292,6 +302,8 @@ class IriBaseNodeDriver : public AbstractDriverNode */ double getRate(void); + static void hupCalled(int sig); + public: /** @@ -302,7 +314,7 @@ class IriBaseNodeDriver : public AbstractDriverNode * variables are also initialized. * */ - IriBaseNodeDriver(ros::NodeHandle &nh); + IriBaseNodeDriver(const ros::NodeHandle &nh = ros::NodeHandle("~")); /** * \brief spin @@ -322,7 +334,7 @@ class IriBaseNodeDriver : public AbstractDriverNode }; template <class Driver> -IriBaseNodeDriver<Driver>::IriBaseNodeDriver(ros::NodeHandle &nh) : +IriBaseNodeDriver<Driver>::IriBaseNodeDriver(const ros::NodeHandle &nh) : public_node_handle_(nh), loop_rate_(DEFAULT_RATE), diagnostic_(), @@ -333,11 +345,15 @@ IriBaseNodeDriver<Driver>::IriBaseNodeDriver(ros::NodeHandle &nh) : ROS_DEBUG("IriBaseNodeDriver::Constructor"); this->driver_.setPostOpenHook(boost::bind(&IriBaseNodeDriver::postOpenHook, this)); this->driver_.setPreCloseHook(boost::bind(&IriBaseNodeDriver::preCloseHook, this)); + this->driver_.setPostStartHook(boost::bind(&IriBaseNodeDriver::postStartHook, this)); + this->driver_.setPreStopHook(boost::bind(&IriBaseNodeDriver::preStopHook, this)); // create the status thread pthread_create(&this->main_thread,NULL,this->mainThread,this); // initialize class attributes + // allow Ctrl+C management + signal(SIGHUP, &IriBaseNodeDriver<Driver>::hupCalled); } template <class Driver> @@ -401,6 +417,34 @@ void IriBaseNodeDriver<Driver>::preNodeCloseHook(void) } +template <class Driver> +void IriBaseNodeDriver<Driver>::postStartHook(void) +{ + ROS_DEBUG("IriBaseNodeDriver::postStartHook"); + /* call the inherited class post start function */ + this->postNodeStartHook(); +} + +template <class Driver> +void IriBaseNodeDriver<Driver>::postNodeStartHook(void) +{ + +} + +template <class Driver> +void IriBaseNodeDriver<Driver>::preStopHook(void) +{ + ROS_DEBUG("IriBaseNodeDriver::preStopHook"); + /* call the inherited class pre stop function */ + this->preNodeStopHook(); +} + +template <class Driver> +void IriBaseNodeDriver<Driver>::preNodeStopHook(void) +{ + +} + template <class Driver> void IriBaseNodeDriver<Driver>::node_config_update(Config &config, uint32_t level) { @@ -515,6 +559,12 @@ int IriBaseNodeDriver<Driver>::spin(void) return 0; } +template <class Driver> +void IriBaseNodeDriver<Driver>::hupCalled(int sig) +{ + ROS_WARN("Unexpected SIGHUP caught. Ignoring it."); +} + template <class Driver> IriBaseNodeDriver<Driver>::~IriBaseNodeDriver() { @@ -549,9 +599,6 @@ int main(int argc, char **argv, std::string node_name) // ROS initialization ros::init(argc, argv, node_name); - // allow Ctrl+C management - signal(SIGHUP, &AbstractDriverNode::hupCalled); - // define and launch generic algorithm implementation object ros::NodeHandle nh; DriverType driver(nh); diff --git a/src/iri_base_driver.cpp b/src/iri_base_driver.cpp index 54bc667ec3732e8e039033b7820d27f7ec2b48a5..71c008e7ee9bf38f0a990be8055a3ee210579621 100644 --- a/src/iri_base_driver.cpp +++ b/src/iri_base_driver.cpp @@ -246,6 +246,16 @@ void IriBaseDriver::setPreCloseHook(boost::function< void() > function) this->preCloseHook = function; } +void IriBaseDriver::setPostStartHook(boost::function< void() > function) +{ + this->postStartHook = function; +} + +void IriBaseDriver::setPreStopHook(boost::function< void() > function) +{ + this->preStopHook = function; +} + IriBaseDriver::~IriBaseDriver() { pthread_mutex_destroy(&this->access_);