iri_base_driver_node.h
Go to the documentation of this file.
00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
00002 // Author Sergi Hernandez & Joan Perez
00003 // All rights reserved.
00004 //
00005 // This file is part of iri-ros-pkg
00006 // iri-ros-pkg is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #ifndef _IRI_BASE_DRIVER_NODE_H
00020 #define _IRI_BASE_DRIVER_NODE_H
00021 
00022 // ros base driver node, diagnostics, and iri_ros driver
00023 #include <driver_base/driver_node.h>
00024 #include <diagnostic_updater/diagnostic_updater.h>
00025 #include "iri_base_driver.h"
00026 
00027 namespace iri_base_driver
00028 {
00029 
00060 template <class Driver>
00061 class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
00062 {
00063   protected:
00072     pthread_t thread;
00073 
00080     void postOpenHook(void);
00081 
00088     virtual void postNodeOpenHook(void) = 0;
00089 
00094     void preCloseHook(void);
00095 
00100     virtual void preNodeCloseHook(void);
00101 
00109     ros::NodeHandle public_node_handle_;
00110 
00119     ros::Rate loop_rate_;
00120 
00127     static const unsigned int DEFAULT_RATE = 10; //[Hz]
00128 
00129   public:
00139     IriBaseNodeDriver(ros::NodeHandle& nh);
00140 
00146     ~IriBaseNodeDriver();
00147 
00154     void addDiagnostics(void);
00155 
00162     void addOpenedTests(void);
00163 
00170     void addStoppedTests(void);
00171 
00178     void addRunningTests(void);
00179 
00186     void reconfigureHook(int level);
00187 
00188   protected:
00195     virtual void addNodeDiagnostics(void) = 0;
00196 
00203     virtual void addNodeOpenedTests(void) = 0;
00204 
00211     virtual void addNodeStoppedTests(void) = 0;
00212 
00219     virtual void addNodeRunningTests(void) = 0;
00220 
00234     static void *mainThread(void *param);
00235 
00242     virtual void mainNodeThread(void) = 0;
00243 
00251     virtual void reconfigureNodeHook(int level) = 0;
00252 };
00253 
00254 template <class Driver>
00255 IriBaseNodeDriver<Driver>::IriBaseNodeDriver(ros::NodeHandle &nh) : 
00256   driver_base::DriverNode<Driver>(nh), 
00257   public_node_handle_(ros::this_node::getName()),
00258   loop_rate_(DEFAULT_RATE)
00259 {
00260   ROS_DEBUG("IriBaseNodeDriver::Constructor");
00261   this->driver_.setPostOpenHook(boost::bind(&IriBaseNodeDriver::postOpenHook, this));
00262   this->driver_.setPreCloseHook(boost::bind(&IriBaseNodeDriver::preNodeCloseHook, this));
00263 
00264   // create the status thread
00265   pthread_create(&this->thread,NULL,this->mainThread,this);
00266   
00267   // initialize class attributes
00268 }
00269 
00270 template <class Driver>
00271 void *IriBaseNodeDriver<Driver>::mainThread(void *param)
00272 {
00273   ROS_DEBUG("IriBaseNodeDriver::mainThread");
00274 
00275   IriBaseNodeDriver<Driver> *iriNode = (IriBaseNodeDriver<Driver> *)param;
00276 
00277   while(ros::ok())
00278   {
00279 //     std::cout << __LINE__ << ": driver state=" << iriNode->driver_.getStateName() << std::endl;
00280     if(iriNode->driver_.isRunning())
00281     {
00282       iriNode->mainNodeThread();
00283 
00284 //       ros::spinOnce();
00285     }
00286     iriNode->loop_rate_.sleep();
00287 
00288   }
00289 
00290   pthread_exit(NULL);
00291 }
00292 
00293 template <class Driver>
00294 void IriBaseNodeDriver<Driver>::postOpenHook(void)
00295 {
00296   ROS_DEBUG("IriBaseNodeDriver::postOpenHook");
00297 
00298   //set hardware id with driver id
00299   this->diagnostic_.setHardwareID(this->driver_.getID());
00300 
00301   postNodeOpenHook();
00302 }
00303 
00304 template <class Driver>
00305 void IriBaseNodeDriver<Driver>::preCloseHook(void)
00306 {
00307   ROS_INFO("IriBaseNodeDriver::preCloseHook");
00308   preNodeCloseHook();
00309 }
00310 
00311 template <class Driver>
00312 void IriBaseNodeDriver<Driver>::preNodeCloseHook(void)
00313 {
00314 }
00315     
00316 template <class Driver>
00317 void IriBaseNodeDriver<Driver>::addDiagnostics(void)
00318 {
00319   ROS_DEBUG("IriBaseNodeDriver::addDiagnostics");
00320   addNodeDiagnostics();
00321 }
00322 
00323 template <class Driver>
00324 void IriBaseNodeDriver<Driver>::addOpenedTests(void)
00325 {
00326   ROS_DEBUG("IriBaseNodeDriver::addOpenedTests");
00327   addNodeOpenedTests();
00328 }
00329 
00330 template <class Driver>
00331 void IriBaseNodeDriver<Driver>::addStoppedTests(void)
00332 {
00333   ROS_DEBUG("IriBaseNodeDriver::addStoppedTests");
00334   addNodeStoppedTests();
00335 }
00336 
00337 template <class Driver>
00338 void IriBaseNodeDriver<Driver>::addRunningTests(void)
00339 {
00340   ROS_DEBUG("IriBaseNodeDriver::addRunningTests");
00341   addNodeRunningTests();
00342 }
00343 
00344 template <class Driver>
00345 void IriBaseNodeDriver<Driver>::reconfigureHook(int level)
00346 {
00347   ROS_DEBUG("IriBaseNodeDriver::reconfigureHook");
00348   reconfigureNodeHook(level);
00349 }
00350 
00351 template <class Driver>
00352 IriBaseNodeDriver<Driver>::~IriBaseNodeDriver()
00353 {
00354   ROS_DEBUG("IriBaseNodeDriver::Destrcutor");
00355   //try access to driver to assert it is
00356   //unlock before its destruction
00357   this->driver_.try_enter();
00358   this->driver_.unlock();
00359   
00360   this->driver_.close();
00361   pthread_cancel(this->thread);
00362   pthread_join(this->thread,NULL);
00363 }
00364 
00365 }
00366 
00367 #endif


iri_base_driver
Author(s): Joan Perez, jnperez at iri.upc.edu
autogenerated on Fri Dec 6 2013 19:58:57