merge from iri_base_driver5

CMakeLists.txt

@@ -26,7 +26,6 @@ SET(headers ./include/iri_base_driver.h ./include/iri_base_driver_node.h)
 FIND_PACKAGE(iriutils REQUIRED)
 
 INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${headers})
-INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
 
 #common commands for building c++ executables and libraries
 rosbuild_add_library(${PROJECT_NAME} SHARED ${sources})

include/iri_base_driver/iri_base_driver.h

@@ -61,10 +61,11 @@ class IriBaseDriver : public driver_base::Driver
     std::string driver_id_;
 
    /**
-    * \brief driver unique identifier
+    * \brief driver mutex
     *
-    * This value is used to identify each driver. Must be set before changing
-    * the driver_base::state=run. Use the abstract function setDriverId().
+    * This variable is used to block access to the driver resource. Public
+    * methods such as lock, unlock and try_enter provide an interface to
+    * avoid multiple concurrent access.
     */
     CMutex access_;
 
@@ -79,6 +80,9 @@ class IriBaseDriver : public driver_base::Driver
     */
     void setDriverId(const std::string & id);
 
+//     typedef boost::function< void() > hookFunction;
+    hookFunction preCloseHook;
+
   public:
    /**
     * \brief Constructor
@@ -199,6 +203,9 @@ class IriBaseDriver : public driver_base::Driver
     * \return bool successful
     */
     virtual bool stopDriver(void) = 0;
+    
+    
+    void setPreCloseHook(hookFunction f);
 
    /**
     * \brief Destructor

include/iri_base_driver/iri_base_driver_node.h

@@ -20,8 +20,8 @@
 #define _IRI_BASE_DRIVER_NODE_H
 
 // ros base driver node, diagnostics, and iri_ros driver
-#include "driver_base/driver_node.h"
-#include "diagnostic_updater/diagnostic_updater.h"
+#include <driver_base/driver_node.h>
+#include <diagnostic_updater/diagnostic_updater.h>
 #include "iri_base_driver.h"
 
 // iri-utils thread server
@@ -100,6 +100,18 @@ class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
     */
     virtual void postNodeOpenHook(void) = 0;
 
+   /**
+    * \brief main node pre close hook
+    *
+    */
+    void preCloseHook(void);
+
+   /**
+    * \brief node implementation pre close hook
+    * 
+    */
+    virtual void preNodeCloseHook(void);
+
    /**
     * \brief public node handle communication object
     *
@@ -258,7 +270,9 @@ IriBaseNodeDriver<Driver>::IriBaseNodeDriver(ros::NodeHandle &nh) :
   public_node_handle_(ros::this_node::getName()),
   loop_rate_(DEFAULT_RATE)
 {
+  ROS_DEBUG("IriBaseNodeDriver::Constructor");
   this->driver_.setPostOpenHook(boost::bind(&IriBaseNodeDriver::postOpenHook, this));
+  this->driver_.setPreCloseHook(boost::bind(&IriBaseNodeDriver::preNodeCloseHook, this));
 
   // create the status thread
   this->thread_server_=CThreadServer::instance();
@@ -274,6 +288,8 @@ IriBaseNodeDriver<Driver>::IriBaseNodeDriver(ros::NodeHandle &nh) :
 template <class Driver>
 void *IriBaseNodeDriver<Driver>::mainThread(void *param)
 {
+  ROS_DEBUG("IriBaseNodeDriver::mainThread");
+
   IriBaseNodeDriver<Driver> *iriNode = (IriBaseNodeDriver<Driver> *)param;
 
   while(ros::ok())
@@ -294,51 +310,70 @@ void *IriBaseNodeDriver<Driver>::mainThread(void *param)
 template <class Driver>
 void IriBaseNodeDriver<Driver>::postOpenHook(void)
 {
+  ROS_DEBUG("IriBaseNodeDriver::postOpenHook");
+
   //set hardware id with driver id
   this->diagnostic_.setHardwareID(this->driver_.getID());
 
   postNodeOpenHook();
 }
 
+template <class Driver>
+void IriBaseNodeDriver<Driver>::preCloseHook(void)
+{
+  ROS_INFO("IriBaseNodeDriver::preCloseHook");
+  preNodeCloseHook();
+}
+
+template <class Driver>
+void IriBaseNodeDriver<Driver>::preNodeCloseHook(void)
+{
+}
+    
 template <class Driver>
 void IriBaseNodeDriver<Driver>::addDiagnostics(void)
 {
+  ROS_DEBUG("IriBaseNodeDriver::addDiagnostics");
   addNodeDiagnostics();
 }
 
 template <class Driver>
 void IriBaseNodeDriver<Driver>::addOpenedTests(void)
 {
+  ROS_DEBUG("IriBaseNodeDriver::addOpenedTests");
   addNodeOpenedTests();
 }
 
 template <class Driver>
 void IriBaseNodeDriver<Driver>::addStoppedTests(void)
 {
+  ROS_DEBUG("IriBaseNodeDriver::addStoppedTests");
   addNodeStoppedTests();
 }
 
 template <class Driver>
 void IriBaseNodeDriver<Driver>::addRunningTests(void)
 {
+  ROS_DEBUG("IriBaseNodeDriver::addRunningTests");
   addNodeRunningTests();
 }
 
 template <class Driver>
 void IriBaseNodeDriver<Driver>::reconfigureHook(int level)
 {
+  ROS_DEBUG("IriBaseNodeDriver::reconfigureHook");
   reconfigureNodeHook(level);
 }
 
 template <class Driver>
 IriBaseNodeDriver<Driver>::~IriBaseNodeDriver()
 {
+  ROS_DEBUG("IriBaseNodeDriver::Destrcutor");
   try
   {
-    std::cout << "Node destructor" << std::endl;
-    std::cout << "try_enter=" << this->driver_.try_enter() << std::endl;
-
-    //unlock access to driver
+    //try access to driver to assert it is
+    //unlock before its destruction
+    this->driver_.try_enter();
     this->driver_.unlock();
   }
   catch(CException e)

src/iri_base_driver.cpp

@@ -34,6 +34,7 @@ void IriBaseDriver::doOpen(void)
 
 void IriBaseDriver::doClose(void)
 {
+  this->preCloseHook();
   if(closeDriver()) this->state_ = CLOSED;
 }
 
@@ -52,6 +53,11 @@ std::string IriBaseDriver::getID(void)
   return driver_id_;
 }
 
+void IriBaseDriver::setPreCloseHook(hookFunction f)
+{
+  preCloseHook = f;
+}
+
 IriBaseDriver::~IriBaseDriver()
 {
 }