mq_channel_element.hpp

Go to the documentation of this file.

/* 
 *  mq_channel_element.hpp - micros message queue element for transport channel
 *  Copyright (C) 2015 Zaile Jiang
 *  
 *  This program 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 2
 *  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 General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/
#ifndef MICROSRTT_MQ_CHANNEL_ELEMENT_HPP
#define MICROSRTT_MQ_CHANNEL_ELEMENT_HPP

#include "ros/serialization.h"
#include "micros_rtt/oro/mq_send_recv.hpp"
#include "micros_rtt/oro/channel_element.hpp"
#include "micros_rtt/oro/data_lockfree.hpp"
#include <stdexcept>

namespace micros_rtt
{
using namespace ros::serialization;
template<typename M>
class MQChannelElement: public ChannelElement<M>, public MQSendRecv
{
  typename DataObjectLockFree<M>::shared_ptr read_sample;
  typename DataObjectLockFree<M>::shared_ptr write_sample;

  
  SerializedMessage read_m;   

public:
  MQChannelElement(ConnectionBasePtr connection, bool is_sender)
      : MQSendRecv(), 
        read_sample(new DataObjectLockFree<M>(M()))
        , write_sample(new DataObjectLockFree<M>(M()))

  {
    setupStream(connection, sizeof(M), is_sender);
  }

  ~MQChannelElement() 
  {
    cleanupStream();
  }

  virtual bool inputReady() 
  {
    if ( mqReady(this) ) 
    {
//      typename ChannelElement<M>::shared_ptr output = this->getOutput();
//      assert(output);
//      output->data_sample(read_sample->Get());
      return true;
    }
    return false;
  }

  virtual bool data_sample(typename ChannelElement<M>::param_t sample)
  {
    // send initial data sample to the other side using a plain write.
    if (mis_sender) 
    {
      typename ChannelElement<M>::shared_ptr output =
          this->getOutput();
 
      write_sample->data_sample(sample);
      // update MQSendRecv buffer:
      // mqNewSample(sizeof(sample));
      // return mqWrite(sample);
      
      return true;
    }
    return false;
  }

//  bool signal()
//  {
//    // copy messages into channel
//    if (mis_sender) 
//    {
//      // this read should always succeed since signal() means
//      // 'data available in a data element'.
//      typename ChannelElement<T>::shared_ptr input =
//          this->getInput();
//      if( input && input->read(read_sample->set(), false) == NewData )
//        return this->write(read_sample->rvalue());
//    } 
//    else 
//    {
//      typename ChannelElement<T>::shared_ptr output =
//          this->getOutput();
//      if (output && mqRead())
//        return output->write(read_sample->rvalue());
//      ;
//    }
//    return false;
//  }

  FlowStatus read(typename ChannelElement<M>::reference_t sample, bool copy_old_data)
  {
    ROS_DEBUG("micros message queue read.");
    //messages got from message queue need to be deserialize.
        
    if (mqRead(read_m))
    {
      deserializeMessage<M>(read_m, sample);
      return NewData;
    }
    
    return NoData;
  }

  bool write(typename ChannelElement<M>::param_t sample)
  {
    //messages sent through message queue need to be serialized first.
    SerializedMessage m = serializeMessage<M>(sample);
    return mqWrite(m);
  }

};

}

#endif