diff --git a/include/iri_base_driver/iri_base_driver_node.h b/include/iri_base_driver/iri_base_driver_node.h index 18437db79466ebe149047cc14ceacd4d5f60902b..870441858735183647b52539dbaa142b09aa11e4 100644 --- a/include/iri_base_driver/iri_base_driver_node.h +++ b/include/iri_base_driver/iri_base_driver_node.h @@ -325,6 +325,7 @@ class IriBaseNodeDriver */ int spin(void); + int nodelet_spin(void); /** * \brief destructor * @@ -559,6 +560,48 @@ int IriBaseNodeDriver<Driver>::spin(void) return 0; } +template <class Driver> +int IriBaseNodeDriver<Driver>::nodelet_spin(void) +{ + ROS_DEBUG("IriBaseNodeDriver::spin"); + + // initialize diagnostics + this->addDiagnostics(); + + // initial call of the reconfigure callback function +// this->reconfigureCallback(this->driver_.config_,iri_base_driver::SensorLevels::RECONFIGURE_CLOSE); + // assign callback to dynamic reconfigure server + this->dsrv_.setCallback(boost::bind(&IriBaseNodeDriver<Driver>::reconfigureCallback, this, _1, _2)); + + // create the status thread + pthread_create(&this->main_thread,NULL,this->mainThread,this); + + // try to go to the running state + this->driver_.goState(iri_base_driver::RUNNING); + + while(ros::ok()) + { + if (!this->driver_.isRunning()) + { + ros::WallDuration(1).sleep(); + this->driver_.goState(iri_base_driver::CLOSED); + this->driver_.goState(iri_base_driver::RUNNING); + } + // update diagnostics + this->diagnostic_.update(); + + ros::WallDuration(this->diagnostic_.getPeriod()).sleep(); + } + + // try to go to the closed state + this->driver_.goState(iri_base_driver::CLOSED); + + // stop ros + ros::shutdown(); + + return 0; +} + template <class Driver> void IriBaseNodeDriver<Driver>::hupCalled(int sig) {