Go to the documentation of this file.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
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),
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 }
00110
00111
00112 #endif // UDPMULTI_TRANSPORT_PUBLISHER_H