SpliceDDSTransport.cpp

Go to the documentation of this file.

#include "madara/transport/splice/SpliceDDSTransport.h"
#include "madara/knowledge/UpdateTypes.h"
#include "madara/utility/Utility.h"
#include <time.h>
 
#include <iostream>
#include <sstream>
#include "madara/logger/GlobalLogger.h"
 
namespace logger = madara::logger;
 
const char* madara::transport::SpliceDDSTransport::topic_names_[] = {
    "MADARA_KaRL_Data", "MADARA_KaRL_Control"};
 
/* Array to hold the names for all ReturnCodes. */
const char* madara::transport::SpliceDDSTransport::ret_code_names[] = {
    "DDS_RETCODE_OK", "DDS_RETCODE_ERROR", "DDS_RETCODE_UNSUPPORTED",
    "DDS_RETCODE_BAD_PARAMETER", "DDS_RETCODE_PRECONDITION_NOT_MET",
    "DDS_RETCODE_OUT_OF_RESOURCES", "DDS_RETCODE_NOT_ENABLED",
    "DDS_RETCODE_IMMUTABLE_POLICY", "DDS_RETCODE_INCONSISTENT_POLICY",
    "DDS_RETCODE_ALREADY_DELETED", "DDS_RETCODE_TIMEOUT", "DDS_RETCODE_NO_DATA",
    "DDS_RETCODE_ILLEGAL_OPERATION"};
 
const char* madara::transport::SpliceDDSTransport::partition_ =
    "Madara_knowledge";
 
madara::transport::SpliceDDSTransport::SpliceDDSTransport(const std::string& id,
    knowledge::ThreadSafeContext& context, TransportSettings& config,
    bool launch_transport)
  : madara::transport::Base(id, config, context),
    domain_(0),
    domain_factory_(0),
    domain_participant_(0),
    publisher_(0),
    subscriber_(0),
    datawriter_(0),
    datareader_(0),
    update_writer_(0),
    update_reader_(0),
    update_topic_(0)
{
  // create a reference to the knowledge base for threading
  knowledge_.use(context);
 
  // set the data plane for the read threads
  read_threads_.set_data_plane(knowledge_);
 
  if (launch_transport)
    setup();
}
madara::transport::SpliceDDSTransport::~SpliceDDSTransport()
{
  close();
}
 
void madara::transport::SpliceDDSTransport::close(void)
{
  this->invalidate_transport();
 
  read_threads_.terminate();
 
  read_threads_.wait();
 
  // if (subscriber_.in ())
  //{
  //  subscriber_->delete_datareader (update_reader_);
  //}
 
  // if (publisher_.in ())
  //{
  //  publisher_->delete_datawriter (update_writer_);
  //  publisher_->delete_datawriter (latency_update_writer_);
  //}
 
  // if (domain_participant_.in ())
  //{
  //  domain_participant_->delete_subscriber (subscriber_);
  //  domain_participant_->delete_publisher (publisher_);
  //  domain_participant_->delete_topic (update_topic_);
  //}
 
  // if (domain_factory_.in ())
  //  domain_factory_->delete_participant (domain_participant_);
 
  update_reader_ = 0;
  update_writer_ = 0;
  update_topic_ = 0;
  subscriber_ = 0;
  publisher_ = 0;
  domain_participant_ = 0;
  domain_factory_ = 0;
 
  this->shutting_down_ = false;
}
 
int madara::transport::SpliceDDSTransport::reliability(void) const
{
  return this->settings_.reliability;
}
 
int madara::transport::SpliceDDSTransport::reliability(const int& setting)
{
  return this->settings_.reliability = setting;
}
 
int madara::transport::SpliceDDSTransport::setup(void)
{
  Base::setup();
  DDS::ReturnCode_t status;
 
  this->is_valid_ = false;
 
  // reset the valid setup flag
  // valid_setup_ = false;
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Creating a participant for topic (%s)\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Participant settings are being read from the OSPL_URI environment"
      " variable\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  // get the domain participant factory
  domain_factory_ = DDS::DomainParticipantFactory::get_instance();
  domain_factory_->get_default_participant_qos(part_qos_);
  domain_participant_ = domain_factory_->create_participant(
      domain_, part_qos_, NULL, DDS::STATUS_MASK_NONE);
 
  // if dp == NULL, we've got an error
  if (domain_participant_ == NULL)
  {
    madara_logger_log(context_.get_logger(), logger::LOG_ERROR,
        "\nSpliceDDSTransport::setup:"
        " splice daemon not running. Try 'ospl start'...\n");
 
    return -2;
  }
 
  domain_participant_->get_default_topic_qos(topic_qos_);
 
  if (madara::transport::RELIABLE == this->settings_.reliability)
  {
    topic_qos_.reliability.kind = DDS::RELIABLE_RELIABILITY_QOS;
    topic_qos_.history.depth = this->settings_.queue_length;
    topic_qos_.resource_limits.max_samples_per_instance =
        this->settings_.queue_length;
    topic_qos_.resource_limits.max_samples = this->settings_.queue_length;
    topic_qos_.destination_order.kind =
        DDS::BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS;
    // topic_qos_.
  }
  // topic_qos_.resource_limits.max_samples_per_instance= 10;
  domain_participant_->set_default_topic_qos(topic_qos_);
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Registering type support\n");
 
  //  Register Update type
  status = this->update_type_support_.register_type(
      domain_participant_, "Knowledge::Update");
  if (int ret = check_status(
                    status, "Knowledge::UpdateTypeSupport::register_type") < 0)
    return ret;
 
  //  Register Mutex type
  // status = this->mutex_type_support_.register_type (
  //  domain_participant_, "Knowledge::Mutex");
  // check_status(status, "Knowledge::MutexTypeSupport::register_type");
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Setting up knowledge domain via topic (%s)\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  // Create Update topic
  update_topic_ = domain_participant_->create_topic(
      madara::utility::dds_topicify(settings_.write_domain).c_str(),
      "Knowledge::Update", topic_qos_, NULL, DDS::STATUS_MASK_NONE);
 
  if (int ret =
          check_handle(update_topic_,
              "DDS::DomainParticipant::create_topic (KnowledgeUpdate)") < 0)
    return ret;
 
  // Get default qos for publisher
  status = domain_participant_->get_default_publisher_qos(pub_qos_);
  if (int ret = check_status(status,
                    "DDS::DomainParticipant::get_default_publisher_qos") < 0)
    return ret;
 
  if (madara::transport::RELIABLE == this->settings_.reliability)
  {
    pub_qos_.presentation.access_scope = DDS::TOPIC_PRESENTATION_QOS;
    pub_qos_.presentation.coherent_access = true;
    pub_qos_.presentation.ordered_access = false;
    // topic_qos_.
  }
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Creating publisher for topic (%s)\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  // Create publisher
  pub_qos_.partition.name.length(1);
  pub_qos_.partition.name[0] = DDS::string_dup(partition_);
  publisher_ = domain_participant_->create_publisher(
      pub_qos_, NULL, DDS::STATUS_MASK_NONE);
  if (int ret = check_handle(
                    publisher_, "DDS::DomainParticipant::create_publisher") < 0)
    return ret;
 
  // Create subscriber
  status = domain_participant_->get_default_subscriber_qos(sub_qos_);
  if (int ret = check_status(status,
                    "DDS::DomainParticipant::get_default_subscriber_qos") < 0)
    return ret;
 
  if (madara::transport::RELIABLE == this->settings_.reliability)
  {
    sub_qos_.presentation.access_scope = DDS::TOPIC_PRESENTATION_QOS;
    sub_qos_.presentation.coherent_access = true;
    sub_qos_.presentation.ordered_access = false;
  }
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Creating subscriber for topic (%s)\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  sub_qos_.partition.name.length(1);
  sub_qos_.partition.name[0] = DDS::string_dup(partition_);
  subscriber_ = domain_participant_->create_subscriber(
      //    sub_qos_, &sub_listener_, DDS::DATA_AVAILABLE_STATUS |
      //    DDS::DATA_ON_READERS_STATUS);
      sub_qos_, NULL, DDS::STATUS_MASK_NONE);
  if (int ret = check_handle(subscriber_,
                    "DDS::DomainParticipant::create_subscriber") < 0)
    return ret;
 
  if (!subscriber_ || !publisher_)
  {
    madara_logger_log(context_.get_logger(), logger::LOG_ERROR,
        "SpliceDDSTransport::setup:"
        " pub or sub could not be created. Try 'ospl stop; ospl start'...\n");
 
    return -2;
  }
 
  // Create datawriter
  publisher_->get_default_datawriter_qos(datawriter_qos_);
  publisher_->copy_from_topic_qos(datawriter_qos_, topic_qos_);
 
  if (madara::transport::RELIABLE == this->settings_.reliability)
  {
    madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
        "SpliceDDSTransport::setup:"
        " Enabling reliable transport for (%s) datawriters\n",
        madara::utility::dds_topicify(settings_.write_domain).c_str());
 
    datawriter_qos_.reliability.kind = DDS::RELIABLE_RELIABILITY_QOS;
    datawriter_qos_.history.depth = this->settings_.queue_length;
    datawriter_qos_.resource_limits.max_samples = this->settings_.queue_length;
    datawriter_qos_.resource_limits.max_samples_per_instance =
        this->settings_.queue_length;
    datawriter_qos_.destination_order.kind =
        DDS::BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS;
  }
  else
  {
    madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
        "SpliceDDSTransport::setup:"
        " Enabling unreliable transport for (%s) datawriters\n",
        madara::utility::dds_topicify(settings_.write_domain).c_str());
  }
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Creating datawriter for topic (%s)\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  // Create Update writer
  datawriter_ = publisher_->create_datawriter(
      update_topic_, datawriter_qos_, NULL, DDS::STATUS_MASK_NONE);
  if (int ret = check_handle(datawriter_,
                    "DDS::Publisher::create_datawriter (Update)") < 0)
    return ret;
  update_writer_ =
      dynamic_cast<Knowledge::UpdateDataWriter_ptr>(datawriter_.in());
  if (int ret = check_handle(update_writer_,
                    "Knowledge::UpdateDataWriter_ptr::narrow") < 0)
    return ret;
 
  // Create Latency Update writer for Read Thread
  latencywriter_ = publisher_->create_datawriter(
      update_topic_, datawriter_qos_, NULL, DDS::STATUS_MASK_NONE);
  if (int ret = check_handle(latencywriter_,
                    "DDS::Publisher::create_datawriter (Update)") < 0)
    return ret;
  latency_update_writer_ =
      dynamic_cast<Knowledge::UpdateDataWriter_ptr>(latencywriter_.in());
  if (int ret = check_handle(latency_update_writer_,
                    "Knowledge::UpdateDataWriter_ptr::narrow") < 0)
    return ret;
 
  // Create datareader
  status = subscriber_->get_default_datareader_qos(datareader_qos_);
  subscriber_->copy_from_topic_qos(datareader_qos_, topic_qos_);
  // publisher_->copy_from_topic_qos(datawriter_qos_, topic_qos_);
  if (int ret = check_status(
                    status, "DDS::Subscriber::get_default_datareader_qos") < 0)
    return ret;
 
  datareader_qos_.reader_data_lifecycle.enable_invalid_samples = FALSE;
 
  if (madara::transport::RELIABLE == this->settings_.reliability)
  {
    madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
        "SpliceDDSTransport::setup:"
        " Enabling reliable transport for (%s) datareaders\n",
        madara::utility::dds_topicify(settings_.write_domain).c_str());
 
    datareader_qos_.reliability.kind = DDS::RELIABLE_RELIABILITY_QOS;
    datareader_qos_.history.depth = this->settings_.queue_length;
    datareader_qos_.resource_limits.max_samples = this->settings_.queue_length;
    datareader_qos_.destination_order.kind =
        DDS::BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS;
 
    // unlike the other qos, we do not set max_samples_per_instance here.
    // that shouldn't be as necessary, since we are using take on the reader
  }
  else
  {
    madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
        "SpliceDDSTransport::setup:"
        " Enabling unreliable transport for (%s) datareaders\n",
        madara::utility::dds_topicify(settings_.write_domain).c_str());
  }
 
  madara_logger_log(context_.get_logger(), logger::LOG_DETAILED,
      "SpliceDDSTransport::setup:"
      " Creating datareader for topic (%s)\n",
      madara::utility::dds_topicify(settings_.write_domain).c_str());
 
  // Create Update datareader
  datareader_ = subscriber_->create_datareader(update_topic_,
      // datareader_qos_, &dr_listener_, DDS::STATUS_MASK_NONE);
      datareader_qos_, NULL, DDS::STATUS_MASK_NONE);
 
  // notes: we set the mask to none because the listener will be called
  // by the subscriber listener with notify_datareaders. This is the Splice
  // way of doing this, since we require subscription information and they
  // have so far not implemented on_subscription_matched.
 
  if (int ret = check_handle(datareader_,
                    "DDS::Subscriber::create_datareader (Update)") < 0)
    return ret;
  update_reader_ =
      dynamic_cast<Knowledge::UpdateDataReader_ptr>(datareader_.in());
  if (int ret = check_handle(update_reader_,
                    "Knowledge::UpdateDataReader_ptr::narrow") < 0)
    return ret;
 
  if (!settings_.no_receiving)
  {
    double hertz = settings_.read_thread_hertz;
    if (hertz < 0.0)
    {
      hertz = 0.0;
    }
 
    madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
        "UdpTransportReadThread::setup:"
        " starting %d threads at %f hertz\n",
        settings_.read_threads, hertz);
 
    for (uint32_t i = 0; i < settings_.read_threads; ++i)
    {
      std::stringstream thread_name;
      thread_name << "read";
      thread_name << i;
 
      read_threads_.run(hertz, thread_name.str(),
          new madara::transport::SpliceReadThread(id_, this->settings_,
              context_, update_reader_, latency_update_writer_, send_monitor_,
              receive_monitor_, packet_scheduler_));
    }
  }
 
  return this->validate_transport();
}
 
long madara::transport::SpliceDDSTransport::send_data(
    const knowledge::KnowledgeMap& updates)
{
  long result = 0;
 
  if (!settings_.no_sending)
  {
    result = prep_send(updates, "SpliceDDSTransport::send_data:");
 
    // get the maximum quality from the updates
    uint32_t quality = knowledge::max_quality(updates);
 
    unsigned long long cur_clock = context_.get_clock();
 
    DDS::ReturnCode_t dds_result;
    DDS::InstanceHandle_t handle;
 
    Knowledge::Update data;
 
    data.buffer =
        Knowledge::seq_oct(result, result, (unsigned char*)buffer_.get_ptr());
    data.clock = cur_clock;
    data.quality = quality;
    data.updates = DDS::ULong(updates.size());
    data.originator = DDS::string_dup(id_.c_str());
    data.type = madara::transport::MULTIASSIGN;
    data.ttl = settings_.get_rebroadcast_ttl();
    data.timestamp = utility::get_time();
    data.madara_id = DDS::string_dup(MADARA_IDENTIFIER);
 
    madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
        "SpliceDDSTransport::send:"
        " sending multiassignment: %d updates, time=llu, quality=%d\n",
        data.updates, cur_clock, quality);
 
    handle = update_writer_->register_instance(data);
    dds_result = update_writer_->write(data, handle);
    result = (long)dds_result;
    // update_writer_->unregister_instance (data, handle);
  }
 
  return result;
}
 
int madara::transport::SpliceDDSTransport::check_handle(
    void* handle, const char* info)
{
  if (!handle)
  {
    madara_logger_log(context_.get_logger(), logger::LOG_ERROR,
        "SpliceDDSTransport::check_handle:"
        " error in %s: Creation failed: invalid handle\n",
        info);
 
    return -2;
  }
 
  return 0;
}
 
int madara::transport::SpliceDDSTransport::check_status(
    DDS::ReturnCode_t status, const char* info)
{
  // if the status is okay, then return without issue
  if ((status == DDS::RETCODE_OK) || (status == DDS::RETCODE_NO_DATA))
    return 0;
 
  madara_logger_log(context_.get_logger(), logger::LOG_ERROR,
      "SpliceDDSTransport::check_status:"
      " error in %s: Creation failed: %s\n",
      info, get_error_name(status));
 
  return -2;
}
 
const char* madara::transport::SpliceDDSTransport::get_error_name(
    DDS::ReturnCode_t status)
{
  return ret_code_names[status];
}