iri_base_driver_node.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author Sergi Hernandez & Joan Perez
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#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 <diagnostic_updater/diagnostic_updater.h>
#include "iri_base_driver.h"

namespace iri_base_driver
{

template <class Driver>
class IriBaseNodeDriver : public driver_base::DriverNode<Driver>
{
  protected:
    pthread_t thread;

    void postOpenHook(void);

    virtual void postNodeOpenHook(void) = 0;

    void preCloseHook(void);

    virtual void preNodeCloseHook(void);

    ros::NodeHandle public_node_handle_;

    ros::Rate loop_rate_;

    static const unsigned int DEFAULT_RATE = 10; //[Hz]

  public:
    IriBaseNodeDriver(ros::NodeHandle& nh);

    ~IriBaseNodeDriver();

    void addDiagnostics(void);

    void addOpenedTests(void);

    void addStoppedTests(void);

    void addRunningTests(void);

    void reconfigureHook(int level);

  protected:
    virtual void addNodeDiagnostics(void) = 0;

    virtual void addNodeOpenedTests(void) = 0;

    virtual void addNodeStoppedTests(void) = 0;

    virtual void addNodeRunningTests(void) = 0;

    static void *mainThread(void *param);

    virtual void mainNodeThread(void) = 0;

    virtual void reconfigureNodeHook(int level) = 0;
};

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)
{
  ROS_DEBUG("IriBaseNodeDriver::Constructor");
  this->driver_.setPostOpenHook(boost::bind(&IriBaseNodeDriver::postOpenHook, this));
  this->driver_.setPreCloseHook(boost::bind(&IriBaseNodeDriver::preNodeCloseHook, this));

  // create the status thread
  pthread_create(&this->thread,NULL,this->mainThread,this);
  
  // initialize class attributes
}

template <class Driver>
void *IriBaseNodeDriver<Driver>::mainThread(void *param)
{
  ROS_DEBUG("IriBaseNodeDriver::mainThread");

  IriBaseNodeDriver<Driver> *iriNode = (IriBaseNodeDriver<Driver> *)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>::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 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);
}

}

#endif