From b28410e136475844b92604f4eb8449d843f84e93 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 13 Jul 2020 12:43:48 +0200
Subject: [PATCH] Added a new spin function (nodelet_spin) to be used in
nodelet nodes where the ros::spin function call is not required.
---
.../iri_base_driver/iri_base_driver_node.h | 43 +++++++++++++++++++
1 file changed, 43 insertions(+)
diff --git a/include/iri_base_driver/iri_base_driver_node.h b/include/iri_base_driver/iri_base_driver_node.h
index 18437db..8704418 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)
{
--
GitLab