Merge branch 'melodic_migration' into 'master'

Added a new spin function (nodelet_spin) to be used in nodelet nodes where the...

See merge request !3

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)
 {