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