opensplice_dds_broker.cpp

Go to the documentation of this file.

#include <iostream>
#include "opensplice_dds_broker/opensplice_dds_broker.h"

PLUGINLIB_EXPORT_CLASS(opensplice_dds_broker::OpenSpliceDDSBroker, micros_swarm::CommInterface)

namespace opensplice_dds_broker{

    OpenSpliceDDSBroker::OpenSpliceDDSBroker()
    {
        packet_publisher_.reset(new opensplice_dds_broker::Publisher("micros_swarm_framework_topic"));
        packet_subscriber_.reset(new opensplice_dds_broker::Subscriber("micros_swarm_framework_topic"));
    }

    void OpenSpliceDDSBroker::init(std::string name, const micros_swarm::PacketParser& parser)
    {
        name_ = name;
        parser_ = parser;
    }

    void OpenSpliceDDSBroker::broadcast(const std::vector<uint8_t>& msg_data)
    {
        opensplice_dds_broker::GSDFPacket dds_msg;
        unsigned int bufsize = msg_data.size();
        if(!msg_data.empty()) {
            dds_msg.data.replace(bufsize, bufsize, (char*)(&msg_data[0]), false);
            packet_publisher_->publish(dds_msg);
        }
    }

    void OpenSpliceDDSBroker::callback(const opensplice_dds_broker::GSDFPacket& dds_msg)
    {
        int msgLen = dds_msg.data.length();
        if (msgLen == 0) {
            std::cout<<"opensplice dds recv error."<<std::endl;
        }
        else {
            uint8_t* buf = (uint8_t*)malloc(sizeof(uint8_t)*msgLen);
            memcpy(buf, dds_msg.data.get_buffer(), msgLen);
            parser_.parse(buf, msgLen);
        }
    }
            
    void OpenSpliceDDSBroker::receive()
    {
        boost::function<void(const opensplice_dds_broker::GSDFPacket&)> func = boost::bind(&OpenSpliceDDSBroker::callback, this, _1);
        packet_subscriber_->subscribe(func);
    }
};