00001
00002
00003 #include <cstdlib>
00004 #include <cstddef>
00005 #include <cassert>
00006 #include <utility>
00007
00008 #include <ros/ros.h>
00009
00010 #include "udpmulti_transport/UDPMultHeader.h"
00011 #include "udpmulti_transport/UDPMultRegisterTopic.h"
00012 #include "udpmulti_transport/udpmulti_publisher.h"
00013
00014 namespace udpmulti_transport {
00015
00016 UDPMultiPublisherImpl::UDPMultiPublisherImpl() :
00017 io_service_()
00018 {
00019 endpoint_ = NULL;
00020 socket_ = NULL;
00021 }
00022
00023 UDPMultiPublisherImpl::~UDPMultiPublisherImpl()
00024 {
00025 if (socket_) delete socket_;
00026 if (endpoint_) delete endpoint_;
00027 endpoint_ = NULL;
00028 socket_ = NULL;
00029 }
00030
00031 void UDPMultiPublisherImpl::initialise(const std::string & topicname) {
00032
00033
00034 ROS_INFO("Waiting for service '/udpmulti_manager/register_topic' to be ready");
00035 ros::service::waitForService("/udpmulti_manager/register_topic");
00036 ROS_INFO("Requesting UDP Multicast port and address");
00037
00038 ros::ServiceClient requestPortClt = nh_.serviceClient<udpmulti_transport::UDPMultRegisterTopic>("/udpmulti_manager/register_topic");
00039 udpmulti_transport::UDPMultRegisterTopic srv;
00040 srv.request.topic = topicname;
00041 if (!requestPortClt.call(srv)) {
00042 ROS_ERROR("Failed to call service register_memory");
00043 }
00044 multicast_address_ = srv.response.multicast_address;
00045 port_ = srv.response.port;
00046 ROS_INFO("Creating multicast connection on '%s:%d'",multicast_address_.c_str(),port_);
00047
00048 endpoint_ = new boost::asio::ip::udp::endpoint(
00049 boost::asio::ip::address::from_string(multicast_address_),port_);
00050 socket_= new boost::asio::ip::udp::socket(io_service_,endpoint_-> protocol());
00051 printf("Endpoint %p / Socket %p\n",endpoint_,socket_);
00052 }
00053
00054 const std::string & UDPMultiPublisherImpl::getMulticastAddress() const {
00055 return multicast_address_;
00056 }
00057
00058 uint32_t UDPMultiPublisherImpl::getPort() const {
00059 return port_;
00060 }
00061
00062 UDPMultHeader UDPMultiPublisherImpl::getUDPHeader() const
00063 {
00064 UDPMultHeader header;
00065 header.multicast_address = multicast_address_;
00066 header.port = port_;
00067 return header;
00068 }
00069
00070 void UDPMultiPublisherImpl::multicast(const ros::Message& message,uint32_t datasize)
00071 {
00072 uint8_t buffer[MAX_UDP_PACKET_SIZE];
00073 if (!datasize) {
00074 datasize = ros::serialization::serializationLength(message);
00075 }
00076 assert(datasize < MAX_UDP_PACKET_SIZE);
00077 assert(socket_);
00078 assert(endpoint_);
00079
00080 ros::serialization::OStream out(buffer,datasize);
00081 ros::serialization::serialize(out,message);
00082
00083 socket_-> send_to(boost::asio::buffer(buffer, datasize),*endpoint_);
00084 io_service_.poll();
00085 }
00086
00087 }