CommunicationGateway.cpp

Go to the documentation of this file.

/*
Copyright 2007, 2008, 2009, 2010, 2011 Instituto de Sistemas e Robotica, Instituto Superior Tecnico

This file is part of MeRMaID.

MeRMaID 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.

MeRMaID 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 MeRMaID.  If not, see <http://www.gnu.org/licenses/>.
*/



/**
 * @Filename CommunicationGateway.cpp
 * @Description CommunicationGateway implementation
 * @Status Work in Progress
 * @Version $Id: CommunicationGateway.cpp 1 2011-03-04 18:13:18Z jreis $
 * @Maintainer
 */


#include "config.h"

#include <CommunicationGateway.hpp>
#include <DataFactory.hpp>
#include <DataFeedDescription.hpp>
#include <Exception.hpp>
#include <Entity.hpp>
#include <EntityDescription.hpp>
#include <ServiceInterfaceDescription.hpp>
#include <ServiceInstanceDescription.hpp>
#include <ServiceTypeDescription.hpp>


#include <iostream>

#include <sstream>

// YARP stuff
#include <yarp/os/all.h>


// YARP-specific communication stuff
#include <YarpDataFeedReporter.hpp>
#include <YarpServiceAsynchInputPort.hpp>
#include <YarpServiceAsynchRequestPort.hpp>
#include <YarpServiceAsynchRequestType.hpp>

using namespace mermaid::support::communication;

using mermaid::support::data::DataFactory;
using mermaid::support::errorhandling::Exception;
using mermaid::support::service::DataFeedDescription;
using mermaid::support::service::Entity;
using mermaid::support::service::EntityDescription;
using mermaid::support::service::ServiceInterfaceDescription;
using mermaid::support::service::ServiceInstanceDescription;
using mermaid::support::service::ServiceTypeDescription;
using std::string;
using yarp::os::Network;
using yarp::os::Port;

CommunicationGateway::CommunicationGateway()
{

#ifdef USE_COMM
  executingSetupTask_ = false;
#endif
  
}; // CommunicationGateway()

CommunicationGateway::~CommunicationGateway()
{
  //std::cerr << "CommunicationGateway::~CommunicationGateway()" << std::endl;
}; // ~CommCommunicationGateway()

void CommunicationGateway::registerService (shared_ptr<Service> s)
{
  //std::cerr << "CommunicationGateway::registerService()" << std::endl;
  
  shared_ptr<ServiceInstanceDescription> serviceInstanceDescription = s->getServiceInstanceDescription();
  std::string serviceInstanceName = serviceInstanceDescription->getServiceInstanceName();
  std::string entityName = s->getEntity()->getEntityDescription()->getEntityName();
  
  //create input port for service
  shared_ptr<YarpServiceAsynchInputPort> servicePort (new YarpServiceAsynchInputPort (s));
  
  //put it in the input port map
  serviceAsynchInputPorts_[serviceInstanceName] = servicePort;
  
  //go through all service types
  std::vector<shared_ptr<ServiceTypeDescription> > serviceTypes = s->getServiceInstanceDescription()->getServiceTypeDescriptions();
  
  std::vector<shared_ptr<ServiceTypeDescription> >::iterator typesIterator;
  
  for (typesIterator = serviceTypes.begin(); typesIterator != serviceTypes.end(); typesIterator++) {
    shared_ptr<ServiceTypeDescription> serviceType = *typesIterator;
    
    //create data feed ports for service
    std::vector< shared_ptr<DataFeedDescription> > dataFeedDescriptions = serviceType->getAllDataFeedDescriptions();
    
    std::vector< shared_ptr<DataFeedDescription> >::iterator dataFeedIt;
    
    for (dataFeedIt = dataFeedDescriptions.begin(); dataFeedIt != dataFeedDescriptions.end(); dataFeedIt++) {
      shared_ptr<DataFeedDescription> dataFeedDescription = *dataFeedIt;
      std::string dataFeedName = dataFeedDescription->getDataFeedName();
      std::cerr << "CommunicationGateway::registerService : adding DataFeed port for DataFeed with dataFeedName=\"" << dataFeedName << "\" to Service with entityName=\"" << entityName << "\", serviceInstanceName=\"" << serviceInstanceName << "\"" << std::endl;
      
      shared_ptr<YarpDataFeedOutputPort> datafeedOutputPort (new YarpDataFeedOutputPort (entityName, serviceInstanceName, dataFeedName));
      
      addYarpDataFeedOutputPort (datafeedOutputPort);
    }
  }
};

void CommunicationGateway::sendServiceRequest (shared_ptr<Service> requesterService, shared_ptr<ServiceRequest> request)
{
  //std::cerr << "CommunicationGateway::sendServiceRequest()" << std::endl;
  
  std::string targetEntityName = request->getTargetEntityName();
  std::string targetServiceName = request->getTargetServiceName();
  
  // get port
  shared_ptr<YarpServiceAsynchRequestPort> requestPort = getYarpServiceAsynchRequestPort (requesterService, request);
  
  //register request
  registerServiceRequest (request);
  
  //send request
  YarpServiceAsynchRequestType& ysart = requestPort->prepare();
  ysart.convertFromServiceRequest (*request);
  requestPort->writeStrict();
  request->notifySent();
  
}; // sendServiceRequest()


void CommunicationGateway::sendServiceReply (shared_ptr<Service> targetService, std::string requesterEntityName, std::string requesterServiceName, shared_ptr<ServiceReply> sr)
{
  //std::cerr << "CommunicationGateway::sendServiceReply()" << std::endl;
  
  //get port
  shared_ptr<YarpServiceAsynchOutputPort> outputPort = getYarpServiceAsynchOutputPort (targetService, requesterEntityName, requesterServiceName);
  
  //send reply
  YarpServiceAsynchReplyType& ysart = outputPort->prepare();
  ysart.convertFromServiceReply (*sr);
  outputPort->writeStrict();
  
  
}; // sendServiceAsynchReply()


shared_ptr<YarpServiceAsynchRequestPort> CommunicationGateway::getYarpServiceAsynchRequestPort (shared_ptr<Service> requesterService, shared_ptr<ServiceRequest> request)
{
  std::string targetEntityName = request->getTargetEntityName();
  std::string targetServiceName = request->getTargetServiceName();
  std::string requesterEntityName = requesterService->getServiceInstanceDescription()->getEntityDescription()->getEntityName();
  std::string requesterServiceName = requesterService->getServiceInstanceDescription()->getServiceInstanceName();
  //! @TODO port map should depend on the targetEntityName also.
  std::pair<std::string, std::string> requesterPair (requesterEntityName, requesterServiceName);
  std::pair<std::string, std::string> targetPair (targetEntityName, targetServiceName);
  shared_ptr<YarpServiceAsynchRequestPort> requestPort = serviceAsynchRequestPorts_[requesterPair][targetPair];
  
  if (requestPort == false) {
    //we need to create the ports
    
    //create request port
    requestPort = shared_ptr<YarpServiceAsynchRequestPort> (new YarpServiceAsynchRequestPort (requesterService, request));
    std::map<std::pair<std::string, std::string>, shared_ptr<YarpServiceAsynchRequestPort> > requesterRequestPorts = serviceAsynchRequestPorts_[requesterPair];
    
    requesterRequestPorts[targetPair] = requestPort;
    serviceAsynchRequestPorts_[requesterPair] = requesterRequestPorts;
    
    //lets create the reply port also
    shared_ptr<YarpServiceAsynchReplyPort> replyPort (new YarpServiceAsynchReplyPort (requesterService, targetEntityName, targetServiceName));
    
    std::map<std::pair<std::string, std::string>, shared_ptr<YarpServiceAsynchReplyPort> > requesterReplyPorts = serviceAsynchReplyPorts_[requesterPair];
    
    requesterReplyPorts[targetPair] = replyPort;
    serviceAsynchReplyPorts_[requesterPair] = requesterReplyPorts;
    
  }
  
  return requestPort;
  
}; // getYarpServiceAsynchRequestPort()


shared_ptr<YarpServiceAsynchReplyPort> CommunicationGateway::getYarpServiceAsynchReplyPort (std::string targetEntityName, std::string targetServiceName, std::string requesterEntityName, std::string requesterServiceName)
{
  std::pair<std::string, std::string> requesterPair (requesterEntityName, requesterServiceName);
  std::pair<std::string, std::string> targetPair (targetEntityName, targetServiceName);
  return serviceAsynchReplyPorts_[requesterPair][targetPair];
}; // getYarpServiceAsynchReplyPort


shared_ptr<YarpServiceAsynchOutputPort> CommunicationGateway::getYarpServiceAsynchOutputPort (shared_ptr<Service> targetService, std::string requesterEntityName, std::string requesterServiceName)
{
  std::string targetEntityName = targetService->getServiceInstanceDescription()->getEntityDescription()->getEntityName();
  std::string targetServiceName = targetService->getServiceInstanceDescription()->getServiceInstanceName();
  std::pair<std::string, std::string> requesterPair (requesterEntityName, requesterServiceName);
  std::pair<std::string, std::string> targetPair (targetEntityName, targetServiceName);
  
  shared_ptr<YarpServiceAsynchOutputPort> outputPort = serviceAsynchOutputPorts_[requesterPair][targetPair];
  
  if (outputPort == false) {
    //we need to create the port
    
    //create output port
    outputPort = shared_ptr<YarpServiceAsynchOutputPort> (new YarpServiceAsynchOutputPort (targetService, requesterEntityName, requesterServiceName));
    std::map<std::pair<std::string, std::string>, shared_ptr<YarpServiceAsynchOutputPort> > requesterOutputPorts = serviceAsynchOutputPorts_[requesterPair];
    
    requesterOutputPorts[targetPair] = outputPort;
    serviceAsynchOutputPorts_[requesterPair] = requesterOutputPorts;
  }
  
  return outputPort;
}; // getYarpServiceAsynchOutputPort()


void CommunicationGateway::sendToDataFeed (std::string entityName, std::string serviceName, std::string dataFeedName, shared_ptr<DataBox> dataBox)
{
  shared_ptr<YarpDataFeedOutputPort> port = getYarpDataFeedOutputPort (entityName, serviceName, dataFeedName);
  Bottle& b = port->prepare();
  b.clear();
  DataFactory::writeDataBoxToBottle (dataBox, b);
  port->writeStrict();
}; // sendDataToDataFeed()

void CommunicationGateway::addYarpDataFeedOutputPort (shared_ptr<YarpDataFeedOutputPort> outputPort)
{
  std::string entityName = outputPort->getEntityName();
  std::string serviceName = outputPort->getServiceInstanceName();
  std::string dataFeedName = outputPort->getDataFeedName();
  
  std::map<std::string, std::map<std::string, shared_ptr<YarpDataFeedOutputPort> > > entityFeedPorts = dataFeedOutputPorts_[entityName];
  
  std::map<std::string, shared_ptr<YarpDataFeedOutputPort> > serviceFeedPorts = entityFeedPorts[serviceName];
  
  serviceFeedPorts[dataFeedName] = outputPort;
  entityFeedPorts[serviceName] = serviceFeedPorts;
  dataFeedOutputPorts_[entityName] = entityFeedPorts;
}; // addYarpDataFeedOutputPort()

shared_ptr<YarpDataFeedOutputPort> CommunicationGateway::getYarpDataFeedOutputPort (std::string entityName, std::string serviceName, std::string feedName)
{
  shared_ptr<YarpDataFeedOutputPort> port = dataFeedOutputPorts_[entityName][serviceName][feedName];
  
  if (port == false) {
    // we need to create the port
    std::stringstream ss;
    ss << "CommunicationGateway::getYarpDataFeedOutputPort() : no data feed port exists with entityName=\"" << entityName << "\", serviceName=\"" << serviceName << "\", feedName=\"" << feedName << "\"";
    throw Exception (ss.str());
  }
  
  return port;
  
}; // getYarpDataFeedOutputPort()

void CommunicationGateway::addYarpDataFeedInputPort (shared_ptr<YarpDataFeedInputPort> inputPort)
{
  std::string consumerEntityName = inputPort->getConsumerService()->getEntity()->getEntityDescription()->getEntityName();
  std::string consumerServiceName = inputPort->getConsumerService()->getServiceInstanceDescription()->getServiceInstanceName();
  std::string producerEntityName = inputPort->getProducerEntityName();
  std::string producerServiceName = inputPort->getProducerServiceName();
  std::string dataFeedName = inputPort->getDataFeedName();
  
  dataFeedInputPorts_[consumerEntityName][consumerServiceName][producerEntityName][producerServiceName][dataFeedName] = inputPort;
}; // addYarpDataFeedInputPort()

shared_ptr<YarpDataFeedInputPort> CommunicationGateway::getYarpDataFeedInputPort (std::string consumerEntityName, std::string consumerServiceName, std::string producerEntityName, std::string producerServiceName, std::string dataFeedName)
{
  shared_ptr<YarpDataFeedInputPort> port = dataFeedInputPorts_[consumerEntityName][consumerServiceName][producerEntityName][producerServiceName][dataFeedName];
  
  if (port == false) {
    //error
    std::stringstream ss;
    ss << "CommunicationGateway::getYarpDataFeedInputPort() : no data feed port exists with consumerEntityName=\"" << consumerEntityName << "\", consumerServiceName=\"" << consumerServiceName << "\", producerEntityName=\"" << producerEntityName << "\", producerServiceName=\"" << producerServiceName << "\", dataFeedName=\"" << dataFeedName << "\"";
    throw Exception (ss.str());
  }
  
  return port;
}; // getYarpDataFeedInputPort()

void CommunicationGateway::connectToDataFeed (std::string producerEntityName, std::string producerServiceName, std::string datafeedName, shared_ptr<Service> consumerService)
{
  //std::cerr << "CommunicationGateway::connectToDataFeed()" << std::endl;
  
  shared_ptr<YarpDataFeedInputPort> port (new YarpDataFeedInputPort (producerEntityName, producerServiceName, datafeedName, consumerService));
  
  this->addYarpDataFeedInputPort (port);
  
  
}; // connectToDataFeed()

void CommunicationGateway::registerServiceRequest (shared_ptr<ServiceRequest> request)
{
  std::string requesterEntityName = request->getRequesterEntityName();
  std::string requesterServiceName = request->getRequesterServiceName();
  int reqId = request->getRequestId();
  
  std::map<std::string, std::map<int, shared_ptr<ServiceRequest> > > entityMap = serviceRequests_[requesterEntityName];
  
  std::map<int, shared_ptr<ServiceRequest> > serviceMap = entityMap[requesterServiceName];
  
  //test if we already have a request registered with the same ID, for the same entity and service
  if (serviceMap.find (reqId) != serviceMap.end()) {
    std::stringstream ss;
    ss << "CommunicationGateway::registerServiceRequest : ServiceRequest already registered with the following identification: EntityName=" << requesterEntityName << " ,ServiceName=" << requesterServiceName << " ,RequestId=" << reqId;
    throw Exception (ss.str());
  }
  else {
    // lets insert the service request to the map
    //std::cerr << "CommunicationGateway::registerService : EntityName=" << requesterEntityName << " ,ServiceName=" << requesterServiceName << " ,RequestId=" << reqId << std::endl;
    serviceMap[reqId] = request;
    entityMap[requesterServiceName] = serviceMap;
    serviceRequests_[requesterEntityName] = entityMap;
  }
}; // registerServiceRequest

shared_ptr<ServiceRequest> CommunicationGateway::getServiceRequest (std::string requesterEntityName, std::string requesterServiceName, int requestId)
{
  std::map<std::string, std::map<int, shared_ptr<ServiceRequest> > > entityMap = serviceRequests_[requesterEntityName];
  std::map<int, shared_ptr<ServiceRequest> > serviceMap = entityMap[requesterServiceName];
  
  if (serviceMap.find (requestId) == serviceMap.end()) {
    std::stringstream ss;
    ss << "CommunicationGateway::getServiceRequest : ServiceRequest with the following identification: EntityName=" << requesterEntityName << " ,ServiceName=" << requesterServiceName << " ,RequestId=" << requestId << " does not exist.";
    throw Exception (ss.str());
  }
  else {
    return serviceMap[requestId];
  }
}; // getServiceRequest

void CommunicationGateway::unregisterServiceRequest (std::string requesterEntityName, std::string requesterServiceName, int requestId)
{
  //! @TODO implement CommunicationGateway::unregisterServiceRequest
}; // unregisterServiceRequest

#ifdef USE_COMM

void CommunicationGateway::addDataFeedSetupTask (shared_ptr<Task> dst)
{
  std::cout << "CommunicationGateway::addDataFeedSetupTask()" << std::endl;
  dataFeedSetupTasks_.push_back (dst);
}

bool CommunicationGateway::isNewDataFeedSetupTaskAvailable()
{
  if (executingSetupTask_ == true) {
    return false;
  }
  else if (dataFeedSetupTasks_.size() > 0) {
    return true;
  }
  else {
    return false;
  }
  
}

shared_ptr<Task> CommunicationGateway::getDataFeedSetupTask()
{
  shared_ptr<Task> t = dataFeedSetupTasks_[0];
  dataFeedSetupTasks_.erase (dataFeedSetupTasks_.begin());
  executingSetupTask_ = true;
  return t;
}

void CommunicationGateway::notifyDataFeedSetupEnded()
{
  executingSetupTask_ = false;
}

#endif