iri_base_algorithm.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 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_ALGORITHM_H
#define _IRI_BASE_ALGORITHM_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>

// diagnostic updater include
#include <diagnostic_updater/diagnostic_updater.h>

namespace algorithm_base
{

class AbstractAlgorithmNode
{
  protected:
    static int ctrl_c_hit_count_;
    
    static void hupCalled(int sig)
    {
      ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
    }
    
    static void sigCalled(int sig)
    {
      ctrl_c_hit_count_++;
    }
};

template <class Algorithm>
class IriBaseAlgorithm : public AbstractAlgorithmNode
{
  public:
    typedef typename Algorithm::Config Config;

  protected:
    pthread_t thread;

    Algorithm alg_;

    ros::NodeHandle public_node_handle_;

    ros::NodeHandle private_node_handle_;

    ros::Rate loop_rate_;

    static const unsigned int DEFAULT_RATE = 10; //[Hz]
    
    diagnostic_updater::Updater diagnostic_;

  public:
    IriBaseAlgorithm(void);

    ~IriBaseAlgorithm(void);

    int spin(void);

  private:
    boost::shared_ptr<boost::thread> ros_thread_;

    dynamic_reconfigure::Server<Config> dsrv_;

  protected:
    void reconfigureCallback(Config &config, uint32_t level);

    virtual void node_config_update(Config &config, uint32_t level) = 0;
   
    void addDiagnostics(void);

    virtual void addNodeDiagnostics(void) = 0;

//     void addOpenedTests(void);
//     void addStoppedTests(void);
//     void addRunningTests(void);
//     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;
};


template <class Algorithm>
IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : 
  public_node_handle_(ros::this_node::getName()),
  private_node_handle_("~"), 
  loop_rate_(DEFAULT_RATE),
  diagnostic_(),
  dsrv_(private_node_handle_)
{
  ROS_DEBUG("IriBaseAlgorithm::Constructor");

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

//   // assign callback to dynamic reconfigure server
//   dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));

}

template <class Algorithm>
IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
{
  ROS_DEBUG("IriBaseAlgorithm::Destructor");
  pthread_cancel(this->thread);
  pthread_join(this->thread,NULL);
}

template <class Algorithm>
void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
{
  ROS_DEBUG("IriBaseAlgorithm::reconfigureCallback");
  this->node_config_update(config, level);
  this->alg_.config_update(config, level);
}

template <class Algorithm>
void IriBaseAlgorithm<Algorithm>::addDiagnostics(void)
{
  ROS_DEBUG("IriBaseAlgorithm::addDiagnostics");
  addNodeDiagnostics();
}

template <class Algorithm>
void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
{
  ROS_DEBUG("IriBaseAlgorithm::mainThread");

  // retrieve base algorithm class
  IriBaseAlgorithm *iriNode = (IriBaseAlgorithm *)param;

  while(ros::ok() && !ctrl_c_hit_count_)
  {
    // run node stuff
    iriNode->mainNodeThread();

    // sleep remainder time
    iriNode->loop_rate_.sleep();
  }

  // kill main thread
  pthread_exit(NULL);
}

template <class Algorithm>
int IriBaseAlgorithm<Algorithm>::spin(void)
{
  ROS_DEBUG("IriBaseAlgorithm::spin");

  // initialize diagnostics
  this->diagnostic_.setHardwareID("none");
  this->addDiagnostics();
  
  // launch ros spin in different thread
  this->ros_thread_.reset( new boost::thread(boost::bind(&ros::spin)) );
  assert(ros_thread_);

  // assign callback to dynamic reconfigure server
  this->dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));

  while(ros::ok() && !ctrl_c_hit_count_)
  {
    // update diagnostics
    this->diagnostic_.update();
    
    ros::WallDuration(0.1).sleep();
  }

  // stop ros
  ros::shutdown();

  // kill ros spin thread
  this->ros_thread_.reset();

  return 0;
}

// definition of the static Ctrl+C counter
int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;

template <class AlgImplTempl>
int main(int argc, char **argv, std::string node_name)
{
  ROS_DEBUG("IriBaseAlgorithm::%s Launched", node_name.c_str());

  // ROS initialization
  ros::init(argc, argv, node_name);

  // allow Ctrl+C management
  signal(SIGHUP, &AbstractAlgorithmNode::hupCalled);

  // define and launch generic algorithm implementation object
  AlgImplTempl algImpl;
  algImpl.spin();

  return 0;
}

}
#endif