ROSiFace.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------

#ifndef ROSIFACE_H
#define ROSIFACE_H

#include "includeROS.h"
#include <sstream>
#include <typeinfo>
#include "MIPMatrix.h"
#include <LogTrace.h>
#include <deque>



#include <boost/bind.hpp>




using namespace MipBaselib;

namespace MipBaselib{
 /* @{ */
 
 enum ROSiFaceOpeningModes{
  ROSIFACE_R,
  ROSIFACE_W,
  ROSIFACE_RW
 };
 
 
 class ROSiFaceBoss{
  public:
   virtual bool updateMessage()=0;/*{
    return false;
   }*/
  
 };
 
 template <class msg_type, typename msg_type_ptr>
 class ROSiFace: public ROSiFaceBoss{
  public:
   
   ROSiFace(const string& topicName, ros::NodeHandle *_node, ROSiFaceOpeningModes op, int queue=1, int bufferDim=1);
   
   ~ROSiFace();
   
   bool sendMessage(const msg_type& message);
   
   bool updateMessage();
   
   bool getMessage(msg_type &msg);
   
   void getMsgList(deque<msg_type> &listMsg);
   
  private:
   
   // properties of this topic
   string _topicName;
   ROSiFaceOpeningModes _openingMode;
   int _queueLength;
   
   // topic variables
   ros::Publisher*  _talkerA;      
   ros::Subscriber*  _subscriberA;
   
   int storedMsgSize;
   
   msg_type _lastMessage;
   deque<msg_type> _storedMessages;
   
   bool _busy;
   
   
   void chatterCallback(const msg_type_ptr& msg);
 };
 
 
 
 template <class msg_type, typename msg_type_ptr> ROSiFace<msg_type, msg_type_ptr>::ROSiFace(const string& topicName, ros::NodeHandle *_node, ROSiFaceOpeningModes op, int queue,int bufferDim)
 {
  _topicName = topicName;
  _openingMode = op;
  _queueLength = queue;
  
  if (op == ROSIFACE_W || op == ROSIFACE_RW){
   cout << "ROSiFace: open a Publisher on the topic " << _topicName << endl;
   this->_talkerA = new ros::Publisher;
   *this->_talkerA = _node->advertise<msg_type>(_topicName, _queueLength);
  }
  if (op == ROSIFACE_R || op == ROSIFACE_RW){
   storedMsgSize  = bufferDim;
   
   cout << "ROSiFace: open a Subscriber on the topic " << _topicName << endl;
   this->_subscriberA = new ros::Subscriber;
   ros::SubscribeOptions so = ros::SubscribeOptions::create<msg_type>(_topicName, _queueLength, boost::bind(&ROSiFace::chatterCallback, this, _1), ros::VoidPtr(), NULL);
   *this->_subscriberA = _node->subscribe(so);
  }
  _busy = false;
 }
 
 
 
 template <class msg_type, typename msg_type_ptr> ROSiFace<msg_type, msg_type_ptr>::ROSiFace::~ROSiFace() {
  if (_openingMode == ROSIFACE_W || _openingMode == ROSIFACE_RW){
   cout << "RosTalker:: Destruct a publisher" << endl;
   delete this->_talkerA;
  }
  if (_openingMode == ROSIFACE_R || _openingMode == ROSIFACE_RW){
   cout << "RosTalker:: Destruct a subscriber" << endl;
   delete this->_subscriberA;
  }
 }
 
 
 
 
  template <class msg_type, typename msg_type_ptr> bool ROSiFace<msg_type, msg_type_ptr>::ROSiFace::sendMessage(const msg_type& message) {
  if (_openingMode == ROSIFACE_R){
   LogTrace::fatal("tentative of writing a read-only ROSiFace.");
   return false;
  }
  if(ros::ok()) {
   this->_talkerA->publish(message);
   return true;
  }
  else {
   cout<< "WARNING! RosTalker::sendRosMsg() ros::ok() is FALSE" << endl;
   return false;
  }
 }
 
 
 template <class msg_type, typename msg_type_ptr> bool ROSiFace<msg_type, msg_type_ptr>::ROSiFace::updateMessage() {
  if (_openingMode == ROSIFACE_W){
   LogTrace::fatal("tentative of reading a write-only ROSiFace.");
   return false;
  }
  if(ros::ok()) {
   ros::spinOnce();
   return true;
  }
  else {
   cout<< "WARNING! RosTalker::sendRosMsg() ros::ok() is FALSE" << endl;
   return false;
  }
 }
 
 template <class msg_type, typename msg_type_ptr> bool ROSiFace<msg_type, msg_type_ptr>::ROSiFace::getMessage(msg_type& msg){
  if (_openingMode == ROSIFACE_W){
   LogTrace::fatal("tentative of reading a write-only ROSiFace.");
   return false;
  }
  if(ros::ok()) {
   msg = _lastMessage;
   return true;
  }else 
   return false;
 }
 
 template <class msg_type, typename msg_type_ptr> void ROSiFace<msg_type, msg_type_ptr>::ROSiFace::getMsgList(deque< msg_type >& listMsg){
  listMsg.clear();
  while(_busy){usleep(100);}
  _busy = true;
  listMsg = _storedMessages;
  _busy = false;
 }
 
 
 template <class msg_type, typename msg_type_ptr> void ROSiFace<msg_type, msg_type_ptr>::ROSiFace::chatterCallback(const msg_type_ptr& msg) {
  _lastMessage = *msg;
  while(_busy){usleep(100);}
  _busy = true;
  int listBufferSize = _storedMessages.size();
  if(listBufferSize<storedMsgSize){
   _storedMessages.push_back(*msg);
  }else{
   _storedMessages.pop_front();
   _storedMessages.push_back(*msg);
  }
  _busy = false;
 }
 
 
 
 
 
 /* @} */
}
#endif // ROSIFACE_H