ros2udpProxy.cpp

Go to the documentation of this file.

#include <string>
#include <sstream>
#include <iostream>
#include <unistd.h>
#include "ros/ros.h"
#include <ros/transport_hints.h>
#include <stdio.h>


#include <SystemConfig.h>
#include <Configuration.h>
#include <exception>

#include <boost/asio.hpp>
#include <boost/thread.hpp> 

#include <sys/ioctl.h>
#include <net/if.h>
#include <netinet/in.h>
#include <arpa/inet.h>


<?messageIncludes?>

using namespace castor;



using boost::asio::ip::udp;



std::string ownRosName;
udp::socket* insocket;
udp::endpoint otherEndPoint;
udp::endpoint destEndPoint;
boost::asio::ip::address multiCastAddress;
boost::asio::io_service io_service;
void handleUdpPacket(const boost::system::error_code& error,   std::size_t bytes_transferred);
void listenForPacket();



<?rosMessageHandler?>

<?rosPublisherDecl?>

boost::array<char,64000> inBuffer;
void listenForPacket() {
        insocket->async_receive_from(boost::asio::buffer(inBuffer), otherEndPoint,
        boost::bind(&handleUdpPacket, boost::asio::placeholders::error,boost::asio::placeholders::bytes_transferred));
}
void handleUdpPacket(const boost::system::error_code& error,   std::size_t bytes_transferred) {
        //std::cout << "From "<<otherEndPoint.address() << std::endl;
        if (!error) { // && otherEndPoint.address() != localIP) {
                uint32_t id = *((uint32_t*)(inBuffer.data()));
                //std::cout << "Got packet"<<std::endl;
                try {   
                        ros::serialization::IStream stream(((uint8_t*)inBuffer.data())+4,bytes_transferred-4);
                        switch(id) {
                                <?udpReception?>                        
                        
                                default:
                                        std::cerr << "Cannot find Matching topic:" << id << std::endl;
                        }
                }
                catch(std::exception& e) {
                        ROS_ERROR_STREAM_THROTTLE(2,"Exception while receiving DDS message:"<<e.what()<< " Discarding message!");
                }
                
        }
        listenForPacket();
        return;
}
void run() {
        io_service.run();
}

int main (int argc, char *argv[])
{
        
        SystemConfigPtr sc = SystemConfig::getInstance();

        Configuration *proxyconf = (*sc)["UdpProxy"];
        
        //std::string port = proxyconf->get<std::string>("UdpProxy","Port",NULL);
        
        std::string baddress = proxyconf->get<std::string>("UdpProxy","MulticastAddress",NULL);
        
        ushort port = proxyconf->get<ushort>("UdpProxy","Port",NULL);
        
        //udp::resolver resolver(io_service);
    //udp::resolver::query query(udp::v4(), baddress, port);

        //bcastEndPoint = *resolver.resolve(query);
        
        
        
        
        
  
        //myEndPoint = udp::endpoint(udp::v4(),bcastEndPoint.port());
        
        multiCastAddress = boost::asio::ip::address::from_string(baddress);
        destEndPoint = udp::endpoint(multiCastAddress,port);
        
        std::cout<<"Opening to "<<multiCastAddress <<std::endl;
        
        
        
        insocket = new udp::socket(io_service,udp::endpoint(multiCastAddress,port));
        
        insocket->set_option(boost::asio::ip::multicast::enable_loopback(false));
        insocket->set_option(boost::asio::ip::multicast::join_group(multiCastAddress));
        listenForPacket();
        
        
         
    ros::init(argc, argv, "udpProxy");

    ros::NodeHandle n;
    ownRosName = ros::this_node::getName();//n.getNamespace();//n.resolveName("ddsProxy",true);
    
    std::cout << ownRosName << std::endl;
    
        <?subscriptions?>
        
    <?advertisement?>
        
        boost::thread iothread(run);
    
    //ros::spin();
        ros::AsyncSpinner *spinner;
        
        spinner = new ros::AsyncSpinner(4);
        spinner->start();
        
        std::cout << "Ros2Udp Proxy running..." <<std::endl;
    while(n.ok()) {
            usleep(300000);
    }
    io_service.stop();
    iothread.join();
    return 0;
}