OutPort.h

説明を見る。

// -*- C++ -*-
#ifndef RTC_OUTPORT_H
#define RTC_OUTPORT_H

#include <iostream>
#include <string>

#include <coil/TimeValue.h>
#include <coil/Time.h>
#include <coil/TimeMeasure.h>
#include <coil/OS.h>

#include <rtm/RTC.h>
#include <rtm/Typename.h>
#include <rtm/OutPortBase.h>
#include <rtm/CdrBufferBase.h>
#include <rtm/PortCallback.h>
#include <rtm/OutPortConnector.h>

template <class DataType>
void setTimestamp(DataType& data)
{
      // set timestamp
      coil::TimeValue tm(coil::gettimeofday());
      data.tm.sec  = tm.sec();
      data.tm.nsec = tm.usec() * 1000;
}

namespace RTC
{
  template <class DataType>
  class OutPort
    : public OutPortBase
  {
  public:
    OutPort(const char* name, DataType& value)
      : OutPortBase(name, toTypename<DataType>()), m_value(value),
        m_onWrite(0), m_onWriteConvert(0)
    {
    }
    
    virtual ~OutPort(void)
    {
    }
    
    virtual bool write(DataType& value)
    {
      RTC_TRACE(("DataType write()"));

      if (m_onWrite != NULL)
        {
          (*m_onWrite)(value);
          RTC_TRACE(("OnWrite called"));
        }

      bool result(true);
      std::vector<const char *> disconnect_ids;
      {
        Guard guard(m_connectorsMutex);
        // check number of connectors
        size_t conn_size(m_connectors.size());
        if (!(conn_size > 0)) { return false; }
        
        m_status.resize(conn_size);

        for (size_t i(0), len(conn_size); i < len; ++i)
          {
            ReturnCode ret;
            if (m_onWriteConvert != NULL)
              {
                RTC_DEBUG(("m_connectors.OnWriteConvert called"));
                ret = m_connectors[i]->write(((*m_onWriteConvert)(value)));
              }
            else
              {
                RTC_DEBUG(("m_connectors.write called"));
                ret = m_connectors[i]->write(value);
              }
            m_status[i] = ret;
            if (ret == PORT_OK) { continue; }
      
            result = false;
            const char* id(m_connectors[i]->profile().id.c_str());
            RTC::ConnectorProfile prof(findConnProfile(id));

            if (ret == CONNECTION_LOST)
              {
                RTC_WARN(("connection_lost id: %s", id));
                if (m_onConnectionLost != 0)
                  {
                    (*m_onConnectionLost)(prof);
                  }
                disconnect_ids.push_back(id);
              }
          }
      }
      std::for_each(disconnect_ids.begin(),disconnect_ids.end(),
                    std::bind1st(std::mem_fun(&PortBase::disconnect),this));
      return result;
    }
    
    bool write()
    {
      return write(m_value);
    }
    
    bool operator<<(DataType& value)
    {
      return write(value);
    }

    DataPortStatus::Enum getStatus(int index)
    {
      return m_status[index];
    }
    DataPortStatusList getStatusList()
    {
      return m_status;
    }

    inline void setOnWrite(OnWrite<DataType>* on_write)
    {
      m_onWrite = on_write;
    }
    
    inline void setOnWriteConvert(OnWriteConvert<DataType>* on_wconvert)
    {
      m_onWriteConvert = on_wconvert;
    }
    
  private:
    std::string m_typename;
    DataType& m_value;
    
    OnWrite<DataType>* m_onWrite;
    
    OnWriteConvert<DataType>* m_onWriteConvert;

    coil::TimeMeasure m_cdrtime;

    DataPortStatusList m_status;
  };
}; // namespace RTC

#endif // RTC_OUTPORT_H

---