udpmulti_transport: udpmulti_transport: udpmulti

00001 #ifndef UDPMULTI_TRANSPORT_PUBLISHER_H
00002 #define UDPMULTI_TRANSPORT_PUBLISHER_H
00003 
00004 #include <boost/asio.hpp>
00005 
00006 
00007 #include <message_transport/simple_publisher_plugin.h>
00008 #include <udpmulti_transport/UDPMultHeader.h>
00009 
00010 namespace udpmulti_transport {
00011 
00012     static const unsigned int MAX_UDP_PACKET_SIZE = 8092;
00013 
00014         class UDPMultiPublisherImpl 
00015         {
00016                 public:
00017             UDPMultiPublisherImpl();
00018                         virtual ~UDPMultiPublisherImpl();
00019 
00020             void setNodeHandle(ros::NodeHandle & nh) {
00021                 nh_ = nh;
00022             }
00023 
00024                         void initialise(const std::string & topicname);
00025                         uint32_t getPort() const;
00026                         const std::string & getMulticastAddress() const;
00027 
00028                         UDPMultHeader getUDPHeader() const;
00029 
00030             template <class M>
00031                         void multicast(const M & message,uint32_t datasize=0) {
00032                 uint8_t buffer[MAX_UDP_PACKET_SIZE];
00033                 if (!datasize) {
00034                     datasize = ros::serialization::serializationLength(message);
00035                 }
00036                 assert(datasize < MAX_UDP_PACKET_SIZE);
00037                 assert(socket_);
00038                 assert(endpoint_);
00039 
00040                 ros::serialization::OStream out(buffer,datasize);
00041                 ros::serialization::serialize(out,message);
00042 
00043                 // ROS_INFO("Sending datagram");
00044                 socket_-> send_to(boost::asio::buffer(buffer, datasize),*endpoint_);
00045                 io_service_.poll();
00046             }
00047                 protected:
00048                         
00049                         uint32_t port_;
00050                         std::string multicast_address_;
00051             boost::asio::io_service io_service_;
00052             boost::asio::ip::udp::endpoint *endpoint_;
00053             boost::asio::ip::udp::socket *socket_;
00054             ros::NodeHandle nh_;
00055 
00056         };
00057 
00058         template <class Base>
00059         class UDPMultiPublisher : 
00060                 public message_transport::SimplePublisherPlugin<Base,udpmulti_transport::UDPMultHeader>
00061         {
00062                 public:
00063                         UDPMultiPublisher() : 
00064                 message_transport::SimplePublisherPlugin<Base,udpmulti_transport::UDPMultHeader>(true), // force latch
00065                                 first_run_(true) {}
00066                         virtual ~UDPMultiPublisher() {}
00067 
00068                         virtual std::string getTransportName() const
00069                         {
00070                                 return "udpmulti";
00071                         }
00072                 protected:
00073                         virtual void postAdvertiseInit() {
00074                 impl.setNodeHandle(this->getNodeHandle());
00075             }
00076                         virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) {
00077                 ROS_INFO("Received connection request");
00078             }
00079                         virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) {
00080                 ROS_INFO("Received disconnection request");
00081             }
00082             
00083                         virtual void publish(const Base& message,
00084                                         const typename message_transport::SimplePublisherPlugin<Base,udpmulti_transport::UDPMultHeader>::PublishFn& publish_fn) const {
00085                                 
00086                 uint32_t datasize;
00087                 if (first_run_) {
00088                     impl.initialise(this->getTopic());
00089                     publish_fn(impl.getUDPHeader());
00090                     first_run_ = false;
00091                 }
00092 
00093                                 
00094                                 datasize = ros::serialization::serializationLength(message);
00095                                 if (datasize > MAX_UDP_PACKET_SIZE) {
00096                                         ROS_ERROR("This type of message is too big (%d bytes) for UDP (max %d bytes)",
00097                                                         datasize, MAX_UDP_PACKET_SIZE);
00098                                         return;
00099                                 }
00100                                 impl.multicast(message,datasize);
00101                         }
00102 
00103                         mutable UDPMultiPublisherImpl impl;
00104             mutable bool first_run_;
00105                         
00106         };
00107 
00108 
00109 } //namespace udpmulti_transport
00110 
00111 
00112 #endif // UDPMULTI_TRANSPORT_PUBLISHER_H
udpmulti_publisher.h
