ros_broker.cpp

Go to the documentation of this file.

#include <iostream>
#include <ros/ros.h>

#include "ros_broker/ros_broker.h"

PLUGINLIB_EXPORT_CLASS(ros_broker::ROSBroker, micros_swarm::CommInterface)

namespace ros_broker{

    ROSBroker::ROSBroker()
    {
        packet_publisher_ = node_handle_.advertise<ros_broker::GSDFPacket>("/micros_swarm_framework_topic", 2000, true);
    }

    void ROSBroker::init(std::string name, const micros_swarm::PacketParser& parser)
    {
        name_ = name;
        parser_ = parser;
    }
            
    void ROSBroker::broadcast(const std::vector<uint8_t>& msg_data)
    {
        static bool flag = false;
        if(!flag) {
            ros::Duration(1).sleep();
            if(!packet_publisher_) {
                ROS_INFO("ROS communicator could not initialize!");
                exit(-1);
            }
            flag=true;
        }
        
        if(ros::ok()) {
            ros_broker::GSDFPacket ros_msg;
            ros_msg.data = msg_data;
            packet_publisher_.publish(ros_msg);
        }
    }
            
    void ROSBroker::callback(const ros_broker::GSDFPacket& ros_msg)
    {
        parser_.parse(ros_msg.data);
    }

    void ROSBroker::receive()
    {
        packet_subscriber_ = node_handle_.subscribe("/micros_swarm_framework_topic", 1000, &ROSBroker::callback, this, ros::TransportHints().udp());
    }
};