diff --git a/CMakeLists.txt b/CMakeLists.txt
index 926896be739aa4dd14a226a1e37a3985819f43f6..9bb3939bbdb57c22676e1de090256e30b34cbc54 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,12 @@
cmake_minimum_required(VERSION 2.8.3)
project(iri_base_driver)
+set(CMAKE_BUILD_TYPE "DEBUG")
+
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure message_generation)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -39,11 +41,10 @@ find_package(catkin REQUIRED)
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
+add_message_files(
+ FILES
+ SensorLevels.msg
+)
## Generate services in the 'srv' folder
# add_service_files(
@@ -60,10 +61,10 @@ find_package(catkin REQUIRED)
# )
## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-# )
+generate_messages(
+ DEPENDENCIES
+ std_msgs # Or other packages containing msgs
+)
###################################
## catkin specific configuration ##
@@ -77,7 +78,7 @@ find_package(catkin REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS roscpp driver_base dynamic_reconfigure
+ CATKIN_DEPENDS roscpp dynamic_reconfigure message_runtime
# DEPENDS system_lib
)
@@ -89,12 +90,14 @@ catkin_package(
## Your package locations should be listed before other locations
# include_directories(include)
INCLUDE_DIRECTORIES(include)
+include_directories(${catkin_INCLUDE_DIRS})
## Declare a cpp library
# add_library(foo
# src/${PROJECT_NAME}/foo.cpp
# )
add_library(${PROJECT_NAME} src/iri_base_driver.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
## Declare a cpp executable
# add_executable(foo_node src/foo_node.cpp)
@@ -102,6 +105,7 @@ add_library(${PROJECT_NAME} src/iri_base_driver.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(foo_node foo_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(foo_node
diff --git a/include/iri_base_driver/iri_base_driver.h b/include/iri_base_driver/iri_base_driver.h
index 90d5cac9a19a3240932fe71af64f580ee45f14d1..137b8053093ba6b669915dbe5cc96f0ec6f73597 100644
--- a/include/iri_base_driver/iri_base_driver.h
+++ b/include/iri_base_driver/iri_base_driver.h
@@ -20,12 +20,16 @@
#define _IRI_BASE_DRIVER_H
// ros base driver class
-#include "driver_base/driver.h"
#include "ros/ros.h"
+// boost libraries
+#include <boost/function.hpp>
+
namespace iri_base_driver
{
+typedef enum {CLOSED=0,OPENED=1,RUNNING=2} state_t;
+
/**
* \brief IRI ROS Base Driver Class
*
@@ -48,16 +52,10 @@ namespace iri_base_driver
* through ROS topics by using those scripts. The scripts can be downloaded
* from the iri_stack SVN.
*/
-class IriBaseDriver : public driver_base::Driver
+class IriBaseDriver
{
- protected:
- /**
- * \brief driver unique identifier
- *
- * This value is used to identify each driver. Must be set before changing
- * the driver_base::state=run. Use the abstract function setDriverId().
- */
- std::string driver_id_;
+ private:
+ state_t current_state_;
/**
* \brief variable to handle the mutual exclusion mechanism
@@ -69,6 +67,23 @@ class IriBaseDriver : public driver_base::Driver
*/
pthread_mutex_t access_;
+ pthread_mutex_t internal_access_;
+
+ boost::function< void() > postOpenHook;
+
+ boost::function< void() > preCloseHook;
+
+ /**
+ * \brief driver unique identifier
+ *
+ * This value is used to identify each driver. Must be set before changing
+ * the driver_base::state=run. Use the abstract function setDriverId().
+ */
+ std::string driver_id_;
+
+ std::string last_message;
+
+ protected:
/**
* \brief Set Driver Id
*
@@ -80,9 +95,6 @@ class IriBaseDriver : public driver_base::Driver
*/
void setDriverId(const std::string & id);
-// typedef boost::function< void() > hookFunction;
- hookFunction preCloseHook;
-
public:
/**
* \brief Constructor
@@ -115,42 +127,6 @@ class IriBaseDriver : public driver_base::Driver
*/
bool try_enter(void);
- /**
- * \brief Generic Open Driver
- *
- * Currently only changes the ROS driver state to open and calls the abstract
- * function openDriver(). Common actions required for opening any kind of
- * drivers should be added here.
- */
- void doOpen(void);
-
- /**
- * \brief Generic Close Driver
- *
- * Currently only changes the ROS driver state to close and calls the abstract
- * function closeDriver(). Common actions required for opening any kind of
- * drivers should be added here.
- */
- void doClose(void);
-
- /**
- * \brief Generic Start Driver
- *
- * Currently only changes the ROS driver state to start and calls the abstract
- * function startDriver().Common actions required for opening any kind of
- * drivers should be added here.
- */
- void doStart(void);
-
- /**
- * \brief Generic Stop Driver
- *
- * Currently only changes the ROS driver state to stop and calls the abstract
- * function stopDriver(). Common actions required for opening any kind of
- * drivers should be added here.
- */
- void doStop(void);
-
/**
* \brief Get Driver Id
*
@@ -162,6 +138,22 @@ class IriBaseDriver : public driver_base::Driver
*/
std::string getID(void);
+ bool goState(state_t target_state);
+
+ state_t getState(void);
+
+ std::string getStateName(void);
+
+ std::string getStatusMessage(void);
+
+ bool isRunning(void);
+
+ bool isOpened(void);
+
+ bool isClosed(void);
+
+ bool isStopped(void);
+
/**
* \brief User Open Driver
*
@@ -204,8 +196,9 @@ class IriBaseDriver : public driver_base::Driver
*/
virtual bool stopDriver(void) = 0;
-
- void setPreCloseHook(hookFunction f);
+ void setPostOpenHook(boost::function< void() > function);
+
+ void setPreCloseHook(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 cdbee840ac9b6f4debd347d564575d98843f0b4a..00b21355a9e31577782c3ef6ee29cd9d615df54b 100644
--- a/include/iri_base_driver/iri_base_driver_node.h
+++ b/include/iri_base_driver/iri_base_driver_node.h
@@ -19,19 +19,50 @@
#ifndef _IRI_BASE_DRIVER_NODE_H
#define _IRI_BASE_DRIVER_NODE_H
-// ros base driver node, diagnostics, and iri_ros driver
-#include <driver_base/driver_node.h>
+#include <ros/ros.h>
+#include <signal.h>
+
+// boost thread includes for ROS::spin thread
+#include <boost/thread.hpp>
+#include <boost/bind.hpp>
+
+// dynamic reconfigure server include
+#include <dynamic_reconfigure/server.h>
+#include <iri_base_driver/SensorLevels.h>
+
+// diagnostic updater include
#include <diagnostic_updater/diagnostic_updater.h>
-#include "iri_base_driver.h"
+
+#include "iri_base_driver/iri_base_driver.h"
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
+ *
+ * This constant determines the default frequency of the mainThread() in HZ.
+ * All nodes will loop at this rate if loop_rate_ variable is not modified.
+ */
+ static const double DEFAULT_RATE = 10.0; //[Hz]
/**
* \brief IRI ROS Base Node Class
*
- * This class inherits from the ROS class driver_base::DriverNode<TemplateDriver>
- * to provide an execution thread to the driver object. The TemplateDriver object
+ * This class provides the base class for any ROS driver. The Driver object
* must be an implementation of the IriBaseDriver class. The inherit template
* design form allows complete access to the driver object while manteining
* flexibility to instantiate any object inherit from IriBaseDriver.
@@ -58,55 +89,36 @@ namespace iri_base_driver
* from the iri_stack SVN.
*/
template <class Driver>
-class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
+class IriBaseNodeDriver : public AbstractDriverNode
{
- protected:
- /**
- * \brief Thread information structure
- *
- * This structure hold system level information of the thread and it is
- * initialized when the thread is first created. This information can not
- * be modified at any time and it is not accessible from outside the class.
- *
- */
- pthread_t thread;
-
- /**
- * \brief main node post open hook
- *
- * This function sets the common parameters for all drivers which might be
- * tuned for the ROS dynamic reconfigure application.
- */
- void postOpenHook(void);
-
- /**
- * \brief node implementation post open hook
- *
- * Abstrac function called by postOpenHook() that adds the specific driver
- * node parameters to the dynamic reconfigure application.
- */
- virtual void postNodeOpenHook(void) = 0;
-
+ public:
/**
- * \brief main node pre close hook
+ * \brief config object
*
+ * All template driver class will need a Config variable to allow ROS
+ * dynamic reconfigure. This config class will contain all driver
+ * parameters which may be modified once the driver node is launched.
*/
- void preCloseHook(void);
+ typedef typename Driver::Config Config;
+ private:
/**
- * \brief node implementation pre close hook
+ * \brief ros spin thread
*
+ * This boost thread object will manage the ros::spin function. It is
+ * instantiated and launched in the spin class method.
*/
- virtual void preNodeCloseHook(void);
+ boost::shared_ptr<boost::thread> spin_thread_;
- /**
- * \brief public node handle communication object
- *
- * This node handle is going to be used to create topics and services within
- * the node namespace. Additional node handles can be instantatied if
- * additional namespaces are needed.
- */
- ros::NodeHandle public_node_handle_;
+ /**
+ * \brief Thread information structure
+ *
+ * This structure hold system level information of the thread and it is
+ * initialized when the thread is first created. This information can not
+ * be modified at any time and it is not accessible from outside the class.
+ *
+ */
+ pthread_t main_thread;
/**
* \brief main thread loop rate
@@ -119,31 +131,38 @@ class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
ros::Rate loop_rate_;
/**
- * \brief default main thread frequency
+ * \brief
*
- * This constant determines the default frequency of the mainThread() in HZ.
- * All nodes will loop at this rate if loop_rate_ variable is not modified.
*/
- static const unsigned int DEFAULT_RATE = 10; //[Hz]
+ diagnostic_updater::FunctionDiagnosticTask driver_status_standard_diagnostic_;
- public:
/**
- * \brief constructor
+ * \brief main node post open hook
*
- * This constructor creates and initializes the main thread of the class. It
- * also sets the driver_base::postOpenHook thread. NodeHandle and loop_rate_
- * variables are also initialized.
+ * This function sets the common parameters for all drivers which might be
+ * tuned for the ROS dynamic reconfigure application.
+ */
+ void postOpenHook(void);
+
+ /**
+ * \brief main node pre close hook
*
- * \param nh a reference to the node handle object to manage all ROS topics.
*/
- IriBaseNodeDriver(ros::NodeHandle& nh);
+ void preCloseHook(void);
/**
- * \brief destructor
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic driver class must
+ * implement it.
*
- * This destructor closes the driver object and kills the main thread.
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
*/
- ~IriBaseNodeDriver();
+ void reconfigureCallback(Config &config, uint32_t level);
/**
* \brief main node add diagnostics
@@ -153,70 +172,91 @@ class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
*/
void addDiagnostics(void);
+ protected:
/**
- * \brief open status driver main node tests
+ * \brief template driver class
*
- * In this function tests checking driver's functionallity in driver_base
- * status=open can be added.
+ * This template class refers to an implementation of an specific driver
+ * interface. Will be used in the derivate class to define the common
+ * behaviour for all the different implementations from the same driver.
*/
- void addOpenedTests(void);
+ Driver driver_;
/**
- * \brief stop status driver main node tests
+ * \brief public node handle communication object
*
- * In this function tests checking driver's functionallity in driver_base
- * status=stop can be added.
+ * This node handle is going to be used to create topics and services within
+ * the node namespace. Additional node handles can be instantatied if
+ * additional namespaces are needed.
*/
- void addStoppedTests(void);
+ ros::NodeHandle public_node_handle_;
/**
- * \brief run status driver main node tests
+ * \brief private node handle object
*
- * In this function tests checking driver's functionallity in driver_base
- * status=run can be added.
+ * This private node handle will be used to define algorithm parameters into
+ * the ROS parametre server. For communication pruposes please use the
+ * previously defined node_handle_ object.
*/
- void addRunningTests(void);
+ ros::NodeHandle private_node_handle_;
/**
- * \brief main node dynamic reconfigure
- *
+ * \brief dynamic reconfigure server
*
- * \param level integer
+ * The dynamic reconfigure server is in charge to retrieve the parameters
+ * defined in the config cfg file through the reconfigureCallback.
*/
- void reconfigureHook(int level);
+ dynamic_reconfigure::Server<Config> dsrv_;
- protected:
/**
- * \brief node add diagnostics
- *
- * In this abstract function additional ROS diagnostics applied to the
- * specific node may be added.
+ * \brief diagnostic updater
+ *
+ * The diagnostic updater allows definition of custom diagnostics.
+ *
*/
- virtual void addNodeDiagnostics(void) = 0;
+ diagnostic_updater::Updater diagnostic_;
/**
- * \brief open status driver specific node tests
- *
- * In this abstract function tests checking driver's functionallity in
- * driver_base status=open can be added.
+ * \brief
+ *
*/
- virtual void addNodeOpenedTests(void) = 0;
+ void statusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
/**
- * \brief stop status driver specific node tests
+ * \brief node implementation post open hook
+ *
+ * Abstrac function called by postOpenHook() that adds the specific driver
+ * node parameters to the dynamic reconfigure application.
+ */
+ virtual void postNodeOpenHook(void) = 0;
+
+ /**
+ * \brief node implementation pre close hook
+ *
+ */
+ virtual void preNodeCloseHook(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic driver class must
+ * implement it.
*
- * In this abstract function tests checking driver's functionallity in
- * driver_base status=stop can be added.
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
*/
- virtual void addNodeStoppedTests(void) = 0;
+ virtual void node_config_update(Config &config, uint32_t level);
/**
- * \brief run status driver specific node tests
+ * \brief node add diagnostics
*
- * In this abstract function tests checking driver's functionallity in
- * driver_base status=run can be added.
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific node may be added.
*/
- virtual void addNodeRunningTests(void) = 0;
+ virtual void addNodeDiagnostics(void) = 0;
/**
* \brief main node thread
@@ -242,27 +282,61 @@ class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
virtual void mainNodeThread(void) = 0;
/**
- * \brief specific node dynamic reconfigure
+ * \brief
+ *
+ */
+ void setRate(double rate_hz);
+
+ /**
+ * \brief
+ *
+ */
+ double getRate(void);
+
+ public:
+
+ /**
+ * \brief constructor
+ *
+ * This constructor creates and initializes the main thread of the class. It
+ * also sets the driver_base::postOpenHook thread. NodeHandle and loop_rate_
+ * variables are also initialized.
*
- * This function is called reconfigureHook()
+ */
+ IriBaseNodeDriver(ros::NodeHandle &nh);
+
+ /**
+ * \brief spin
*
- * \param level integer
+ * This method is meant to spin the node to update all ROS features. It also
+ * launches de main thread. Once the object is instantiated, it will not
+ * start iterating until this method is called.
+ */
+ int spin(void);
+
+ /**
+ * \brief destructor
+ *
+ * This destructor closes the driver object and kills the main thread.
*/
- virtual void reconfigureNodeHook(int level) = 0;
+ ~IriBaseNodeDriver();
};
template <class Driver>
IriBaseNodeDriver<Driver>::IriBaseNodeDriver(ros::NodeHandle &nh) :
- driver_base::DriverNode<Driver>(nh),
- public_node_handle_(ros::this_node::getName()),
- loop_rate_(DEFAULT_RATE)
+ public_node_handle_(nh),
+ loop_rate_(DEFAULT_RATE),
+ diagnostic_(),
+ dsrv_(private_node_handle_),
+ private_node_handle_("~"),
+ driver_status_standard_diagnostic_("Driver Status", boost::bind(&IriBaseNodeDriver::statusDiagnostic, this, _1))
{
ROS_DEBUG("IriBaseNodeDriver::Constructor");
this->driver_.setPostOpenHook(boost::bind(&IriBaseNodeDriver::postOpenHook, this));
- this->driver_.setPreCloseHook(boost::bind(&IriBaseNodeDriver::preNodeCloseHook, this));
+ this->driver_.setPreCloseHook(boost::bind(&IriBaseNodeDriver::preCloseHook, this));
// create the status thread
- pthread_create(&this->thread,NULL,this->mainThread,this);
+ pthread_create(&this->main_thread,NULL,this->mainThread,this);
// initialize class attributes
}
@@ -276,20 +350,26 @@ void *IriBaseNodeDriver<Driver>::mainThread(void *param)
while(ros::ok())
{
-// std::cout << __LINE__ << ": driver state=" << iriNode->driver_.getStateName() << std::endl;
if(iriNode->driver_.isRunning())
- {
iriNode->mainNodeThread();
-
-// ros::spinOnce();
- }
iriNode->loop_rate_.sleep();
-
}
pthread_exit(NULL);
}
+template <class Driver>
+void IriBaseNodeDriver<Driver>::setRate(double rate_hz)
+{
+ this->loop_rate_=rate_hz;
+}
+
+template <class Driver>
+double IriBaseNodeDriver<Driver>::getRate(void)
+{
+ return this->loop_rate_;
+}
+
template <class Driver>
void IriBaseNodeDriver<Driver>::postOpenHook(void)
{
@@ -298,54 +378,136 @@ void IriBaseNodeDriver<Driver>::postOpenHook(void)
//set hardware id with driver id
this->diagnostic_.setHardwareID(this->driver_.getID());
- postNodeOpenHook();
+ /* call the inherited class post open function */
+ this->postNodeOpenHook();
}
template <class Driver>
void IriBaseNodeDriver<Driver>::preCloseHook(void)
{
- ROS_INFO("IriBaseNodeDriver::preCloseHook");
- preNodeCloseHook();
+ ROS_DEBUG("IriBaseNodeDriver::preCloseHook");
+ /* call the inherited class pre close function */
+ this->preNodeCloseHook();
}
template <class Driver>
void IriBaseNodeDriver<Driver>::preNodeCloseHook(void)
{
+
}
-
+
template <class Driver>
-void IriBaseNodeDriver<Driver>::addDiagnostics(void)
+void IriBaseNodeDriver<Driver>::node_config_update(Config &config, uint32_t level)
{
- ROS_DEBUG("IriBaseNodeDriver::addDiagnostics");
- addNodeDiagnostics();
+
}
template <class Driver>
-void IriBaseNodeDriver<Driver>::addOpenedTests(void)
+void IriBaseNodeDriver<Driver>::statusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
- ROS_DEBUG("IriBaseNodeDriver::addOpenedTests");
- addNodeOpenedTests();
+ if (driver_.isRunning())
+ stat.summary(0, "OK");
+ else
+ stat.summary(2, "not running");
+
+ stat.add("Driver state:", this->driver_.getStateName());
+ stat.add("Latest status message:", this->driver_.getStatusMessage());
}
template <class Driver>
-void IriBaseNodeDriver<Driver>::addStoppedTests(void)
+void IriBaseNodeDriver<Driver>::addDiagnostics(void)
{
- ROS_DEBUG("IriBaseNodeDriver::addStoppedTests");
- addNodeStoppedTests();
+ ROS_DEBUG("IriBaseNodeDriver::addDiagnostics");
+ this->diagnostic_.add(this->driver_status_standard_diagnostic_);
+
+ /* add the inherited class diagnostics */
+ this->addNodeDiagnostics();
}
template <class Driver>
-void IriBaseNodeDriver<Driver>::addRunningTests(void)
+void IriBaseNodeDriver<Driver>::reconfigureCallback(Config &config, uint32_t level)
{
- ROS_DEBUG("IriBaseNodeDriver::addRunningTests");
- addNodeRunningTests();
+ iri_base_driver::state_t original_state;
+
+ /// @todo Move this into the Driver class?
+ ROS_DEBUG("IriBaseNodeDriver::reconfigureCallback");
+
+ original_state=this->driver_.getState();
+ /* go to the desired state for the update */
+ switch(level)
+ {
+ case iri_base_driver::SensorLevels::RECONFIGURE_STOP:
+ this->driver_.goState(iri_base_driver::OPENED);
+ break;
+ case iri_base_driver::SensorLevels::RECONFIGURE_CLOSE:
+ this->driver_.goState(iri_base_driver::CLOSED);
+ break;
+ case iri_base_driver::SensorLevels::RECONFIGURE_RUNNING:
+ this->driver_.goState(iri_base_driver::RUNNING);
+ break;
+ }
+
+ /* update the driver */
+ this->driver_.config_update(config, level);
+ /* update the node */
+ this->node_config_update(config, level);
+
+ /* return to the previous state */
+ this->driver_.goState(original_state);
}
template <class Driver>
-void IriBaseNodeDriver<Driver>::reconfigureHook(int level)
+int IriBaseNodeDriver<Driver>::spin(void)
{
- ROS_DEBUG("IriBaseNodeDriver::reconfigureHook");
- reconfigureNodeHook(level);
+ ROS_DEBUG("IriBaseNodeDriver::spin");
+
+ // initialize diagnostics
+ this->addDiagnostics();
+
+ // launch ros spin in different thread
+ this->spin_thread_.reset(new boost::thread(boost::bind(&ros::spin)));
+ assert(spin_thread_);
+
+ // 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(0.1).sleep();
+ }
+
+ // try to go to the closed state
+ this->driver_.goState(iri_base_driver::CLOSED);
+
+ // stop ros
+ ros::shutdown();
+
+ if (this->spin_thread_ && !this->spin_thread_->timed_join((boost::posix_time::milliseconds) 2000))
+ {
+ ROS_ERROR("ROS thread did not die after two seconds. Pretending that it did. This is probably a bad sign.");
+ }
+
+ // kill ros spin thread
+ this->spin_thread_.reset();
+
+ return 0;
}
template <class Driver>
@@ -354,12 +516,43 @@ IriBaseNodeDriver<Driver>::~IriBaseNodeDriver()
ROS_DEBUG("IriBaseNodeDriver::Destrcutor");
//try access to driver to assert it is
//unlock before its destruction
- this->driver_.try_enter();
- this->driver_.unlock();
- this->driver_.close();
- pthread_cancel(this->thread);
- pthread_join(this->thread,NULL);
+ this->driver_.goState(iri_base_driver::CLOSED);
+ pthread_cancel(this->main_thread);
+ pthread_join(this->main_thread,NULL);
+}
+
+/**
+ * \brief base main
+ *
+ * This main is common for all the driver nodes. The DriverType class
+ * refers to an specific implementation derived from a generic driver.
+ *
+ * First, ros::init is called providing the node name. Ctrl+C control is
+ * activated for sercure and safe exit. Finally, a DriverType object is
+ * defined and launched to spin for endless loop execution.
+ *
+ * \param argc integer with number of input parameters
+ * \param argv array with all input strings
+ * \param node_name name of the node
+ */
+template <class DriverType>
+int main(int argc, char **argv, std::string node_name)
+{
+ ROS_DEBUG("IriBaseDriver::%s Launched", node_name.c_str());
+
+ // 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);
+ driver.spin();
+
+ return 0;
}
}
diff --git a/msg/SensorLevels.msg b/msg/SensorLevels.msg
new file mode 100644
index 0000000000000000000000000000000000000000..5502f4c46535b006c19364d7de99f0293f55b6c9
--- /dev/null
+++ b/msg/SensorLevels.msg
@@ -0,0 +1,4 @@
+byte RECONFIGURE_CLOSE = 3 # Parameters that need a sensor to be stopped completely when changed
+byte RECONFIGURE_STOP = 1 # Parameters that need a sensor to stop streaming when changed
+byte RECONFIGURE_RUNNING = 0 # Parameters that can be changed while a sensor is streaming
+
diff --git a/package.xml b/package.xml
index 4b3baf87c2c552fde5cc19c1731a24b11b34f344..44cb7490c36ef296085ed28883401cddab1e4b47 100644
--- a/package.xml
+++ b/package.xml
@@ -34,14 +34,14 @@
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<build_depend>roscpp</build_depend>
- <build_depend>driver_base</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>message_generation</build_depend>
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<run_depend>roscpp</run_depend>
- <run_depend>driver_base</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>message_runtime</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
diff --git a/src/iri_base_driver.cpp b/src/iri_base_driver.cpp
index 984f667e0f70e6cb852336486b0f0c4cb1933d31..99ff593e7966e256ecbe5a5c10067c3351a9ea59 100644
--- a/src/iri_base_driver.cpp
+++ b/src/iri_base_driver.cpp
@@ -3,9 +3,13 @@
namespace iri_base_driver
{
-IriBaseDriver::IriBaseDriver() : driver_id_("none")
+IriBaseDriver::IriBaseDriver()
{
+ this->current_state_=iri_base_driver::CLOSED;
+ this->driver_id_="none";
+ this->last_message="not operational yet";
pthread_mutex_init(&this->access_,NULL);
+ pthread_mutex_init(&this->internal_access_,NULL);
}
void IriBaseDriver::lock(void)
@@ -31,40 +35,221 @@ void IriBaseDriver::setDriverId(const std::string & id)
driver_id_ = id;
}
-void IriBaseDriver::doOpen(void)
+std::string IriBaseDriver::getID(void)
{
- if(openDriver()) this->state_ = OPENED;
+ return this->driver_id_;
}
-void IriBaseDriver::doClose(void)
+bool IriBaseDriver::goState(state_t target_state)
{
- this->preCloseHook();
- if(closeDriver()) this->state_ = CLOSED;
+ std::string transition;
+ bool error=false;
+
+ pthread_mutex_lock(&this->internal_access_);
+ try{
+ do{
+ switch(this->current_state_)
+ {
+ case iri_base_driver::CLOSED: if(target_state == iri_base_driver::OPENED || target_state == iri_base_driver::RUNNING)
+ {
+ transition="open";
+ ROS_DEBUG("Trying transition open from CLOSED to OPENED.");
+ if(this->openDriver())
+ {
+ this->current_state_=iri_base_driver::OPENED;
+ this->postOpenHook();
+ this->last_message="Transition open from CLOSED to OPENED succeeded.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::CLOSED;
+ this->last_message="Transition open from CLOSED to OPENED failed.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ error=true;
+ }
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::CLOSED;
+ this->last_message="No transition required";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ break;
+ case iri_base_driver::OPENED: if(target_state == iri_base_driver::CLOSED)
+ {
+ transition="close";
+ ROS_DEBUG("Trying transition close from OPENED to CLOSED.");
+ this->preCloseHook();
+ if(this->closeDriver())
+ {
+ this->current_state_=iri_base_driver::CLOSED;
+ this->last_message="Transition close from OPENED to CLOSED succeeded.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::OPENED;
+ this->last_message="Transition close from OPENED to CLOSED failed.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ error=true;
+ }
+ }
+ else if(target_state == iri_base_driver::RUNNING)
+ {
+ transition="start";
+ ROS_DEBUG("Trying transition start from OPENED to RUNNING.");
+ if(this->startDriver())
+ {
+ this->current_state_=iri_base_driver::RUNNING;
+ this->last_message="Transition start from OPENED to RUNNING succeeded.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::OPENED;
+ this->last_message="Transition start from OPENED to RUNNING failed.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ error=true;
+ }
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::OPENED;
+ this->last_message="No transition required";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ break;
+ case RUNNING: if(target_state == iri_base_driver::OPENED || target_state == iri_base_driver::CLOSED)
+ {
+ transition="stop";
+ ROS_DEBUG("Trying transition stop from RUNNING to OPENED.");
+ if(this->stopDriver())
+ {
+ this->current_state_=iri_base_driver::OPENED;
+ this->last_message="Transition stop from RUNING to OPENED succeeded.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::RUNNING;
+ this->last_message="Transition stop from RUNNING to OPENED failed.";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ error=true;
+ }
+ }
+ else
+ {
+ this->current_state_=iri_base_driver::RUNNING;
+ this->last_message="No transition required";
+ ROS_DEBUG("%s",this->last_message.c_str());
+ }
+ break;
+ }
+ }while(!error && this->current_state_!=target_state);
+ }catch(...){
+ ROS_WARN("Caught exception in state transition %s\n",transition.c_str());
+ error=true;
+ }
+ pthread_mutex_unlock(&this->internal_access_);
+
+ return error;
}
-void IriBaseDriver::doStart(void)
+state_t IriBaseDriver::getState(void)
{
- if(startDriver()) this->state_ = RUNNING;
+ return this->current_state_;
}
-void IriBaseDriver::doStop(void)
+std::string IriBaseDriver::getStateName(void)
{
- if(stopDriver()) this->state_ = OPENED;
+ switch(this->current_state_)
+ {
+ case iri_base_driver::CLOSED: return std::string("closed");
+ case iri_base_driver::OPENED: return std::string("oepned");
+ case iri_base_driver::RUNNING: return std::string("running");
+ }
}
-std::string IriBaseDriver::getID(void)
+std::string IriBaseDriver::getStatusMessage(void)
+{
+ return this->last_message;
+}
+
+bool IriBaseDriver::isRunning(void)
+{
+ pthread_mutex_lock(&this->internal_access_);
+ if(this->current_state_ == iri_base_driver::RUNNING)
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return true;
+ }
+ else
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return false;
+ }
+}
+
+bool IriBaseDriver::isOpened(void)
+{
+ pthread_mutex_lock(&this->internal_access_);
+ if(this->current_state_ == iri_base_driver::OPENED)
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return true;
+ }
+ else
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return false;
+ }
+}
+
+bool IriBaseDriver::isClosed(void)
+{
+ pthread_mutex_lock(&this->internal_access_);
+ if(this->current_state_ == iri_base_driver::CLOSED)
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return true;
+ }
+ else
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return false;
+ }
+}
+
+bool IriBaseDriver::isStopped(void)
+{
+ pthread_mutex_lock(&this->internal_access_);
+ if(this->current_state_ == iri_base_driver::OPENED || this->current_state_ == iri_base_driver::CLOSED)
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return true;
+ }
+ else
+ {
+ pthread_mutex_unlock(&this->internal_access_);
+ return false;
+ }
+}
+
+void IriBaseDriver::setPostOpenHook(boost::function< void() > function)
{
- return driver_id_;
+ this->postOpenHook = function;
}
-void IriBaseDriver::setPreCloseHook(hookFunction f)
+void IriBaseDriver::setPreCloseHook(boost::function< void() > function)
{
- preCloseHook = f;
+ this->preCloseHook = function;
}
IriBaseDriver::~IriBaseDriver()
{
pthread_mutex_destroy(&this->access_);
+ pthread_mutex_destroy(&this->internal_access_);
}
}