├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── udp_bridge │ ├── connection.h │ ├── defragmenter.h │ ├── packet.h │ ├── remote_node.h │ ├── statistics.h │ ├── types.h │ ├── udp_bridge.h │ ├── utilities.h │ └── wrapped_packet.h ├── launch ├── test_fragments.launch ├── test_mininet_multilink_operator.launch ├── test_mininet_multilink_robot.launch └── test_video.launch ├── mainpage.dox ├── msg ├── BridgeInfo.msg ├── ConnectionInternal.msg ├── DataRates.msg ├── MessageInternal.msg ├── Remote.msg ├── RemoteConnection.msg ├── RemoteSubscribeInternal.msg ├── ResendRequest.msg ├── TopicInfo.msg ├── TopicRemoteConnectionDetails.msg ├── TopicRemoteDetails.msg ├── TopicStatistics.msg └── TopicStatisticsArray.msg ├── package.xml ├── rosdoc.yaml ├── src ├── connection.cpp ├── defragmenter.cpp ├── packet.cpp ├── remote_node.cpp ├── ros_messages.h ├── statistics.cpp ├── udp_bridge.cpp ├── udp_bridge_node.cpp ├── udpbridge_ui.cpp ├── utilities.cpp └── wrapped_packet.cpp ├── srv ├── AddRemote.srv ├── ListRemotes.srv └── Subscribe.srv └── test ├── mininet ├── multilink.py ├── operator.bash ├── operator_launch.bash ├── operator_setup.bash ├── robot.bash ├── robot_launch.bash └── robot_setup.bash └── utest.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | doc/html 2 | doc/manifest.yaml 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(udp_bridge) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | message_generation 10 | topic_tools 11 | ) 12 | 13 | find_package(ZLIB REQUIRED) 14 | 15 | add_message_files( 16 | FILES 17 | BridgeInfo.msg 18 | ConnectionInternal.msg 19 | DataRates.msg 20 | MessageInternal.msg 21 | Remote.msg 22 | RemoteConnection.msg 23 | RemoteSubscribeInternal.msg 24 | ResendRequest.msg 25 | TopicInfo.msg 26 | TopicRemoteConnectionDetails.msg 27 | TopicRemoteDetails.msg 28 | TopicStatistics.msg 29 | TopicStatisticsArray.msg 30 | ) 31 | 32 | add_service_files( 33 | FILES 34 | Subscribe.srv 35 | AddRemote.srv 36 | ListRemotes.srv 37 | ) 38 | 39 | generate_messages( 40 | ) 41 | 42 | catkin_package( 43 | # INCLUDE_DIRS include 44 | # LIBRARIES udp_bridge 45 | # CATKIN_DEPENDS geographic_msgs geometry_msgs mission_plan roscpp std_msgs 46 | # DEPENDS system_lib 47 | ) 48 | 49 | include_directories( 50 | include 51 | ${catkin_INCLUDE_DIRS} 52 | ${ZLIB_INCLUDE_DIRS} 53 | ) 54 | 55 | set(UDP_BRIDGE_SOURCES 56 | src/connection.cpp 57 | src/defragmenter.cpp 58 | src/packet.cpp 59 | src/remote_node.cpp 60 | src/statistics.cpp 61 | src/udp_bridge.cpp 62 | src/utilities.cpp 63 | src/wrapped_packet.cpp 64 | ) 65 | 66 | add_executable(udp_bridge_node src/udp_bridge_node.cpp ${UDP_BRIDGE_SOURCES}) 67 | add_executable(udp_bridge_ui src/udpbridge_ui.cpp) 68 | 69 | add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) 70 | add_dependencies(${PROJECT_NAME}_ui ${PROJECT_NAME}_generate_messages_cpp) 71 | 72 | target_link_libraries(udp_bridge_node ${catkin_LIBRARIES} ${ZLIB_LIBRARIES}) 73 | target_link_libraries(udp_bridge_ui ${catkin_LIBRARIES}) 74 | 75 | if(CATKIN_ENABLE_TESTING) 76 | find_package(rostest REQUIRED) 77 | catkin_add_gtest(utest test/utest.cpp ${UDP_BRIDGE_SOURCES}) 78 | target_link_libraries(utest ${catkin_LIBRARIES} ${ZLIB_LIBRARIES}) 79 | endif() 80 | 81 | 82 | # Mark executables and/or libraries for installation 83 | install(TARGETS 84 | udp_bridge_node 85 | udp_bridge_ui 86 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 88 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | ) 90 | 91 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2016-2020, Roland Arsenault 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # udp_bridge package overview 2 | 3 | The udp_bridge package connects multiple ROS cores over an unreliable network using UDP datagrams. The main design goal is to resume the transmission of select ROS topics as quickly as possible once a telemetry link is re-established after a lost connection. 4 | 5 | Initial remote nodes, connections, and transmitted topics may be specified as parameters then service calls may be used to manage romotes and transmitted topics. An rqt plugin can be used to display udp_bridge status as well as add connections and topics. 6 | 7 | ## ROS API 8 | 9 | List of nodes: 10 | 11 | - udp_bridge_node 12 | 13 | ### udp_bridge_node 14 | 15 | udp_bridge_node subscribes to topics to be sent to remote udp_bridge_nodes and publishes messages received from remote udp_bridge_nodes and provides services to manage remote connections and transmitted topics. 16 | 17 | udp_bridge_node uses names to identify remote nodes. Make sure to set the name private parameter if the node name is not unique in the network. 18 | 19 | #### Usage 20 | 21 | rosrun udp_bridge udp_bridge_node 22 | 23 | Example of running two bridge nodes on a single ROS core for testing. 24 | 25 | ROS_NAMESPACE=foo rosrun udp_bridge udp_bridge_node _name:=node_a 26 | ROS_NAMESPACE=bar rosrun udp_bridge udp_bridge_node _name:=node_b _port:=4201 27 | 28 | rosservice call /foo/udp_bridge_node/add_remote 29 | rosservice call /foo/udp_bridge_node/remote_advertise 30 | 31 | rostopic pub /send_topic hello 32 | rostopic echo /receive_topic 33 | 34 | #### ROS topics 35 | 36 | Publishes to: 37 | 38 | - ~/bridge_info [udp_bridge/BridgeInfo] 39 | - ~/topic_statistics [udp_bridge/TopicStatisticsArray] 40 | 41 | #### ROS Parameters 42 | 43 | Reads the following parameters from the parameter server. Lists are encoded as labeled structures to allow updating details in launch files. 44 | 45 | - ~name: [string] name used to when communicating with remotes. Should be unique. Defaults to the node's name. 46 | - ~port: [integer] UDP port number used for listening for incoming data. Defaults to 4200. 47 | - ~maxPacketSize: [integer] Maximum packet size to use to send data. Defaults to 65500. 48 | - ~remotes: [struct] List of initial remotes nodes. 49 | - (remote_label): 50 | - name: Name of the remote. Defaults to the remote label if omitted. 51 | - connections: Named connections to the remote. 52 | - (connection_id): 53 | - host: 54 | - port: 55 | - returnHost: 56 | - returnPort: 57 | - maximumBytesPerSecond: Data rate limit for this connection. 0 means default (500000) 58 | - topics: 59 | - (topic_label): 60 | - source: 61 | - destination: 62 | - period: 63 | - queue_size: 64 | 65 | #### ROS services 66 | 67 | - add_remote: [udp_bridge/AddRemote] 68 | - list_remotes: [udp_bridge/] 69 | - remote_advertise: [udp_bridge/] 70 | - remote_subscribe: [udp_bridge/] 71 | 72 | ## Developer documentation 73 | 74 | Developer documentation can be generated with rosdoc_lite. 75 | 76 | roscd udp_bridge 77 | rosdoc_lite . 78 | 79 | The main page will generated at doc/html/index.html 80 | -------------------------------------------------------------------------------- /include/udp_bridge/connection.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_CONNECTION_H 2 | #define UDP_BRIDGE_CONNECTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "udp_bridge/types.h" 14 | #include "udp_bridge/wrapped_packet.h" 15 | #include "udp_bridge/DataRates.h" 16 | 17 | namespace udp_bridge 18 | { 19 | 20 | class ConnectionException 21 | { 22 | public: 23 | ConnectionException(const std::string &msg):msg_(msg) {} 24 | const std::string & getMessage() const {return msg_;} 25 | private: 26 | std::string msg_; 27 | }; 28 | 29 | /// Represents a connection to a remote node. 30 | /// Multiple connections to a remote node may be used for redundancy. 31 | class Connection 32 | { 33 | public: 34 | Connection(std::string id, std::string const &host, uint16_t port, std::string return_host=std::string(), uint16_t return_port=0); 35 | 36 | void setHostAndPort(const std::string& host, uint16_t port); 37 | void setRateLimit(uint32_t maximum_bytes_per_second); 38 | uint32_t rateLimit() const; 39 | 40 | std::string str() const; 41 | 42 | const std::string &id() const; 43 | 44 | // Used to tell the remote host the address to get back to us. 45 | const std::string& returnHost() const; 46 | uint16_t returnPort() const; 47 | void setReturnHostAndPort(const std::string &return_host, uint16_t return_port); 48 | 49 | // IP address and port from incoming packets. May differ from where packets are sent depending 50 | // on network architecture. 51 | const std::string& sourceIPAddress() const; 52 | uint16_t sourcePort() const; 53 | void setSourceIPAndPort(const std::string &source_ip, uint16_t source_port); 54 | 55 | 56 | const std::string& host() const; 57 | uint16_t port() const; 58 | const std::string& ip_address() const; 59 | std::string ip_address_with_port() const; 60 | 61 | const sockaddr_in* socket_address() const; 62 | 63 | const double& last_receive_time() const; 64 | void update_last_receive_time(double t, int data_size, bool duplicate); 65 | 66 | void resend_packets(const std::vector &missing_packets, int socket); 67 | 68 | SendResult send(const std::vector& packets, int socket, const std::string& remote, bool is_overhead); 69 | 70 | PacketSizeData send(const std::vector &data, int socket, PacketSendCategory category); 71 | 72 | //bool can_send(uint32_t byte_count, double time); 73 | 74 | /// Returns the average received data rate at the specified time. 75 | /// The first element of the returned pair is for unique bytes/second and the second 76 | /// element is for duplicated bytes per second. Duplicated bytes are for packets 77 | /// that had already been received on another connection. 78 | std::pair data_receive_rate(double time); 79 | 80 | /// Returns the average data rate. 81 | DataRates data_sent_rate(ros::Time time, PacketSendCategory category); 82 | 83 | /// Remove saved sent packets older than cutoff_time. 84 | void cleanup_sent_packets(ros::Time cutoff_time); 85 | 86 | private: 87 | void resolveHost(); 88 | 89 | /// connection_id of the connection 90 | std::string id_; 91 | 92 | std::string host_; 93 | std::string ip_address_; ///< resolved ip address 94 | uint16_t port_; 95 | 96 | std::vector addresses_; 97 | 98 | /// Used by the remote to refer to us. Useful if they are behind a nat 99 | std::string return_host_; 100 | uint16_t return_port_ = 0; 101 | 102 | std::string source_ip_address_; ///< source ip address of incoming packets from remote 103 | uint16_t source_port_ = 0; ///< source port of incoming packets from remote 104 | 105 | /// Time in seconds since epoch that last packet was received 106 | double last_receive_time_ = 0.0; 107 | 108 | static constexpr uint32_t default_rate_limit = 50000; 109 | 110 | /// Maximum bytes per second to send. 111 | uint32_t data_rate_limit_ = default_rate_limit; 112 | 113 | /// Info about a received packet useful for data rate statistics. 114 | struct ReceivedSize 115 | { 116 | /// size in bytes of a received packet 117 | uint16_t size = 0; 118 | 119 | /// indicates if this is a duplicate packet that has already been seen on a 120 | /// different channel. 121 | bool duplicate = false; 122 | }; 123 | std::map > data_size_received_history_; 124 | 125 | /// History of recently sent packets. 126 | /// Each connection keeps a buffer of sent packets in case a resend is needed. 127 | /// The same data packets are replicated for each connection to account for 128 | /// different source_node or connection_id. 129 | std::map sent_packets_; 130 | 131 | PacketSendStatistics sent_packet_statistics_; 132 | }; 133 | 134 | 135 | std::string addressToDotted(const sockaddr_in &address); 136 | 137 | } // namespace udp_bridge 138 | 139 | #endif 140 | -------------------------------------------------------------------------------- /include/udp_bridge/defragmenter.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_DEFRAGMENTER_H 2 | #define UDP_BRIDGE_DEFRAGMENTER_H 3 | 4 | #include 5 | #include "packet.h" 6 | 7 | namespace udp_bridge 8 | { 9 | 10 | /// Collects and reassembles udp_bridge::Fragment packets. 11 | class Defragmenter 12 | { 13 | /// Keeps track of the fragments of a fragmented packet. 14 | struct Fragments 15 | { 16 | uint16_t fragment_count; 17 | ros::Time first_arrival_time; 18 | std::map > fragment_map; 19 | }; 20 | public: 21 | /// returns true if supplied fragment completed a packet 22 | bool addFragment(std::vector fragment); 23 | 24 | /// returns a list of complete packets 25 | std::vector > getPackets(); 26 | 27 | /// Discard incomplete packets older than maxAge and 28 | /// returns number of discarded packets. 29 | int cleanup(ros::Duration maxAge); 30 | private: 31 | /// map of packet id 32 | std::map fragment_map_; 33 | std::vector > complete_packets_; 34 | }; 35 | 36 | } // namespace udp_bridge 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /include/udp_bridge/packet.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_PACKET_H 2 | #define UDP_BRIDGE_PACKET_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | /// \file packet.h 11 | /// Defines the structures used for sending data as UDP packets. 12 | /// In addition to the structs defined here, ROS message types 13 | /// are also used. 14 | /// \defgroup packet UDP Packets 15 | 16 | namespace udp_bridge 17 | { 18 | 19 | constexpr uint8_t maximum_node_name_size = 24; 20 | constexpr uint8_t maximum_connection_id_size = 8; 21 | 22 | /// Packet type identifiers. 23 | /// \ingroup packet 24 | enum class PacketType: uint8_t { 25 | Data, ///< packet containing MessageInternal message containing a message from a transmitted topic 26 | Compressed, ///< packet of type CompressedPacket 27 | SubscribeRequest, ///< packet containing a RemoteSubscribeInternal message 28 | Fragment, ///< packet of type Fragment 29 | BridgeInfo, ///< packet containing a BridgeInfo message 30 | TopicStatistics, ///< packet containing a TopicStatistics message 31 | WrappedPacket, ///< packet of type SequencedPacket 32 | ResendRequest, ///< packet containing a ResendRequest message 33 | Connection, ///< packet of type ConnectionInternal 34 | }; 35 | 36 | template PacketType packetTypeOf(const MessageType&); 37 | 38 | #pragma pack(push, 1) 39 | 40 | struct PacketHeader 41 | { 42 | PacketType type; 43 | }; 44 | 45 | /// Represents any supported packet. 46 | /// \ingroup packet 47 | struct Packet: public PacketHeader 48 | { 49 | uint8_t data[]; 50 | }; 51 | 52 | struct CompressedPacketHeader: public PacketHeader 53 | { 54 | /// Size needed for the uncompressed version compressed_data. 55 | uint32_t uncompressed_size; 56 | }; 57 | 58 | /// Packet containing a compressed packet. 59 | /// \ingroup packet 60 | struct CompressedPacket: public CompressedPacketHeader 61 | { 62 | uint8_t compressed_data[]; 63 | }; 64 | 65 | struct FragmentHeader: public PacketHeader 66 | { 67 | uint32_t packet_id; 68 | uint16_t fragment_number; 69 | uint16_t fragment_count; 70 | }; 71 | 72 | /// Packet containing a fragment of a larger packet. 73 | /// \ingroup packet 74 | struct Fragment: public FragmentHeader 75 | { 76 | uint8_t fragment_data[]; 77 | }; 78 | 79 | struct SequencedPacketHeader: public PacketHeader 80 | { 81 | char source_node[maximum_node_name_size]; 82 | char connection_id[maximum_connection_id_size]; 83 | uint64_t packet_number; 84 | uint32_t packet_size; 85 | }; 86 | 87 | /// Wrapper packet containing a packet as well as connection metadata 88 | /// \ingroup packet 89 | struct SequencedPacket: public SequencedPacketHeader 90 | { 91 | uint8_t packet[]; 92 | }; 93 | 94 | #pragma pack(pop) 95 | 96 | std::vector compress(const std::vector &data); 97 | std::vector uncompress(std::vector const &data); 98 | 99 | } // namespace udp_bridge 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /include/udp_bridge/remote_node.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_REMOTE_NODE_H 2 | #define UDP_BRIDGE_REMOTE_NODE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace udp_bridge 15 | { 16 | 17 | class Connection; 18 | 19 | // Represents a remote udp_bridge node. 20 | // Multiple connections to the remote node 21 | // may be specified. 22 | class RemoteNode 23 | { 24 | public: 25 | RemoteNode(std::string remote_name, std::string local_name); 26 | 27 | void update(const Remote& remote_message); 28 | void update(const BridgeInfo& bridge_info, const SourceInfo& source_info); 29 | 30 | const std::string &name() const; 31 | 32 | // returns a name sutible for use as a topic name. 33 | std::string topicName() const; 34 | 35 | std::shared_ptr connection(std::string connection_id); 36 | std::vector > connections(); 37 | 38 | std::shared_ptr newConnection(std::string connection_id, std::string host, uint16_t port); 39 | 40 | std::vector unwrap(std::vector const &message, const SourceInfo& source_info); 41 | 42 | std::vector > getPacketsToResend(const ResendRequest& resend_request); 43 | 44 | Defragmenter& defragmenter(); 45 | 46 | void publishTopicStatistics(const TopicStatisticsArray& statistics); 47 | 48 | void clearReceivedPacketTimesBefore(ros::Time time); 49 | 50 | ResendRequest getMissingPackets(); 51 | 52 | private: 53 | // name of the remote udp_bridge node 54 | std::string name_; 55 | 56 | // name of the local udp_bridge node; 57 | std::string local_name_; 58 | 59 | std::map > connections_; 60 | 61 | Defragmenter defragmenter_; 62 | 63 | std::map received_packet_times_; 64 | std::map resend_request_times_; 65 | 66 | ros::Publisher bridge_info_publisher_; 67 | ros::Publisher topic_statistics_publisher_; 68 | 69 | uint64_t next_packet_number_ = 0; 70 | ros::Time last_packet_time_; 71 | }; 72 | 73 | } // namespace udp_bridge 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /include/udp_bridge/statistics.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_STATISTICS_H 2 | #define UDP_BRIDGE_STATISTICS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace udp_bridge 10 | { 11 | 12 | enum struct SendResult 13 | { 14 | success, 15 | failed, 16 | dropped, ///< dropped due to rate limit 17 | }; 18 | 19 | /// Data sizes used for calculating message data rates 20 | struct MessageSizeData 21 | { 22 | /// Size of message data before compression and packing into one or more packets 23 | int message_size = 0; 24 | 25 | /// Number of fragments if the packet had to be split to respect max packet size. 1 if the packet didn't need to be split. 26 | int fragment_count = 0; 27 | 28 | /// Number of bytes attempted to be sent. 29 | int sent_size = 0; 30 | 31 | ros::Time timestamp; 32 | std::map > send_results; 33 | }; 34 | 35 | 36 | enum struct PacketSendCategory 37 | { 38 | message, 39 | overhead, 40 | resend 41 | }; 42 | 43 | struct PacketSizeData 44 | { 45 | ros::Time timestamp; 46 | u_int16_t size; 47 | PacketSendCategory category; 48 | SendResult send_result; 49 | }; 50 | 51 | template class Statistics 52 | { 53 | public: 54 | void add(const T& data) 55 | { 56 | if(!data.timestamp.isZero()) 57 | data_.push_back(data); 58 | 59 | // only keep 10 seconds of data 60 | while(!data_.empty() && data_.front().timestamp < data.timestamp - ros::Duration(10.0)) 61 | data_.pop_front(); 62 | } 63 | protected: 64 | std::deque data_; 65 | }; 66 | 67 | class MessageStatistics: public Statistics 68 | { 69 | public: 70 | void add(const MessageSizeData& data); 71 | 72 | std::vector get(); 73 | }; 74 | 75 | 76 | class PacketSendStatistics: public Statistics 77 | { 78 | public: 79 | DataRates get() const; 80 | DataRates get(PacketSendCategory category) const; 81 | 82 | bool can_send(uint32_t data_size, uint32_t bytes_per_second_limit, ros::Time time) const; 83 | 84 | private: 85 | DataRates get(PacketSendCategory *category) const; 86 | 87 | }; 88 | 89 | } // namespace udp_bridge 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /include/udp_bridge/types.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_TYPES_H 2 | #define UDP_BRIDGE_TYPES_H 3 | 4 | #include "udp_bridge/statistics.h" 5 | 6 | namespace udp_bridge 7 | { 8 | 9 | struct ConnectionRateInfo 10 | { 11 | float period; 12 | ros::Time last_sent_time; 13 | }; 14 | 15 | struct RemoteDetails 16 | { 17 | RemoteDetails()=default; 18 | //RemoteDetails(std::string const &destination_topic, float period):destination_topic(destination_topic),period(period) 19 | //{} 20 | 21 | std::string destination_topic; 22 | std::map connection_rates; 23 | 24 | }; 25 | 26 | struct SubscriberDetails 27 | { 28 | ros::Subscriber subscriber; 29 | std::map remote_details; 30 | MessageStatistics statistics; 31 | }; 32 | 33 | // Name and address/port from which a message was received 34 | struct SourceInfo 35 | { 36 | std::string node_name; 37 | std::string host; 38 | uint16_t port = 0; 39 | }; 40 | 41 | } // namespace udp_bridge 42 | 43 | #endif -------------------------------------------------------------------------------- /include/udp_bridge/udp_bridge.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_UDP_BRIDGE_H 2 | #define UDP_BRIDGE_UDP_BRIDGE_H 3 | 4 | #include 5 | #include "udp_bridge/Subscribe.h" 6 | #include "udp_bridge/AddRemote.h" 7 | #include "udp_bridge/ListRemotes.h" 8 | #include 9 | #include "connection.h" 10 | #include "packet.h" 11 | #include "defragmenter.h" 12 | #include "udp_bridge/types.h" 13 | #include "udp_bridge/wrapped_packet.h" 14 | #include "udp_bridge/ConnectionInternal.h" 15 | #include 16 | 17 | namespace udp_bridge 18 | { 19 | 20 | class RemoteNode; 21 | 22 | /// The top level class containing all the functionality of the udp_bridge_node. 23 | /// Once the constructor has read initial parameters and created the network socket, 24 | /// the \ref spin method then sets up the services and reads the initial remote 25 | /// connections and topics from the parameter server before going into a loop to read 26 | /// incoming UDP packets and decode them. 27 | /// 28 | /// Meanwhile, the \ref callback method handles sending ROS messages from locally 29 | /// subscribed topics to remote nodes. 30 | /// 31 | /// Additional timer callbacks are used to locally publish and send to remotes 32 | /// information about the local bridge (udp_bridge::UDPBridge::bridgeInfoCallback) and 33 | /// data statistics (udp_bridge::UDPBridge::statsReportCallback). 34 | class UDPBridge 35 | { 36 | public: 37 | UDPBridge(); 38 | 39 | /// Listens in a loop for incoming UDP packets and decodes them. 40 | /// The decode(std::vector const &message, const SourceInfo& source_info) is 41 | /// called when a packet is received. 42 | void spin(); 43 | 44 | private: 45 | /// Sets the node name as seen by other udp_bridge nodes 46 | /// Warns if truncated to size specified in packet header. 47 | void setName(const std::string &name); 48 | 49 | /// Callback method for locally subscribed topics. 50 | /// ShapeShifter is used to be agnostic of message type at compile time. 51 | void callback(const ros::MessageEvent& event); 52 | 53 | /// Decodes outer layer of packets received over the UDP link and calls appropriate 54 | /// handlers based on packet type. 55 | /// @param message packet data as vector of bytes 56 | /// @param source_info info about the packet sender 57 | /// 58 | /// The main packet sorting method. More specific packet decoding routines get 59 | /// selected based on Packet::type. 60 | void decode(std::vector const &message, const SourceInfo& source_info); 61 | 62 | /// Decodes data from a remote subscription received over the UDP link. 63 | /// @param message bytes representing a serialized MessageInternal message 64 | /// @param source_info info about the packet sender 65 | void decodeData(std::vector const &message, const SourceInfo& source_info); 66 | 67 | /// Decodes topic info from remote. 68 | void decodeBridgeInfo(std::vector const &message, const SourceInfo& source_info); 69 | 70 | /// Decodes statistics from remote. 71 | void decodeTopicStatistics(std::vector const &message, const SourceInfo& source_info); 72 | 73 | /// Decodes a request from a remote node to subscribe to a local topic. 74 | void decodeSubscribeRequest(std::vector const &message, const SourceInfo& source_info); 75 | 76 | /// Decodes a request from a remote node to resend packets. 77 | void decodeResendRequest(std::vector const &message, const SourceInfo& source_info); 78 | 79 | /// Unwraps and decodes a packet. 80 | void unwrap(std::vector const &message, const SourceInfo& source_info); 81 | 82 | /// Decodes connection management messages. 83 | void decodeConnection(std::vector const &message, const SourceInfo& source_info); 84 | 85 | /// Service handler for local request to subscribe to a remote topic. 86 | bool remoteSubscribe(udp_bridge::Subscribe::Request &request, udp_bridge::Subscribe::Response &response); 87 | 88 | /// Service handler to advertise on a remote node a local topic. 89 | bool remoteAdvertise(udp_bridge::Subscribe::Request &request, udp_bridge::Subscribe::Response &response); 90 | 91 | /// Service handler to add a named remote 92 | bool addRemote(udp_bridge::AddRemote::Request &request, udp_bridge::AddRemote::Response &response); 93 | 94 | /// Service handler to list named remotes 95 | bool listRemotes(udp_bridge::ListRemotes::Request &request, udp_bridge::ListRemotes::Response &response); 96 | 97 | 98 | /// Map of remotes and connections 99 | using RemoteConnectionsList = std::map >; 100 | 101 | /// Convert a message to a packet and send it to remotes. 102 | template MessageSizeData send(MessageType const &message, const RemoteConnectionsList& remotes, bool is_overhead); 103 | 104 | /// Convert a message to a packet and send it to remote using all connections. 105 | template MessageSizeData send(MessageType const &message, const std::string& remote, bool is_overhead); 106 | 107 | /// Return a list of all remotes. 108 | RemoteConnectionsList allRemotes() const; 109 | 110 | // /// Sends the raw data to the connection. Returns number of bytes sent. 111 | // int send(const std::vector& data, const sockaddr_in* address); 112 | 113 | /// Timer callback where data rate stats are reported 114 | void statsReportCallback(const ros::TimerEvent&); 115 | 116 | /// Timer callback where info on available topics are periodically reported 117 | void bridgeInfoCallback(const ros::TimerEvent&); 118 | 119 | 120 | /// Send topics and remotes info to remotes and publishes locally 121 | void sendBridgeInfo(); 122 | 123 | /// Send pending connection requests 124 | void sendConnectionRequests(); 125 | 126 | /// Splits up a packet, if necessary. 127 | /// Returns a vector of fragment packets, or a vector with the data if fragmenting is not necessary. 128 | std::vector< std::vector > fragment(const std::vector& data); 129 | 130 | /// Remove old buffered packets 131 | void cleanupSentPackets(); 132 | 133 | /// Find missing packet and request resend 134 | void resendMissingPackets(); 135 | 136 | void addSubscriberConnection(std::string const &source_topic, std::string const &destination_topic, uint32_t queue_size, float period, std::string remote_node, std::string connection_id); 137 | 138 | void maximumPacketSizeCallback(const std_msgs::Int32::ConstPtr& msg); 139 | 140 | /// Name used to identify this node to other udp_bridge nodes 141 | std::string name_; 142 | 143 | int m_socket; 144 | uint16_t m_port {4200}; 145 | int m_max_packet_size {65500}; 146 | uint32_t next_fragmented_packet_id_ {0}; 147 | 148 | ros::NodeHandle m_nodeHandle; 149 | 150 | ros::ServiceServer subscribe_service_; 151 | ros::ServiceServer advertise_service_; 152 | ros::ServiceServer add_remote_service_; 153 | ros::ServiceServer list_remotes_service_; 154 | 155 | ros::Publisher m_topicStatisticsPublisher; 156 | ros::Publisher m_bridge_info_publisher; 157 | 158 | ros::Subscriber maximum_packet_size_subscriber_; 159 | 160 | std::map m_subscribers; 161 | 162 | std::map m_publishers; 163 | 164 | ros::Timer stats_report_timer_; 165 | ros::Timer bridge_info_timer_; 166 | 167 | uint64_t next_packet_number_ = 0; 168 | ros::Time last_packet_number_assign_time_; 169 | 170 | uint64_t next_connection_internal_message_sequence_number_ = 0; 171 | 172 | struct PendingConnection 173 | { 174 | ConnectionInternal message; 175 | std::shared_ptr connection; 176 | }; 177 | 178 | /// Map pending remote connections to their message sequence_number. 179 | std::map pending_connections_; 180 | 181 | std::map > remote_nodes_; 182 | }; 183 | 184 | } // namespace udp_bridge 185 | 186 | #endif 187 | -------------------------------------------------------------------------------- /include/udp_bridge/utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_UTILITIES_H 2 | #define UDP_BRIDGE_UTILITIES_H 3 | 4 | #include 5 | 6 | namespace udp_bridge 7 | { 8 | 9 | /// Converts a string to a legal ROS topic name by replacing non-legal characters with legal_char 10 | /// and prepending legal_first if necessary. 11 | std::string topicString(const std::string& topic, bool keep_slashes = false, char legal_first = 'r', char legal_char = '_'); 12 | 13 | } // namespace udp_bridge 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /include/udp_bridge/wrapped_packet.h: -------------------------------------------------------------------------------- 1 | #ifndef UDP_BRIDGE_WRAPPED_PACKET_H 2 | #define UDP_BRIDGE_WRAPPED_PACKET_H 3 | 4 | #include "udp_bridge/packet.h" 5 | #include 6 | 7 | namespace udp_bridge 8 | { 9 | 10 | /// Generates a SequencedPacket that can be sent using UDP. 11 | struct WrappedPacket: SequencedPacketHeader 12 | { 13 | WrappedPacket() = default; 14 | WrappedPacket(uint64_t packet_number, const std::vector& data); 15 | WrappedPacket(const WrappedPacket& other, std::string source_node, std::string connection_id); 16 | std::vector packet; 17 | ros::Time timestamp; 18 | }; 19 | 20 | } // namespace udp_bridge 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /launch/test_fragments.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | name: sender 5 | port: 4200 6 | maxPacketSize: 65 7 | remotes: 8 | receiver: 9 | connections: 10 | default: 11 | host: localhost 12 | port: 4201 13 | topics: 14 | test_topics: 15 | source: sender_topic 16 | destination: receiver_topic 17 | period: 1.0 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /launch/test_mininet_multilink_operator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/test_mininet_multilink_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/test_video.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | name: sender 5 | port: 4200 6 | maxPacketSize: 1400 7 | remotes: 8 | receiver: 9 | connections: 10 | default: 11 | host: localhost 12 | port: 4201 13 | maximumBytesPerSecond: 1000000 14 | topics: 15 | video: 16 | source: image_raw 17 | destination: /bridged/image_raw 18 | period: 1.0 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | See [README.md](README.md) for user documentation. 6 | 7 | \section codeapi Code Design 8 | 9 | See the udp_bridge::UDPBridge class documentation for an overview of the code layout. 10 | 11 | \section testing UDP Bridge Testing 12 | 13 | \subsection unit_testing Unit test 14 | 15 | catkin_make run_tests_udp_bridge 16 | 17 | \subsection testing_fragments Testing fragments 18 | 19 | In multiple terminals: 20 | 21 | roscore 22 | mon launch udp_bridge test_fragments.launch 23 | rostopic pub -r 1 /sender/sender_topic std_msgs/String "data: 'This is a test of sending fragmented packets over udp_bridge with a small packet size setting.'" 24 | rostopic echo /receiver/receiver_topic 25 | rostopic echo /sender/sender_bridge/bridge_info 26 | rostopic echo /sender/sender_bridge/topic_statistics 27 | rqt 28 | 29 | \subsection mininet Simulating network issues 30 | 31 | Following labs demonstrate using mininet to simulate a network and netem to add delays and packet loss. 32 | 33 | http://ce.sc.edu/cyberinfra/workshops/Material/NTP/Lab%201.pdf 34 | http://ce.sc.edu/cyberinfra/workshops/Material/NTP/Lab%202.pdf 35 | http://ce.sc.edu/cyberinfra/workshops/Material/NTP/Lab%203.pdf 36 | http://ce.sc.edu/cyberinfra/workshops/Material/NTP/Lab%204.pdf 37 | 38 | http://mininet.org/ 39 | 40 | \subsubsection installing_mininet Installing latest mininet 41 | 42 | cd ~/src/ 43 | git clone https://github.com/mininet/mininet 44 | PYTHON=python3 ./mininet/util/install.sh -nvf 45 | 46 | 47 | 48 | 49 | */ -------------------------------------------------------------------------------- /msg/BridgeInfo.msg: -------------------------------------------------------------------------------- 1 | # List of local topics and remote connections. 2 | 3 | # Name of the node in the udp_bridge network 4 | string name 5 | 6 | # Time this info was collected/valid 7 | time stamp 8 | 9 | # Local topics available 10 | TopicInfo[] topics 11 | 12 | # Remote nodes known to this node 13 | Remote[] remotes 14 | 15 | int64 next_packet_number 16 | time last_packet_time 17 | 18 | uint16 local_port 19 | int32 maximum_packet_size 20 | -------------------------------------------------------------------------------- /msg/ConnectionInternal.msg: -------------------------------------------------------------------------------- 1 | # This is an internal message used to manage 2 | # connections between nodes 3 | 4 | # ID of the connection 5 | string connection_id 6 | 7 | # increasing number used to match acknowlegements 8 | # with reqests as well as detecting out of order requests 9 | uint64 sequence_number 10 | 11 | uint8 OPERATION_CONNECT = 0 12 | uint8 OPERATION_CONNECT_ACKNOWLEDGE = 1 13 | uint8 OPERATION_DISCONNECT = 2 14 | uint8 OPERATION_DISCONNECT_ACKNOWLEDGE = 3 15 | 16 | uint8 operation 17 | 18 | # return host and port may be useful when a remote udp_bridge 19 | # node is behind a NAT 20 | 21 | # optional host remote should use to reach us 22 | string return_host 23 | 24 | # optional port remote should use to reach us 25 | uint16 return_port 26 | 27 | # optional data rate limit for the return connection 28 | uint32 return_maximum_bytes_per_second 29 | -------------------------------------------------------------------------------- /msg/DataRates.msg: -------------------------------------------------------------------------------- 1 | # rate of data succesfully sent 2 | float32 success_bytes_per_second 3 | 4 | # rate of data that failed to be sent 5 | float32 failed_bytes_per_second 6 | 7 | # rate of data dropped due to rate limit 8 | float32 dropped_bytes_per_second 9 | -------------------------------------------------------------------------------- /msg/MessageInternal.msg: -------------------------------------------------------------------------------- 1 | # Internal message used to transmit a message from a specific topic 2 | string source_topic 3 | string destination_topic 4 | string md5sum 5 | string datatype 6 | string message_definition 7 | uint8[] data 8 | -------------------------------------------------------------------------------- /msg/Remote.msg: -------------------------------------------------------------------------------- 1 | # Message used by BridgeInfo message and ListRemotes service. 2 | # Describes a remote node and connections to it 3 | 4 | # name of the node, each udp_bridge node should have a unique name 5 | string name 6 | 7 | # name sanitized if necessary to be used as a topic name 8 | string topic_name 9 | 10 | RemoteConnection[] connections 11 | -------------------------------------------------------------------------------- /msg/RemoteConnection.msg: -------------------------------------------------------------------------------- 1 | # This message is used as part of the Remote message 2 | # Represents a connection from a node to a remote node 3 | 4 | # ID of the connection 5 | string connection_id 6 | 7 | # host name or ip used to reach the remote node 8 | string host 9 | 10 | # port to use to reach the remote node 11 | uint16 port 12 | 13 | # resolved ip address used to reach the remote node 14 | string ip_address 15 | 16 | # return host and port may be useful when a remote udp_bridge 17 | # node is behind a NAT 18 | 19 | # optional host remote should use to reach us 20 | string return_host 21 | 22 | # optional port remote should use to reach us 23 | uint16 return_port 24 | 25 | # source ip address of incoming packets 26 | string source_ip_address 27 | 28 | # source port of incoming packets 29 | uint16 source_port 30 | 31 | # maximum rate to send on this connection 32 | uint32 maximum_bytes_per_second 33 | 34 | # received data rate via this connection 35 | float32 received_bytes_per_second 36 | 37 | # rate of data received that was already received on another connection 38 | float32 duplicate_bytes_per_second 39 | 40 | # data rates for message packets 41 | DataRates message 42 | 43 | # data rates for overhead messages 44 | DataRates overhead 45 | 46 | # data rates for resent packets 47 | DataRates resend 48 | 49 | -------------------------------------------------------------------------------- /msg/RemoteSubscribeInternal.msg: -------------------------------------------------------------------------------- 1 | # Internal message used to request a topic from a remote over a specific connection. 2 | string source_topic 3 | string destination_topic 4 | uint32 queue_size 5 | float32 period 6 | string connection_id -------------------------------------------------------------------------------- /msg/ResendRequest.msg: -------------------------------------------------------------------------------- 1 | # Internal message used to ask to resend missing packets 2 | uint64[] missing_packets 3 | -------------------------------------------------------------------------------- /msg/TopicInfo.msg: -------------------------------------------------------------------------------- 1 | # This message is used as part of BridgeInfo. 2 | # Information about a local topic 3 | # and the remotes that are subscribed to it. 4 | string topic 5 | string datatype 6 | TopicRemoteDetails[] remotes -------------------------------------------------------------------------------- /msg/TopicRemoteConnectionDetails.msg: -------------------------------------------------------------------------------- 1 | # Part of TopicRemoteDetails specifing details about a specific connection to a remote 2 | 3 | # connection id 4 | string connection_id 5 | 6 | # Optional rate limit for sending topic if positive, topic disabled if negative. 7 | # No rate limit if zero, otherwise, period is 8 | # time in seconds to wait between sending messages from this topic. 9 | float32 period 10 | -------------------------------------------------------------------------------- /msg/TopicRemoteDetails.msg: -------------------------------------------------------------------------------- 1 | # Part of TopicInfo specifing details about a topic send of a specific connection to a remote 2 | string remote 3 | 4 | # The optional different topic used to publish on the remote. 5 | string destination_topic 6 | 7 | TopicRemoteConnectionDetails[] connections 8 | -------------------------------------------------------------------------------- /msg/TopicStatistics.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | string source_node 3 | string destination_node 4 | string connection_id 5 | string source_topic 6 | string destination_topic 7 | 8 | float32 messages_per_second 9 | float32 message_bytes_per_second 10 | 11 | float32 average_fragment_count 12 | 13 | DataRates send 14 | -------------------------------------------------------------------------------- /msg/TopicStatisticsArray.msg: -------------------------------------------------------------------------------- 1 | TopicStatistics[] topics 2 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | udp_bridge 4 | 0.0.0 5 | The udp_bridge package 6 | 7 | Roland Arsenault 8 | 9 | BSD 10 | 11 | catkin 12 | message_generation 13 | message_runtime 14 | roscpp 15 | topic_tools 16 | rostest 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox *.md' 3 | javadoc_autobrief: YES 4 | -------------------------------------------------------------------------------- /src/connection.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/connection.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace udp_bridge 12 | { 13 | 14 | Connection::Connection(std::string id, std::string const &host, uint16_t port, std::string return_host, uint16_t return_port): 15 | id_(id), host_(host), port_(port), return_host_(return_host), return_port_(return_port) 16 | { 17 | resolveHost(); 18 | } 19 | 20 | void Connection::setHostAndPort(const std::string &host, uint16_t port) 21 | { 22 | if(host != host_ || port != port_) 23 | { 24 | host_ = host; 25 | port_ = port; 26 | resolveHost(); 27 | } 28 | } 29 | 30 | const std::string& Connection::sourceIPAddress() const 31 | { 32 | return source_ip_address_; 33 | } 34 | 35 | uint16_t Connection::sourcePort() const 36 | { 37 | return source_port_; 38 | } 39 | 40 | void Connection::setSourceIPAndPort(const std::string &source_ip, uint16_t source_port) 41 | { 42 | source_ip_address_ = source_ip; 43 | source_port_ = source_port; 44 | } 45 | 46 | void Connection::setRateLimit(uint32_t maximum_bytes_per_second) 47 | { 48 | if(maximum_bytes_per_second == 0) 49 | data_rate_limit_ = default_rate_limit; 50 | else 51 | data_rate_limit_ = maximum_bytes_per_second; 52 | } 53 | 54 | uint32_t Connection::rateLimit() const 55 | { 56 | return data_rate_limit_; 57 | } 58 | 59 | void Connection::resolveHost() 60 | { 61 | addresses_.clear(); 62 | 63 | if(host_.empty() || port_ == 0) 64 | return; 65 | 66 | struct addrinfo hints = {0}, *addresses; 67 | 68 | hints.ai_family = AF_INET; 69 | hints.ai_socktype = SOCK_DGRAM; 70 | hints.ai_protocol = IPPROTO_UDP; 71 | 72 | std::string port_string = std::to_string(port_); 73 | 74 | int ret = getaddrinfo(host_.c_str(), port_string.c_str(), &hints, &addresses); 75 | if(ret != 0) 76 | throw std::runtime_error(gai_strerror(ret)); 77 | 78 | for (struct addrinfo *address = addresses; address != nullptr; address = address->ai_next) 79 | { 80 | addresses_.push_back(*reinterpret_cast(address->ai_addr)); 81 | if(addresses_.size() == 1) 82 | ip_address_ = addressToDotted(addresses_.back()); 83 | } 84 | freeaddrinfo(addresses); 85 | } 86 | 87 | const sockaddr_in* Connection::socket_address() const 88 | { 89 | if(addresses_.empty()) 90 | return nullptr; 91 | return &addresses_.front(); 92 | } 93 | 94 | std::string Connection::str() const 95 | { 96 | std::stringstream ret; 97 | ret << host_ << ":" << port_; 98 | return ret.str(); 99 | } 100 | 101 | const std::string& Connection::id() const 102 | { 103 | return id_; 104 | } 105 | 106 | const std::string& Connection::returnHost() const 107 | { 108 | return return_host_; 109 | } 110 | 111 | void Connection::setReturnHostAndPort(const std::string &return_host, uint16_t return_port) 112 | { 113 | return_host_ = return_host; 114 | return_port_ = return_port; 115 | } 116 | 117 | uint16_t Connection::returnPort() const 118 | { 119 | return return_port_; 120 | } 121 | 122 | const std::string& Connection::host() const 123 | { 124 | return host_; 125 | } 126 | 127 | uint16_t Connection::port() const 128 | { 129 | return port_; 130 | } 131 | 132 | const std::string& Connection::ip_address() const 133 | { 134 | return ip_address_; 135 | } 136 | 137 | std::string Connection::ip_address_with_port() const 138 | { 139 | return ip_address_+":"+std::to_string(port_); 140 | } 141 | 142 | const double& Connection::last_receive_time() const 143 | { 144 | return last_receive_time_; 145 | } 146 | 147 | void Connection::update_last_receive_time(double t, int data_size, bool duplicate) 148 | { 149 | last_receive_time_ = t; 150 | ReceivedSize rs; 151 | rs.size = data_size; 152 | rs.duplicate = duplicate; 153 | data_size_received_history_[t].push_back(rs); 154 | } 155 | 156 | 157 | SendResult Connection::send(const std::vector& packets, int socket, const std::string& remote, bool is_overhead) 158 | { 159 | uint32_t total_size = 0; 160 | for(const auto& p: packets) 161 | total_size += p.packet_size; 162 | 163 | auto now = ros::Time::now(); 164 | if(!sent_packet_statistics_.can_send(total_size, data_rate_limit_, now)) 165 | { 166 | for(const auto& p: packets) 167 | { 168 | 169 | PacketSizeData size_data; 170 | size_data.timestamp = now; 171 | size_data.size = p.packet_size; 172 | if(is_overhead) 173 | size_data.category = PacketSendCategory::overhead; 174 | else 175 | size_data.category = PacketSendCategory::message; 176 | size_data.send_result = SendResult::dropped; 177 | sent_packet_statistics_.add(size_data); 178 | } 179 | return SendResult::dropped; 180 | } 181 | 182 | SendResult ret = SendResult::success; 183 | for(const auto& p: packets) 184 | { 185 | auto packet = WrappedPacket(p, remote, id_); 186 | sent_packets_[packet.packet_number] = packet; 187 | PacketSendCategory category = PacketSendCategory::message; 188 | if(is_overhead) 189 | category = PacketSendCategory::overhead; 190 | auto send_ret = send(packet.packet, socket, category); 191 | if(send_ret.send_result != SendResult::success) 192 | ret = send_ret.send_result; 193 | } 194 | return ret; 195 | } 196 | 197 | 198 | PacketSizeData Connection::send(const std::vector &data, int socket, PacketSendCategory category) 199 | { 200 | PacketSizeData ret; 201 | ret.timestamp = ros::Time::now(); 202 | ret.size = data.size(); 203 | ret.category = category; 204 | ret.send_result = SendResult::failed; 205 | if(addresses_.empty()) 206 | { 207 | sent_packet_statistics_.add(ret); 208 | return ret; 209 | } 210 | 211 | if(!sent_packet_statistics_.can_send(data.size(), data_rate_limit_, ret.timestamp)) 212 | { 213 | ret.send_result = SendResult::dropped; 214 | sent_packet_statistics_.add(ret); 215 | return ret; 216 | } 217 | 218 | int bytes_sent = 0; 219 | try 220 | { 221 | int tries = 0; 222 | while (true) 223 | { 224 | pollfd p; 225 | p.fd = socket; 226 | p.events = POLLOUT; 227 | int poll_ret = poll(&p, 1, 10); 228 | if(poll_ret > 0 && p.revents & POLLOUT) 229 | { 230 | bytes_sent = sendto(socket, data.data(), data.size(), 0, reinterpret_cast(addresses_.data()), sizeof(sockaddr_in)); 231 | if(bytes_sent == -1) 232 | switch(errno) 233 | { 234 | case EAGAIN: 235 | tries+=1; 236 | break; 237 | case ECONNREFUSED: 238 | sent_packet_statistics_.add(ret); 239 | return ret; 240 | default: 241 | throw(ConnectionException(strerror(errno))); 242 | } 243 | if(bytes_sent < data.size()) 244 | throw(ConnectionException("only "+std::to_string(bytes_sent) +" of " +std::to_string(data.size()) + " sent")); 245 | else 246 | { 247 | ret.send_result = SendResult::success; 248 | sent_packet_statistics_.add(ret); 249 | return ret; 250 | } 251 | } 252 | else 253 | tries+=1; 254 | 255 | if(tries >= 20) 256 | if(poll_ret == 0) 257 | throw(ConnectionException("Timeout")); 258 | else 259 | throw(ConnectionException(std::to_string(errno) +": "+ strerror(errno))); 260 | } 261 | } 262 | catch(const ConnectionException& e) 263 | { 264 | ROS_WARN_STREAM("error sending data of size " << data.size() << ": " << e.getMessage()); 265 | } 266 | sent_packet_statistics_.add(ret); 267 | return ret; 268 | } 269 | 270 | std::pair Connection::data_receive_rate(double time) 271 | { 272 | double five_secs_ago = time - 5; 273 | while(!data_size_received_history_.empty() && data_size_received_history_.begin()->first < five_secs_ago) 274 | data_size_received_history_.erase(data_size_received_history_.begin()); 275 | 276 | double dt = 1.0; 277 | if(!data_size_received_history_.empty()) 278 | dt = std::max(dt, time - data_size_received_history_.begin()->first); 279 | 280 | uint32_t unique_sum = 0; 281 | uint32_t duplicate_sum = 0; 282 | for(auto dsv: data_size_received_history_) 283 | for(auto ds: dsv.second) 284 | { 285 | if(ds.duplicate) 286 | duplicate_sum += ds.size; 287 | else 288 | unique_sum += ds.size; 289 | } 290 | 291 | return std::make_pair(unique_sum/dt, duplicate_sum/dt); 292 | } 293 | 294 | DataRates Connection::data_sent_rate(ros::Time time, PacketSendCategory category) 295 | { 296 | return sent_packet_statistics_.get(category); 297 | } 298 | 299 | 300 | void Connection::resend_packets(const std::vector &missing_packets, int socket) 301 | { 302 | for(auto packet_number: missing_packets) 303 | { 304 | auto packet_to_resend = sent_packets_.find(packet_number); 305 | if(packet_to_resend != sent_packets_.end()) 306 | send(packet_to_resend->second.packet, socket, PacketSendCategory::resend); 307 | } 308 | } 309 | 310 | void Connection::cleanup_sent_packets(ros::Time cutoff_time) 311 | { 312 | std::vector expired; 313 | for(auto sp: sent_packets_) 314 | if(sp.second.timestamp < cutoff_time) 315 | expired.push_back(sp.first); 316 | for(auto e: expired) 317 | sent_packets_.erase(e); 318 | } 319 | 320 | 321 | 322 | 323 | std::string addressToDotted(const sockaddr_in& address) 324 | { 325 | return std::to_string(reinterpret_cast(&address.sin_addr.s_addr)[0])+"."+ 326 | std::to_string(reinterpret_cast(&address.sin_addr.s_addr)[1])+"."+ 327 | std::to_string(reinterpret_cast(&address.sin_addr.s_addr)[2])+"."+ 328 | std::to_string(reinterpret_cast(&address.sin_addr.s_addr)[3]); 329 | } 330 | 331 | } // namespace udp_bridge 332 | -------------------------------------------------------------------------------- /src/defragmenter.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/defragmenter.h" 2 | 3 | namespace udp_bridge 4 | { 5 | 6 | bool Defragmenter::addFragment(std::vector fragment) 7 | { 8 | Fragment* fragment_packet = reinterpret_cast(fragment.data()); 9 | if(fragment_packet->type == PacketType::Fragment) 10 | { 11 | ROS_DEBUG_STREAM("Fragment " << int(fragment_packet->fragment_number) << " of " << int(fragment_packet->fragment_count) << " for packet " << fragment_packet->packet_id); 12 | if(fragment_map_.find(fragment_packet->packet_id) == fragment_map_.end()) 13 | { 14 | fragment_map_[fragment_packet->packet_id].first_arrival_time = ros::Time::now(); 15 | fragment_map_[fragment_packet->packet_id].fragment_count = fragment_packet->fragment_count; 16 | } 17 | fragment_map_[fragment_packet->packet_id].fragment_map[fragment_packet->fragment_number] = std::vector(fragment.begin()+sizeof(FragmentHeader), fragment.end()); 18 | if(fragment_map_[fragment_packet->packet_id].fragment_map.size() < fragment_packet->fragment_count) 19 | return false; 20 | ROS_DEBUG_STREAM("assembling packet from " << int(fragment_packet->fragment_count) << " fragments"); 21 | complete_packets_.push_back(std::vector()); 22 | for(auto f: fragment_map_[fragment_packet->packet_id].fragment_map) 23 | complete_packets_.back().insert(complete_packets_.back().end(),f.second.begin(),f.second.end()); 24 | fragment_map_.erase(fragment_packet->packet_id); 25 | return true; 26 | } 27 | else 28 | ROS_WARN_STREAM("Trying to add a fragment from a packet of type " << int(fragment_packet->type)); 29 | return false; 30 | } 31 | 32 | std::vector > Defragmenter::getPackets() 33 | { 34 | return std::move(complete_packets_); 35 | } 36 | 37 | int Defragmenter::cleanup(ros::Duration maxAge) 38 | { 39 | ros::Time now = ros::Time::now(); 40 | if(!now.is_zero()) 41 | { 42 | std::vector discard_pile; 43 | ros::Time discard_time = now-maxAge; 44 | for(auto fragments: fragment_map_) 45 | if(fragments.second.first_arrival_time < discard_time) 46 | discard_pile.push_back(fragments.first); 47 | for(auto id: discard_pile) 48 | fragment_map_.erase(id); 49 | return discard_pile.size(); 50 | } 51 | return 0; 52 | } 53 | 54 | } 55 | -------------------------------------------------------------------------------- /src/packet.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/packet.h" 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace udp_bridge 13 | { 14 | 15 | template<> PacketType packetTypeOf(const MessageInternal&) { return PacketType::Data;} 16 | template<> PacketType packetTypeOf(const RemoteSubscribeInternal&) { return PacketType::SubscribeRequest;} 17 | template<> PacketType packetTypeOf(const BridgeInfo&) { return PacketType::BridgeInfo;} 18 | template<> PacketType packetTypeOf(const TopicStatisticsArray&) { return PacketType::TopicStatistics;} 19 | template<> PacketType packetTypeOf(const ResendRequest&) { return PacketType::ResendRequest;} 20 | template<> PacketType packetTypeOf(const ConnectionInternal&) { return PacketType::Connection;} 21 | 22 | 23 | std::vector compress(const std::vector& data) 24 | { 25 | uLong compressed_buffer_size = compressBound(data.size()); 26 | std::vector compressed_buffer(sizeof(CompressedPacketHeader)+compressed_buffer_size); 27 | CompressedPacket * compressed_packet = reinterpret_cast(compressed_buffer.data()); 28 | int compress_return = ::compress(compressed_packet->compressed_data, &compressed_buffer_size, data.data(), data.size()); 29 | if(compress_return == Z_OK) 30 | { 31 | compressed_packet->type = PacketType::Compressed; 32 | compressed_packet->uncompressed_size = data.size(); 33 | compressed_buffer.resize(sizeof(CompressedPacketHeader)+compressed_buffer_size); 34 | if(compressed_buffer.size() < data.size()) 35 | return compressed_buffer; 36 | } 37 | return data; 38 | } 39 | 40 | std::vector uncompress(std::vector const &data) 41 | { 42 | uLong comp_size = data.size()-sizeof(CompressedPacketHeader); 43 | const CompressedPacket *compressed_packet = reinterpret_cast(data.data()); 44 | uLong decomp_size = compressed_packet->uncompressed_size; 45 | std::vector ret(decomp_size); 46 | int decomp_ret = ::uncompress(ret.data(),&decomp_size,compressed_packet->compressed_data,comp_size); 47 | if(decomp_ret != Z_OK) 48 | { 49 | std::cerr << "uncompress: comp size: " << comp_size << std::endl; 50 | std::cerr << "uncompress: decomp size in: " << compressed_packet->uncompressed_size << std::endl; 51 | std::cerr << "uncompress: decomp size out: " << decomp_size << std::endl; 52 | switch(decomp_ret) 53 | { 54 | case Z_MEM_ERROR: 55 | std::cerr << "uncompress: not enough memory" << std::endl; 56 | break; 57 | case Z_BUF_ERROR: 58 | std::cerr << "uncompress: not enough room in output buffer" << std::endl; 59 | break; 60 | case Z_DATA_ERROR: 61 | std::cerr << "uncompress: corrupt or incomplete input data" << std::endl; 62 | break; 63 | } 64 | ret.clear(); 65 | } 66 | return ret; 67 | } 68 | 69 | 70 | 71 | } // namespace udp_bridge 72 | -------------------------------------------------------------------------------- /src/remote_node.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/remote_node.h" 2 | #include "udp_bridge/BridgeInfo.h" 3 | #include "udp_bridge/connection.h" 4 | #include "udp_bridge/utilities.h" 5 | 6 | namespace udp_bridge 7 | { 8 | 9 | RemoteNode::RemoteNode(std::string remote_name, std::string local_name): name_(remote_name), local_name_(local_name) 10 | { 11 | assert(remote_name != local_name); 12 | ros::NodeHandle private_nodeHandle("~"); 13 | 14 | bridge_info_publisher_ = private_nodeHandle.advertise("remotes/"+topicName()+"/bridge_info",1,true); 15 | 16 | topic_statistics_publisher_ = private_nodeHandle.advertise("remotes/"+topicName()+"/topic_statistics",1,true); 17 | } 18 | 19 | void RemoteNode::update(const Remote& remote_message) 20 | { 21 | assert(name_ == remote_message.name); 22 | for(auto c: remote_message.connections) 23 | { 24 | auto connection = connections_[c.connection_id]; 25 | if(!connection) 26 | connection = newConnection(c.connection_id, c.host, c.port); 27 | connection->setReturnHostAndPort(c.return_host, c.return_port); 28 | connection->setRateLimit(c.maximum_bytes_per_second); 29 | } 30 | } 31 | 32 | void RemoteNode::update(const BridgeInfo& bridge_info, const SourceInfo& source_info) 33 | { 34 | bridge_info_publisher_.publish(bridge_info); 35 | for(auto remote: bridge_info.remotes) 36 | if(remote.name == local_name_) 37 | { 38 | for(auto connection_info: remote.connections) 39 | { 40 | auto c = connection(connection_info.connection_id); 41 | auto host = connection_info.return_host; 42 | if(host.empty()) 43 | host = source_info.host; 44 | auto port = connection_info.return_port; 45 | if(port == 0) 46 | port = source_info.port; 47 | if(!c) 48 | c = newConnection(connection_info.connection_id, host, port); 49 | else 50 | { 51 | if(!connection_info.return_host.empty() || connection_info.return_port != 0) 52 | c->setHostAndPort(connection_info.return_host, connection_info.return_port); 53 | } 54 | c->setSourceIPAndPort(source_info.host, source_info.port); 55 | } 56 | if(bridge_info.next_packet_number < next_packet_number_) 57 | { 58 | ROS_WARN_STREAM("Received next packet number that is less than previous one: " << bridge_info.next_packet_number << " time: " << bridge_info.last_packet_time << " (previous: " << next_packet_number_ << " time: " << last_packet_time_ << ")"); 59 | if(bridge_info.next_packet_number == 0 || bridge_info.last_packet_time > last_packet_time_) 60 | { 61 | // Try to detect remote restart of udp_bridge. Assume a reset if next packet number is zero 62 | // or if a timestamp for a lower numbered packet is greater than what we last saw for a larger 63 | // packet number. 64 | ROS_WARN_STREAM("Assuming remote udp_bridge restart"); 65 | received_packet_times_.clear(); 66 | resend_request_times_.clear(); 67 | } 68 | } 69 | next_packet_number_ = bridge_info.next_packet_number; 70 | last_packet_time_ = bridge_info.last_packet_time; 71 | } 72 | } 73 | 74 | 75 | std::string RemoteNode::topicName() const 76 | { 77 | return topicString(name_); 78 | } 79 | 80 | std::shared_ptr RemoteNode::connection(std::string connection_id) 81 | { 82 | if(connections_.find(connection_id) != connections_.end()) 83 | return connections_[connection_id]; 84 | return {}; 85 | } 86 | 87 | std::vector > RemoteNode::connections() 88 | { 89 | std::vector > ret; 90 | for(auto connection: connections_) 91 | if(connection.second) 92 | ret.push_back(connection.second); 93 | return ret; 94 | } 95 | 96 | std::shared_ptr RemoteNode::newConnection(std::string connection_id, std::string host, uint16_t port) 97 | { 98 | connections_[connection_id] = std::make_shared(connection_id, host, port); 99 | return connections_[connection_id]; 100 | } 101 | 102 | 103 | std::vector RemoteNode::unwrap(std::vector const &message, const SourceInfo& source_info) 104 | { 105 | if(message.size() < sizeof(SequencedPacketHeader) || reinterpret_cast(message.data())->packet_size != message.size()) 106 | { 107 | ROS_ERROR_STREAM("Can't unwrap packet of size " << message.size()); 108 | if(message.size() >= sizeof(SequencedPacketHeader)) 109 | ROS_ERROR_STREAM("Packet reports size: " << reinterpret_cast(message.data())->packet_size); 110 | } 111 | else 112 | { 113 | auto packet = reinterpret_cast(message.data()); 114 | bool duplicate = received_packet_times_.find(packet->packet_number) != received_packet_times_.end(); 115 | auto c = connection(packet->connection_id); 116 | if(!c) 117 | c = newConnection(packet->connection_id, source_info.host, source_info.port); 118 | c->update_last_receive_time(ros::Time::now().toSec(), message.size(), duplicate); 119 | if(!duplicate) 120 | { 121 | received_packet_times_[packet->packet_number] = ros::Time::now(); 122 | try 123 | { 124 | return std::vector(message.begin()+sizeof(SequencedPacketHeader), message.end()); 125 | } 126 | catch(const std::exception& e) 127 | { 128 | ROS_ERROR_STREAM("problem decoding packet: " << e.what()); 129 | } 130 | } 131 | } 132 | return {}; 133 | } 134 | 135 | Defragmenter& RemoteNode::defragmenter() 136 | { 137 | return defragmenter_; 138 | } 139 | 140 | void RemoteNode::publishTopicStatistics(const TopicStatisticsArray& statistics) 141 | { 142 | topic_statistics_publisher_.publish(statistics); 143 | } 144 | 145 | void RemoteNode::clearReceivedPacketTimesBefore(ros::Time time) 146 | { 147 | std::vector expired; 148 | for(auto pt: received_packet_times_) 149 | if(pt.second < time) 150 | expired.push_back(pt.first); 151 | for(auto e: expired) 152 | received_packet_times_.erase(e); 153 | 154 | expired.clear(); 155 | for(auto rt: resend_request_times_) 156 | if(rt.second < time) 157 | expired.push_back(rt.first); 158 | for(auto e: expired) 159 | resend_request_times_.erase(e); 160 | } 161 | 162 | ResendRequest RemoteNode::getMissingPackets() 163 | { 164 | ros::Time now = ros::Time::now(); 165 | if(now.isValid() && !now.isZero()) 166 | { 167 | ros::Time too_old = now - ros::Duration(5.0); 168 | clearReceivedPacketTimesBefore(too_old); 169 | ros::Time can_resend_time = now - ros::Duration(0.2); 170 | 171 | std::vector missing; 172 | if(!received_packet_times_.empty()) 173 | for(auto i = received_packet_times_.begin()->first+1; i < received_packet_times_.rbegin()->first; i++) 174 | if(received_packet_times_.find(i) == received_packet_times_.end()) 175 | missing.push_back(i); 176 | ResendRequest rr; 177 | for(auto m: missing) 178 | { 179 | if(resend_request_times_[m] < can_resend_time) 180 | { 181 | rr.missing_packets.push_back(m); 182 | resend_request_times_[m] = now; 183 | } 184 | } 185 | return rr; 186 | } 187 | return {}; 188 | } 189 | 190 | } // namespace udp_bridge 191 | -------------------------------------------------------------------------------- /src/ros_messages.h: -------------------------------------------------------------------------------- 1 | // this is used for doxygen only. 2 | #error 3 | 4 | /// \defgroup messages ROS Messages 5 | /// ROS messages and services defined and used by udp_bridge 6 | 7 | namespace udp_bridge 8 | { 9 | /// See: udp_bridge/BridgeInfo. 10 | /// \ingroup packet messages 11 | class BridgeInfo{}; 12 | 13 | /// See: udp_bridge/MessageInternal. 14 | /// \ingroup packet messages 15 | class MessageInternal{}; 16 | 17 | /// See: udp_bridge/Remote. 18 | /// \ingroup messages 19 | class Remote{}; 20 | 21 | /// See: udp_bridge/RemoteConnection. 22 | /// \ingroup messages 23 | class RemoteConnection{}; 24 | 25 | /// See: udp_bridge/RemoteSubscribeInternal. 26 | /// \ingroup packet messages 27 | class RemoteSubscribeInternal{}; 28 | 29 | /// See: udp_bridge/ResendRequest. 30 | /// \ingroup packet messages 31 | class ResendRequest{}; 32 | 33 | /// See: udp_bridge/TopicInfo. 34 | /// \ingroup messages 35 | class TopicInfo{}; 36 | 37 | /// See: udp_bridge/TopicStatistics. 38 | /// \ingroup messages 39 | class TopicStatistics{}; 40 | 41 | 42 | 43 | /// See: udp_bridge/AddRemote. 44 | /// \ingroup messages 45 | class AddRemote{}; 46 | 47 | /// See: udp_bridge/ListRemotes. 48 | /// \ingroup messages 49 | class ListRemotes{}; 50 | 51 | /// See: udp_bridge/Subscribe. 52 | /// \ingroup messages 53 | class Subscribe{}; 54 | 55 | } 56 | -------------------------------------------------------------------------------- /src/statistics.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/statistics.h" 2 | 3 | namespace udp_bridge 4 | { 5 | 6 | void MessageStatistics::add(const MessageSizeData& data) 7 | { 8 | ROS_DEBUG_STREAM_NAMED("statistics", "msg size: " << data.message_size << " sent size: " << data.sent_size); 9 | for(auto result: data.send_results) 10 | { 11 | ROS_DEBUG_STREAM_NAMED("statistics", " remote: " << result.first); 12 | for(auto connection: result.second) 13 | switch (connection.second) 14 | { 15 | case SendResult::success: 16 | ROS_DEBUG_STREAM_NAMED("statistics", " " << connection.first << " send result: success"); 17 | break; 18 | case SendResult::failed: 19 | ROS_DEBUG_STREAM_NAMED("statistics", " " << connection.first << " send result: failed"); 20 | break; 21 | case SendResult::dropped: 22 | ROS_DEBUG_STREAM_NAMED("statistics", " " << connection.first << " send result: dropped"); 23 | break; 24 | } 25 | } 26 | 27 | Statistics::add(data); 28 | } 29 | 30 | std::vector MessageStatistics::get() 31 | { 32 | struct Totals 33 | { 34 | int total_message_size = 0; 35 | int total_fragment_count = 0; 36 | int total_ok_sent_bytes = 0; 37 | int total_failed_sent_bytes = 0; 38 | uint32_t total_dropped_bytes = 0; 39 | int total_data_point_count = 0; 40 | ros::Time earliest; 41 | ros::Time latest; 42 | }; 43 | 44 | std::map, Totals> totals_by_connection; 45 | 46 | for(auto data_point: data_) 47 | for(auto remote_send_result: data_point.send_results) 48 | for(auto connection_send_result: remote_send_result.second) 49 | { 50 | Totals& totals = totals_by_connection[std::make_pair(remote_send_result.first, connection_send_result.first)]; 51 | totals.total_message_size += data_point.message_size; 52 | totals.total_fragment_count += data_point.fragment_count; 53 | switch(connection_send_result.second) 54 | { 55 | case SendResult::success: 56 | totals.total_ok_sent_bytes += data_point.sent_size; 57 | break; 58 | case SendResult::failed: 59 | totals.total_failed_sent_bytes += data_point.sent_size; 60 | break; 61 | case SendResult::dropped: 62 | totals.total_dropped_bytes += data_point.sent_size; 63 | break; 64 | } 65 | totals.total_data_point_count++; 66 | if(data_point.timestamp > totals.latest) 67 | totals.latest = data_point.timestamp; 68 | if(totals.earliest.isZero() || data_point.timestamp < totals.earliest) 69 | totals.earliest = data_point.timestamp; 70 | } 71 | 72 | std::vector ret; 73 | 74 | for(auto totals: totals_by_connection) 75 | { 76 | TopicStatistics ts; 77 | ts.destination_node = totals.first.first; 78 | ts.connection_id = totals.first.second; 79 | auto& data = totals.second; 80 | ts.stamp = data.latest; 81 | float count = data.total_data_point_count; 82 | ts.average_fragment_count = data.total_fragment_count/count; 83 | 84 | if(data.latest > data.earliest) 85 | { 86 | double time_span = (data.latest-data.earliest).toSec(); 87 | 88 | // account for the time until the next sample 89 | time_span *= (count+1)/count; 90 | 91 | ts.messages_per_second = count/time_span; 92 | ts.message_bytes_per_second = data.total_message_size/time_span; 93 | ts.send.success_bytes_per_second = data.total_ok_sent_bytes/time_span; 94 | ts.send.failed_bytes_per_second = data.total_failed_sent_bytes/time_span; 95 | ts.send.dropped_bytes_per_second = data.total_dropped_bytes/time_span; 96 | } 97 | 98 | ret.push_back(ts); 99 | 100 | } 101 | 102 | return ret; 103 | } 104 | 105 | DataRates PacketSendStatistics::get() const 106 | { 107 | return get(nullptr); 108 | } 109 | 110 | DataRates PacketSendStatistics::get(PacketSendCategory category) const 111 | { 112 | return get(&category); 113 | } 114 | 115 | DataRates PacketSendStatistics::get(PacketSendCategory* category) const 116 | { 117 | DataRates ret; 118 | ros::Time earliest; 119 | ros::Time latest; 120 | 121 | for(auto data_point: data_) 122 | if(category==nullptr || data_point.category == *category) 123 | { 124 | switch(data_point.send_result) 125 | { 126 | case SendResult::success: 127 | ret.success_bytes_per_second += data_point.size; 128 | break; 129 | case SendResult::failed: 130 | ret.failed_bytes_per_second += data_point.size; 131 | break; 132 | case SendResult::dropped: 133 | ret.dropped_bytes_per_second += data_point.size; 134 | break; 135 | } 136 | if(data_point.timestamp > latest) 137 | latest = data_point.timestamp; 138 | if(earliest.isZero() || data_point.timestamp < earliest) 139 | earliest = data_point.timestamp; 140 | } 141 | 142 | // If time span is less than 1 sec, assume it's 1 sec 143 | // to avoid data rate spikes at the start of sampling 144 | if(latest > earliest) 145 | { 146 | double time_span = (latest-earliest).toSec(); 147 | if(time_span > 1.0) 148 | { 149 | ret.success_bytes_per_second /= time_span; 150 | ret.failed_bytes_per_second /= time_span; 151 | ret.dropped_bytes_per_second /= time_span; 152 | } 153 | } 154 | return ret; 155 | } 156 | 157 | bool PacketSendStatistics::can_send(uint32_t data_size, uint32_t bytes_per_second_limit, ros::Time time) const 158 | { 159 | auto one_second_ago = time - ros::Duration(1.0); 160 | auto start = data_.begin(); 161 | while(start != data_.end() && start->timestamp < one_second_ago) 162 | start++; 163 | uint32_t total_sent = 0; 164 | while(start != data_.end()) 165 | { 166 | if(start->send_result != SendResult::dropped) 167 | total_sent += start->size; 168 | start++; 169 | } 170 | return (total_sent+data_size) < bytes_per_second_limit; 171 | } 172 | 173 | 174 | } // namespace udp_bridge 175 | -------------------------------------------------------------------------------- /src/udp_bridge.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/udp_bridge.h" 2 | 3 | #include "ros/ros.h" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "udp_bridge/RemoteSubscribeInternal.h" 11 | #include "udp_bridge/MessageInternal.h" 12 | #include "udp_bridge/TopicStatisticsArray.h" 13 | #include "udp_bridge/BridgeInfo.h" 14 | #include "udp_bridge/ResendRequest.h" 15 | #include "udp_bridge/remote_node.h" 16 | #include "udp_bridge/types.h" 17 | #include "udp_bridge/utilities.h" 18 | 19 | namespace udp_bridge 20 | { 21 | 22 | UDPBridge::UDPBridge() 23 | { 24 | auto name = ros::this_node::getName(); 25 | auto last_slash = name.rfind('/'); 26 | if(last_slash != std::string::npos) 27 | name = name.substr(last_slash+1); 28 | setName(ros::param::param("~name", name)); 29 | 30 | ROS_INFO_STREAM("name: " << name_); 31 | 32 | m_port = ros::param::param("~port", m_port); 33 | ROS_INFO_STREAM("port: " << m_port); 34 | 35 | m_max_packet_size = ros::param::param("~maxPacketSize", m_max_packet_size); 36 | ROS_INFO_STREAM("maxPacketSize: " << m_max_packet_size); 37 | 38 | maximum_packet_size_subscriber_ = ros::NodeHandle("~").subscribe("maximum_packet_size", 1, &UDPBridge::maximumPacketSizeCallback, this); 39 | 40 | m_socket = socket(AF_INET, SOCK_DGRAM, 0); 41 | if(m_socket < 0) 42 | { 43 | ROS_ERROR("Failed creating socket"); 44 | exit(1); 45 | } 46 | 47 | sockaddr_in bind_address; // address to listen on 48 | memset((char *)&bind_address, 0, sizeof(bind_address)); 49 | bind_address.sin_family = AF_INET; 50 | bind_address.sin_addr.s_addr = htonl(INADDR_ANY); 51 | bind_address.sin_port = htons(m_port); 52 | 53 | if(bind(m_socket, (sockaddr*)&bind_address, sizeof(bind_address)) < 0) 54 | { 55 | ROS_ERROR("Error binding socket"); 56 | exit(1); 57 | } 58 | 59 | timeval socket_timeout; 60 | socket_timeout.tv_sec = 0; 61 | socket_timeout.tv_usec = 1000; 62 | if(setsockopt(m_socket, SOL_SOCKET, SO_RCVTIMEO, &socket_timeout, sizeof(socket_timeout)) < 0) 63 | { 64 | ROS_ERROR("Error setting socket timeout"); 65 | exit(1); 66 | } 67 | 68 | int buffer_size; 69 | unsigned int s = sizeof(buffer_size); 70 | getsockopt(m_socket, SOL_SOCKET, SO_RCVBUF, (void*)&buffer_size, &s); 71 | ROS_INFO_STREAM("recv buffer size:" << buffer_size); 72 | buffer_size = 500000; 73 | setsockopt(m_socket, SOL_SOCKET, SO_RCVBUF, &buffer_size, sizeof(buffer_size)); 74 | getsockopt(m_socket, SOL_SOCKET, SO_RCVBUF, (void*)&buffer_size, &s); 75 | ROS_INFO_STREAM("recv buffer size set to:" << buffer_size); 76 | buffer_size = 500000; 77 | setsockopt(m_socket, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size)); 78 | getsockopt(m_socket, SOL_SOCKET, SO_SNDBUF, (void*)&buffer_size, &s); 79 | ROS_INFO_STREAM("send buffer size set to:" << buffer_size); 80 | 81 | ros::NodeHandle private_nodeHandle("~"); 82 | subscribe_service_ = private_nodeHandle.advertiseService("remote_subscribe", &UDPBridge::remoteSubscribe, this); 83 | advertise_service_ = private_nodeHandle.advertiseService("remote_advertise", &UDPBridge::remoteAdvertise, this); 84 | 85 | add_remote_service_ = private_nodeHandle.advertiseService("add_remote", &UDPBridge::addRemote, this); 86 | list_remotes_service_ = private_nodeHandle.advertiseService("list_remotes", &UDPBridge::listRemotes, this); 87 | 88 | m_topicStatisticsPublisher = private_nodeHandle.advertise("topic_statistics",10,false); 89 | m_bridge_info_publisher = private_nodeHandle.advertise("bridge_info", 1, true); 90 | 91 | XmlRpc::XmlRpcValue remotes_dict; 92 | if(private_nodeHandle.getParam("remotes",remotes_dict)) 93 | { 94 | if(remotes_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct) 95 | { 96 | for(auto remote:remotes_dict) 97 | { 98 | Remote remote_info; 99 | remote_info.name = remote.first; 100 | if(remote.second.hasMember("name")) 101 | remote_info.name = std::string(remote.second["name"]); 102 | 103 | remote_nodes_[remote_info.name] = std::make_shared(remote_info.name, name_); 104 | remote_nodes_[remote_info.name]->update(remote_info); 105 | 106 | if(remote.second.hasMember("connections")) 107 | for(auto connection_info: remote.second["connections"]) 108 | { 109 | RemoteConnection connection; 110 | connection.connection_id = connection_info.first; 111 | if(connection_info.second.hasMember("host")) 112 | connection.host = std::string(connection_info.second["host"]); 113 | connection.port = 0; 114 | if(connection_info.second.hasMember("port")) 115 | connection.port = int(connection_info.second["port"]); 116 | if(connection_info.second.hasMember("returnHost")) 117 | connection.return_host = std::string(connection_info.second["returnHost"]); 118 | connection.return_port = 0; 119 | if(connection_info.second.hasMember("returnPort")) 120 | connection.return_port =int(connection_info.second["remotePort"]); 121 | if(connection_info.second.hasMember("maximumBytesPerSecond")) 122 | { 123 | auto mbps = int(connection_info.second["maximumBytesPerSecond"]); 124 | if(mbps > 0) 125 | connection.maximum_bytes_per_second = mbps; 126 | } 127 | remote_info.connections.push_back(connection); 128 | remote_nodes_[remote_info.name]->update(remote_info); 129 | 130 | if(connection_info.second.hasMember("topics")) 131 | for(auto topic: connection_info.second["topics"]) 132 | { 133 | int queue_size = 1; 134 | if (topic.second.hasMember("queue_size")) 135 | queue_size = topic.second["queue_size"]; 136 | double period = 0.0; 137 | if (topic.second.hasMember("period")) 138 | period = topic.second["period"]; 139 | std::string source = topic.first; 140 | if (topic.second.hasMember("source")) 141 | { 142 | source = std::string(topic.second["source"]); 143 | source = ros::names::resolve(source); 144 | std::string destination = source; 145 | if (topic.second.hasMember("destination")) 146 | destination = std::string(topic.second["destination"]); 147 | addSubscriberConnection(source, destination, 1, period, remote_info.name, connection.connection_id); 148 | } 149 | } 150 | } 151 | } 152 | } 153 | } 154 | 155 | stats_report_timer_ = m_nodeHandle.createTimer(ros::Duration(1.0), &UDPBridge::statsReportCallback, this); 156 | bridge_info_timer_ = m_nodeHandle.createTimer(ros::Duration(2.0), &UDPBridge::bridgeInfoCallback, this); 157 | } 158 | 159 | void UDPBridge::setName(const std::string &name) 160 | { 161 | name_ = name; 162 | if(name_.size() >= maximum_node_name_size-1) 163 | { 164 | name_ = name_.substr(0, maximum_node_name_size-1); 165 | ROS_WARN_STREAM("udp_bridge name truncated to " << name_); 166 | } 167 | } 168 | 169 | void UDPBridge::spin() 170 | { 171 | 172 | while(ros::ok()) 173 | { 174 | sockaddr_in remote_address; 175 | socklen_t remote_address_length = sizeof(remote_address); 176 | while(true) 177 | { 178 | pollfd p; 179 | p.fd = m_socket; 180 | p.events = POLLIN; 181 | int ret = poll(&p, 1, 10); 182 | if(ret < 0) 183 | ROS_WARN_STREAM("poll error: " << int(errno) << " " << strerror(errno)); 184 | if(ret > 0 && p.revents & POLLIN) 185 | { 186 | int receive_length = 0; 187 | std::vector buffer; 188 | int buffer_size; 189 | unsigned int buffer_size_size = sizeof(buffer_size); 190 | getsockopt(m_socket,SOL_SOCKET,SO_RCVBUF,&buffer_size,&buffer_size_size); 191 | buffer.resize(buffer_size); 192 | receive_length = recvfrom(m_socket, &buffer.front(), buffer_size, 0, (sockaddr*)&remote_address, &remote_address_length); 193 | if(receive_length > 0) 194 | { 195 | SourceInfo source_info; 196 | source_info.host = addressToDotted(remote_address); 197 | source_info.port = ntohs(remote_address.sin_port); 198 | buffer.resize(receive_length); 199 | decode(buffer, source_info); 200 | } 201 | } 202 | else 203 | break; 204 | } 205 | for(auto remote: remote_nodes_) 206 | { 207 | if(remote.second) 208 | { 209 | int discard_count = remote.second->defragmenter().cleanup(ros::Duration(5)); 210 | if(discard_count) 211 | ROS_INFO_STREAM("Discarded " << discard_count << " incomplete packets from " << remote.first); 212 | } 213 | } 214 | cleanupSentPackets(); 215 | resendMissingPackets(); 216 | ros::spinOnce(); 217 | } 218 | } 219 | 220 | void UDPBridge::callback(const ros::MessageEvent& event) 221 | { 222 | auto header = event.getConnectionHeader(); 223 | ROS_DEBUG_STREAM_NAMED("send_messages", "Message from: " << header["callerid"]); 224 | ROS_DEBUG_STREAM_NAMED("send_messages", " topic: " << header["topic"]); 225 | ROS_DEBUG_STREAM_NAMED("send_messages", " type: " << header["type"]); 226 | // skip messages published by us to prevent loops 227 | if(event.getPublisherName() == ros::this_node::getName()) 228 | return; 229 | auto now = event.getReceiptTime(); 230 | ROS_DEBUG_STREAM_NAMED("send_messages",now); 231 | auto topic = header.find("topic"); 232 | if(topic == header.end()) 233 | return; 234 | 235 | auto topic_name = topic->second; 236 | auto msg = event.getConstMessage(); 237 | 238 | RemoteConnectionsList destinations; 239 | // figure out which remote connection is due for a message to be sent. 240 | for(auto& remote_details: m_subscribers[topic_name].remote_details) 241 | { 242 | std::unordered_set periods; // group the sending to connections with same period 243 | for(auto& connection_rate: remote_details.second.connection_rates) 244 | if(connection_rate.second.period >= 0) 245 | if(connection_rate.second.period == 0 || now-connection_rate.second.last_sent_time > ros::Duration(connection_rate.second.period) || periods.count(connection_rate.second.period) > 0) 246 | { 247 | destinations[remote_details.first].push_back(connection_rate.first); 248 | ROS_DEBUG_STREAM_NAMED("send_messages", "adding destination " << remote_details.first << ":" << connection_rate.first << " last sent time: " << connection_rate.second.last_sent_time); 249 | connection_rate.second.last_sent_time = now; 250 | if(periods.count(connection_rate.second.period) == 0) 251 | periods.insert(connection_rate.second.period); 252 | } 253 | } 254 | 255 | MessageSizeData size_data; 256 | size_data.message_size = msg->size(); 257 | ROS_DEBUG_STREAM_NAMED("send_messages", "message size: " << size_data.message_size); 258 | size_data.timestamp = now; 259 | size_data.send_results[""][""]; 260 | m_subscribers[topic_name].statistics.add(size_data); 261 | if (destinations.empty()) 262 | { 263 | ROS_DEBUG_STREAM_NAMED("send_messages","No ready destination"); 264 | return; 265 | } 266 | 267 | // First, serialize message in a MessageInternal message 268 | MessageInternal message; 269 | 270 | message.source_topic = topic_name; 271 | message.datatype = msg->getDataType(); 272 | message.md5sum = msg->getMD5Sum(); 273 | message.message_definition = msg->getMessageDefinition(); 274 | 275 | 276 | message.data.resize(msg->size()); 277 | 278 | ros::serialization::OStream stream(message.data.data(), msg->size()); 279 | msg->write(stream); 280 | 281 | for(auto destination: destinations) 282 | { 283 | message.destination_topic = m_subscribers[topic_name].remote_details[destination.first].destination_topic; 284 | RemoteConnectionsList connections; 285 | connections[destination.first] = destination.second; 286 | size_data = send(message, connections, false); 287 | size_data.message_size = msg->size(); 288 | m_subscribers[topic_name].statistics.add(size_data); 289 | } 290 | } 291 | 292 | void UDPBridge::decode(std::vector const &message, const SourceInfo& source_info) 293 | { 294 | if(message.empty()) 295 | { 296 | ROS_DEBUG_STREAM("empty message from: " << source_info.node_name << " " << source_info.host << ":" << source_info.port); 297 | return; 298 | } 299 | 300 | const Packet *packet = reinterpret_cast(message.data()); 301 | ROS_DEBUG_STREAM_NAMED("receive", "Received packet of type " << int(packet->type) << " and size " << message.size() << " from '" << source_info.node_name << "' (" << source_info.host << ":" << source_info.port << ")"); 302 | std::shared_ptr remote_node; 303 | auto remote_node_iterator = remote_nodes_.find(source_info.node_name); 304 | if(remote_node_iterator != remote_nodes_.end()) 305 | remote_node = remote_node_iterator->second; 306 | switch(packet->type) 307 | { 308 | case PacketType::Data: 309 | decodeData(message, source_info); 310 | break; 311 | case PacketType::Compressed: 312 | decode(uncompress(message), source_info); 313 | break; 314 | case PacketType::SubscribeRequest: 315 | decodeSubscribeRequest(message, source_info); 316 | break; 317 | case PacketType::Fragment: 318 | if(remote_node) 319 | if(remote_node->defragmenter().addFragment(message)) 320 | for(auto p: remote_node->defragmenter().getPackets()) 321 | decode(p, source_info); 322 | break; 323 | case PacketType::BridgeInfo: 324 | decodeBridgeInfo(message, source_info); 325 | break; 326 | case PacketType::TopicStatistics: 327 | decodeTopicStatistics(message, source_info); 328 | break; 329 | case PacketType::WrappedPacket: 330 | unwrap(message, source_info); 331 | break; 332 | case PacketType::ResendRequest: 333 | decodeResendRequest(message, source_info); 334 | break; 335 | case PacketType::Connection: 336 | decodeConnection(message, source_info); 337 | break; 338 | default: 339 | ROS_WARN_STREAM("Unknown packet type: " << int(packet->type) << " from: " << source_info.node_name << " " << source_info.host << ":" << source_info.port); 340 | } 341 | } 342 | 343 | void UDPBridge::decodeData(std::vector const &message, const SourceInfo& source_info) 344 | { 345 | const Packet *packet = reinterpret_cast(message.data()); 346 | 347 | auto payload_size = message.size() - sizeof(PacketHeader); 348 | 349 | MessageInternal outer_message; 350 | 351 | ros::serialization::IStream stream(const_cast(packet->data),payload_size); 352 | ros::serialization::Serializer::read(stream, outer_message); 353 | 354 | topic_tools::ShapeShifter ss; 355 | ss.morph(outer_message.md5sum, outer_message.datatype, outer_message.message_definition, ""); 356 | ros::serialization::IStream message_stream(outer_message.data.data(), outer_message.data.size()); 357 | ss.read(message_stream); 358 | ROS_DEBUG_STREAM_NAMED("receive", "decoded message of type: " << ss.getDataType() << " and size: " << ss.size()); 359 | 360 | 361 | // publish, but first advertise if publisher not present 362 | auto topic = outer_message.destination_topic; 363 | if(topic.empty()) 364 | topic = outer_message.source_topic; 365 | if (m_publishers.find(topic) == m_publishers.end()) 366 | { 367 | m_publishers[topic] = ss.advertise(m_nodeHandle, topic, 1); 368 | sendBridgeInfo(); 369 | } 370 | 371 | m_publishers[topic].publish(ss); 372 | } 373 | 374 | void UDPBridge::decodeBridgeInfo(std::vector const &message, const SourceInfo& source_info) 375 | { 376 | const Packet* packet = reinterpret_cast(message.data()); 377 | 378 | ros::serialization::IStream stream(const_cast(packet->data),message.size()-sizeof(PacketHeader)); 379 | 380 | BridgeInfo bridge_info; 381 | try 382 | { 383 | ros::serialization::Serializer::read(stream, bridge_info); 384 | } 385 | catch(const std::exception& e) 386 | { 387 | ROS_WARN_STREAM("Error decoding BridgeInfo: " << e.what()); 388 | return; 389 | } 390 | 391 | auto remote_node = remote_nodes_[source_info.node_name]; 392 | if(!remote_node) 393 | { 394 | remote_node = std::make_shared(source_info.node_name, name_); 395 | remote_nodes_[source_info.node_name] = remote_node; 396 | } 397 | remote_node->update(bridge_info, source_info); 398 | 399 | } 400 | 401 | void UDPBridge::decodeResendRequest(std::vector const &message, const SourceInfo& source_info) 402 | { 403 | const Packet* packet = reinterpret_cast(message.data()); 404 | 405 | ros::serialization::IStream stream(const_cast(packet->data),message.size()-sizeof(PacketHeader)); 406 | 407 | ResendRequest rr; 408 | ros::serialization::Serializer::read(stream, rr); 409 | 410 | ROS_DEBUG_STREAM_NAMED("resend", "Request from " << source_info.node_name << " to resend " << rr.missing_packets.size() << " packets"); 411 | 412 | auto remote_node = remote_nodes_.find(source_info.node_name); 413 | if(remote_node != remote_nodes_.end()) 414 | for(auto connection: remote_node->second->connections()) 415 | connection->resend_packets(rr.missing_packets, m_socket); 416 | } 417 | 418 | void UDPBridge::decodeTopicStatistics(std::vector const &message, const SourceInfo& source_info) 419 | { 420 | const Packet* packet = reinterpret_cast(message.data()); 421 | 422 | ros::serialization::IStream stream(const_cast(packet->data),message.size()-sizeof(PacketHeader)); 423 | 424 | TopicStatisticsArray topic_statistics; 425 | ros::serialization::Serializer::read(stream, topic_statistics); 426 | 427 | auto remote = remote_nodes_.find(source_info.node_name); 428 | if(remote != remote_nodes_.end()) 429 | remote->second->publishTopicStatistics(topic_statistics); 430 | } 431 | 432 | 433 | void UDPBridge::addSubscriberConnection(std::string const &source_topic, std::string const &destination_topic, uint32_t queue_size, float period, std::string remote_node, std::string connection_id) 434 | { 435 | if(!remote_node.empty()) 436 | { 437 | if(m_subscribers.find(source_topic) == m_subscribers.end()) 438 | { 439 | queue_size = std::max(queue_size, uint32_t(1)); 440 | 441 | m_subscribers[source_topic].subscriber = m_nodeHandle.subscribe(source_topic, queue_size, &UDPBridge::callback, this); 442 | } 443 | m_subscribers[source_topic].remote_details[remote_node].destination_topic = destination_topic; 444 | m_subscribers[source_topic].remote_details[remote_node].connection_rates[connection_id].period = period; 445 | sendBridgeInfo(); 446 | } 447 | } 448 | 449 | void UDPBridge::decodeSubscribeRequest(std::vector const &message, const SourceInfo& source_info) 450 | { 451 | const Packet* packet = reinterpret_cast(message.data()); 452 | 453 | ros::serialization::IStream stream(const_cast(packet->data),message.size()-sizeof(PacketHeader)); 454 | 455 | RemoteSubscribeInternal remote_request; 456 | ros::serialization::Serializer::read(stream, remote_request); 457 | 458 | addSubscriberConnection(remote_request.source_topic, remote_request.destination_topic, remote_request.queue_size, remote_request.period, source_info.node_name, remote_request.connection_id); 459 | } 460 | 461 | void UDPBridge::unwrap(const std::vector& message, const SourceInfo& source_info) 462 | { 463 | if(message.size() > sizeof(SequencedPacketHeader)) 464 | { 465 | const SequencedPacket* wrapped_packet = reinterpret_cast(message.data()); 466 | 467 | 468 | 469 | auto remote_iterator = remote_nodes_.find(wrapped_packet->source_node); 470 | std::shared_ptr remote; 471 | if(remote_iterator != remote_nodes_.end()) 472 | remote = remote_iterator->second; 473 | 474 | auto updated_source_info = source_info; 475 | updated_source_info.node_name = wrapped_packet->source_node; 476 | 477 | if(!remote) 478 | { 479 | if(wrapped_packet->source_node == name_) 480 | { 481 | ROS_ERROR_STREAM("Received a packet from a node with our name: " << name_ << " connection: " << wrapped_packet->connection_id << " host: " << source_info.host << " port: " << source_info.port); 482 | return; 483 | } 484 | else 485 | { 486 | remote = std::make_shared(wrapped_packet->source_node, name_); 487 | remote_nodes_[wrapped_packet->source_node] = remote; 488 | } 489 | } 490 | try 491 | { 492 | decode(remote->unwrap(message, updated_source_info), updated_source_info); 493 | } 494 | catch(const std::exception& e) 495 | { 496 | ROS_ERROR_STREAM("problem decoding packet: " << e.what()); 497 | } 498 | } 499 | else 500 | ROS_ERROR_STREAM("Incomplete wrapped packet of size: " << message.size()); 501 | } 502 | 503 | void UDPBridge::decodeConnection(std::vector const &message, const SourceInfo& source_info) 504 | { 505 | const Packet* packet = reinterpret_cast(message.data()); 506 | 507 | ros::serialization::IStream stream(const_cast(packet->data),message.size()-sizeof(PacketHeader)); 508 | 509 | ConnectionInternal connection_internal; 510 | ros::serialization::Serializer::read(stream, connection_internal); 511 | 512 | switch(connection_internal.operation) 513 | { 514 | case ConnectionInternal::OPERATION_CONNECT: 515 | { 516 | auto remote = remote_nodes_[source_info.node_name]; 517 | if(!remote) 518 | { 519 | remote = std::make_shared(source_info.node_name, name_); 520 | remote_nodes_[source_info.node_name] = remote; 521 | } 522 | uint16_t port = source_info.port; 523 | if(connection_internal.return_port != 0) 524 | port = connection_internal.return_port; 525 | auto host = source_info.host; 526 | if(!connection_internal.return_host.empty()) 527 | host = connection_internal.return_host; 528 | auto connection = remote->connection(connection_internal.connection_id); 529 | if(!connection) 530 | { 531 | connection = remote->newConnection(connection_internal.connection_id, host, port); 532 | remote->connections().push_back(connection); 533 | } 534 | else 535 | connection->setHostAndPort(host, port); 536 | if(connection_internal.return_maximum_bytes_per_second > 0) 537 | connection->setRateLimit(connection_internal.return_maximum_bytes_per_second); 538 | connection_internal.operation = ConnectionInternal::OPERATION_CONNECT_ACKNOWLEDGE; 539 | connection_internal.return_host = connection->returnHost(); 540 | connection_internal.return_port = connection->returnPort(); 541 | send(connection_internal, source_info.node_name, true); 542 | } 543 | break; 544 | case ConnectionInternal::OPERATION_CONNECT_ACKNOWLEDGE: 545 | if(pending_connections_.find(connection_internal.sequence_number) != pending_connections_.end()) 546 | { 547 | auto remote = remote_nodes_[source_info.node_name]; 548 | if(!remote) 549 | { 550 | remote = std::make_shared(source_info.node_name, name_); 551 | remote_nodes_[source_info.node_name] = remote; 552 | } 553 | auto connection = remote->connection(connection_internal.connection_id); 554 | auto pending_connection = pending_connections_[connection_internal.sequence_number].connection; 555 | if(pending_connection) 556 | { 557 | if(!connection) 558 | { 559 | connection = pending_connection; 560 | remote->connections().push_back(connection); 561 | } 562 | else 563 | { 564 | connection->setRateLimit(pending_connection->rateLimit()); 565 | connection->setReturnHostAndPort(pending_connection->returnHost(), pending_connection->returnPort()); 566 | } 567 | } 568 | pending_connections_.erase(connection_internal.sequence_number); 569 | } 570 | break; 571 | default: 572 | ROS_WARN_STREAM("Unhandled ConnectionInternal operation type: " << connection_internal.operation); 573 | } 574 | 575 | } 576 | 577 | 578 | void UDPBridge::resendMissingPackets() 579 | { 580 | for(auto remote: remote_nodes_) 581 | if(remote.second) 582 | { 583 | auto rr = remote.second->getMissingPackets(); 584 | if(!rr.missing_packets.empty()) 585 | { 586 | ROS_DEBUG_STREAM_NAMED("resend", "Sending a request to resend " << rr.missing_packets.size() << " packets"); 587 | send(rr, remote.first, true); 588 | } 589 | } 590 | } 591 | 592 | 593 | template MessageSizeData UDPBridge::send(MessageType const &message, const RemoteConnectionsList& remotes, bool is_overhead) 594 | { 595 | auto serial_size = ros::serialization::serializationLength(message); 596 | 597 | ROS_DEBUG_STREAM_NAMED("send_messages", "serial size: " << serial_size); 598 | 599 | std::vector packet_data(sizeof(PacketHeader)+serial_size); 600 | Packet * packet = reinterpret_cast(packet_data.data()); 601 | ros::serialization::OStream stream(packet->data, serial_size); 602 | ros::serialization::serialize(stream,message); 603 | packet->type = packetTypeOf(message); 604 | 605 | auto packet_size = packet_data.size(); 606 | 607 | ROS_DEBUG_STREAM_NAMED("send_messages", "packet size: " << packet_size); 608 | 609 | packet_data = compress(packet_data); 610 | 611 | ROS_DEBUG_STREAM_NAMED("send_messages", "compressed packet size: " << packet_data.size()); 612 | 613 | auto fragments = fragment(packet_data); 614 | ROS_DEBUG_STREAM_NAMED("send_messages", "fragment count: " << fragments.size()); 615 | 616 | auto size_data = send(fragments, remotes, is_overhead); 617 | 618 | for(auto remote_result: size_data.send_results) 619 | { 620 | for(auto connection_result: remote_result.second) 621 | { 622 | switch(connection_result.second) 623 | { 624 | case SendResult::success: 625 | ROS_DEBUG_STREAM_NAMED("send_messages", remote_result.first << ":" << connection_result.first << " success"); 626 | break; 627 | case SendResult::failed: 628 | ROS_DEBUG_STREAM_NAMED("send_messages", remote_result.first << ":" << connection_result.first << " failed"); 629 | break; 630 | case SendResult::dropped: 631 | ROS_DEBUG_STREAM_NAMED("send_messages", remote_result.first << ":" << connection_result.first << " dropped"); 632 | break; 633 | } 634 | } 635 | } 636 | 637 | size_data.message_size = serial_size; 638 | size_data.fragment_count = fragments.size(); 639 | return size_data; 640 | } 641 | 642 | template MessageSizeData UDPBridge::send(MessageType const &message, const std::string& remote, bool is_overhead) 643 | { 644 | RemoteConnectionsList rcl; 645 | rcl[remote]; 646 | return send(message, rcl, is_overhead); 647 | } 648 | 649 | 650 | // wrap and send a series of packets that should be sent as a group, such as fragments. 651 | template<> MessageSizeData UDPBridge::send(const std::vector >& data, const RemoteConnectionsList& remotes, bool is_overhead) 652 | { 653 | MessageSizeData size_data; 654 | std::vector wrapped_packets; 655 | 656 | ROS_DEBUG_STREAM_NAMED("send_messages", "begin packet number: " << next_packet_number_); 657 | 658 | for(auto packet: data) 659 | { 660 | uint64_t packet_number = next_packet_number_++; 661 | last_packet_number_assign_time_ = ros::Time::now(); 662 | wrapped_packets.push_back(WrappedPacket(packet_number, packet)); 663 | size_data.sent_size += wrapped_packets.back().packet.size(); 664 | } 665 | 666 | ROS_DEBUG_STREAM_NAMED("send_messages", "end packet number: " << next_packet_number_); 667 | 668 | if(!wrapped_packets.empty()) 669 | { 670 | size_data.timestamp = wrapped_packets.front().timestamp; 671 | for(auto remote: remotes) 672 | { 673 | std::vector > connections; 674 | if(remote.first.empty()) // connection request? 675 | { 676 | for(auto sequence_number_str: remote.second) 677 | for(auto pending_connection: pending_connections_) 678 | if(std::to_string(pending_connection.first) == sequence_number_str) 679 | connections.push_back(pending_connection.second.connection); 680 | } 681 | else 682 | { 683 | auto remote_node = remote_nodes_.find(remote.first); 684 | if(remote_node != remote_nodes_.end()) 685 | { 686 | if(remote.second.empty()) 687 | connections = remote_node->second->connections(); 688 | else 689 | for(auto connection_id: remote.second) 690 | connections.push_back(remote_node->second->connection(connection_id)); 691 | } 692 | } 693 | for(auto connection: connections) 694 | if(connection) 695 | { 696 | auto result = connection->send(wrapped_packets, m_socket, name_, is_overhead); 697 | size_data.send_results[remote.first][connection->id()]=result; 698 | } 699 | 700 | } 701 | } 702 | ROS_DEBUG_STREAM_NAMED("send_messages", "sent size: " << size_data.sent_size); 703 | 704 | return size_data; 705 | } 706 | 707 | void UDPBridge::cleanupSentPackets() 708 | { 709 | auto now = ros::Time::now(); 710 | if(now.isValid && !now.isZero()) 711 | { 712 | auto old_enough = now - ros::Duration(3.0); 713 | for(auto remote: remote_nodes_) 714 | if(remote.second) 715 | for(auto connection: remote.second->connections()) 716 | if(connection) 717 | connection->cleanup_sent_packets(old_enough); 718 | } 719 | } 720 | 721 | 722 | bool UDPBridge::remoteSubscribe(udp_bridge::Subscribe::Request &request, udp_bridge::Subscribe::Response &response) 723 | { 724 | ROS_INFO_STREAM("subscribe: remote: " << request.remote << ":" << " connection: " << request.connection_id << " source topic: " << request.source_topic << " destination topic: " << request.destination_topic); 725 | 726 | udp_bridge::RemoteSubscribeInternal remote_request; 727 | remote_request.source_topic = request.source_topic; 728 | remote_request.destination_topic = request.destination_topic; 729 | remote_request.queue_size = request.queue_size; 730 | remote_request.period = request.period; 731 | remote_request.connection_id = request.connection_id; 732 | 733 | auto ret = send(remote_request, request.remote, true); 734 | return ret.send_results[request.remote][request.connection_id] == SendResult::success; 735 | } 736 | 737 | bool UDPBridge::remoteAdvertise(udp_bridge::Subscribe::Request &request, udp_bridge::Subscribe::Response &response) 738 | { 739 | auto remote = remote_nodes_.find(request.remote); 740 | if(remote == remote_nodes_.end()) 741 | return false; 742 | auto connection = remote->second->connection(request.connection_id); 743 | if(!connection) 744 | return false; 745 | 746 | addSubscriberConnection(request.source_topic, request.destination_topic, request.queue_size, request.period, request.remote, request.connection_id); 747 | sendBridgeInfo(); 748 | return true; 749 | } 750 | 751 | void UDPBridge::statsReportCallback(ros::TimerEvent const &event) 752 | { 753 | ros::Time now = ros::Time::now(); 754 | TopicStatisticsArray tsa; 755 | for(auto &subscriber: m_subscribers) 756 | { 757 | for(auto ts: subscriber.second.statistics.get()) 758 | { 759 | ts.source_node = name_; 760 | ts.source_topic = subscriber.first; 761 | auto details = subscriber.second.remote_details.find(ts.destination_node); 762 | if(details != subscriber.second.remote_details.end()) 763 | ts.destination_topic = details->second.destination_topic; 764 | tsa.topics.push_back(ts); 765 | } 766 | } 767 | 768 | m_topicStatisticsPublisher.publish(tsa); 769 | 770 | send(tsa, allRemotes(), true); 771 | } 772 | 773 | std::vector > UDPBridge::fragment(const std::vector& data) 774 | { 775 | std::vector > ret; 776 | 777 | unsigned long max_fragment_payload_size = m_max_packet_size-(sizeof(FragmentHeader)+sizeof(SequencedPacketHeader)); 778 | if(data.size()+sizeof(SequencedPacketHeader) > m_max_packet_size) 779 | { 780 | for(unsigned long i = 0; i < data.size(); i += max_fragment_payload_size) 781 | { 782 | unsigned long fragment_data_size = std::min(max_fragment_payload_size, data.size()-i); 783 | ret.push_back(std::vector(sizeof(FragmentHeader)+fragment_data_size)); 784 | Fragment * fragment_packet = reinterpret_cast(ret.back().data()); 785 | fragment_packet->type = PacketType::Fragment; 786 | fragment_packet->packet_id = next_fragmented_packet_id_; 787 | fragment_packet->fragment_number = ret.size(); 788 | memcpy(fragment_packet->fragment_data, &data.at(i), fragment_data_size); 789 | } 790 | for(auto & fragment_vector: ret) 791 | reinterpret_cast(fragment_vector.data())->fragment_count = ret.size(); 792 | next_fragmented_packet_id_++; 793 | if(ret.size() > std::numeric_limits::max()) 794 | { 795 | ROS_WARN_STREAM("Dropping " << ret.size() << " fragments, max frag count:" << std::numeric_limits::max()); 796 | return {}; 797 | } 798 | } 799 | if(ret.empty()) 800 | ret.push_back(data); 801 | return ret; 802 | } 803 | 804 | void UDPBridge::bridgeInfoCallback(ros::TimerEvent const &event) 805 | { 806 | sendBridgeInfo(); 807 | sendConnectionRequests(); 808 | } 809 | 810 | void UDPBridge::sendBridgeInfo() 811 | { 812 | BridgeInfo bi; 813 | bi.stamp = ros::Time::now(); 814 | bi.name = name_; 815 | ros::master::V_TopicInfo topics; 816 | ros::master::getTopics(topics); 817 | for(auto mti: topics) 818 | { 819 | TopicInfo ti; 820 | ti.topic = mti.name; 821 | ti.datatype = mti.datatype; 822 | if (m_subscribers.find(mti.name) != m_subscribers.end()) 823 | { 824 | for(auto r: m_subscribers[mti.name].remote_details) 825 | { 826 | TopicRemoteDetails trd; 827 | trd.remote = r.first; 828 | trd.destination_topic = r.second.destination_topic; 829 | for(auto c:r.second.connection_rates) 830 | { 831 | TopicRemoteConnectionDetails trcd; 832 | trcd.connection_id = c.first; 833 | trcd.period = c.second.period; 834 | trd.connections.push_back(trcd); 835 | } 836 | ti.remotes.push_back(trd); 837 | } 838 | } 839 | bi.topics.push_back(ti); 840 | } 841 | 842 | for(auto remote_node: remote_nodes_) 843 | if(remote_node.second) 844 | { 845 | udp_bridge::Remote remote; 846 | remote.name = remote_node.first; 847 | remote.topic_name = remote_node.second->topicName(); 848 | for(auto connection: remote_node.second->connections()) 849 | if(connection) 850 | { 851 | RemoteConnection rc; 852 | rc.connection_id = connection->id(); 853 | rc.host = connection->host(); 854 | rc.port = connection->port(); 855 | rc.ip_address = connection->ip_address(); 856 | rc.return_host = connection->returnHost(); 857 | rc.return_port = connection->returnPort(); 858 | rc.source_ip_address = connection->sourceIPAddress(); 859 | rc.source_port = connection->sourcePort(); 860 | rc.maximum_bytes_per_second = connection->rateLimit(); 861 | auto receive_rates = connection->data_receive_rate(bi.stamp.toSec()); 862 | rc.received_bytes_per_second = receive_rates.first + receive_rates.second; 863 | rc.duplicate_bytes_per_second = receive_rates.second; 864 | rc.message = connection->data_sent_rate(bi.stamp, PacketSendCategory::message); 865 | rc.overhead = connection->data_sent_rate(bi.stamp, PacketSendCategory::overhead); 866 | rc.resend = connection->data_sent_rate(bi.stamp, PacketSendCategory::resend); 867 | remote.connections.push_back(rc); 868 | } 869 | bi.remotes.push_back(remote); 870 | } 871 | 872 | bi.next_packet_number = next_packet_number_; 873 | bi.last_packet_time = last_packet_number_assign_time_; 874 | bi.local_port = m_port; 875 | bi.maximum_packet_size = m_max_packet_size; 876 | 877 | ROS_DEBUG_STREAM_NAMED("bridge_info","Publishing and sending BridgeInfo"); 878 | 879 | m_bridge_info_publisher.publish(bi); 880 | 881 | send(bi, allRemotes(), true); 882 | } 883 | 884 | void UDPBridge::sendConnectionRequests() 885 | { 886 | for(auto pending_connection: pending_connections_) 887 | { 888 | RemoteConnectionsList rcl; 889 | rcl[""].push_back(std::to_string(pending_connection.first)); 890 | send(pending_connection.second.message, rcl, true); 891 | } 892 | } 893 | 894 | bool UDPBridge::addRemote(udp_bridge::AddRemote::Request &request, udp_bridge::AddRemote::Response &response) 895 | { 896 | auto connection_id = request.connection_id; 897 | if(connection_id.empty()) 898 | connection_id = "default"; 899 | 900 | if(!request.name.empty()) 901 | { 902 | auto remote = remote_nodes_[request.name]; 903 | if(!remote) 904 | { 905 | remote = std::make_shared(request.name, name_); 906 | remote_nodes_[request.name] = remote; 907 | } 908 | 909 | uint16_t port = 4200; 910 | if (request.port != 0) 911 | port = request.port; 912 | 913 | auto connection = remote->connection(connection_id); 914 | if(!request.address.empty()) 915 | { 916 | if(!connection) 917 | { 918 | connection = remote->newConnection(connection_id, request.address, port); 919 | remote->connections().push_back(connection); 920 | } 921 | else 922 | connection->setHostAndPort(request.address, port); 923 | } 924 | if(connection) 925 | { 926 | connection->setReturnHostAndPort(request.return_address, request.return_port); 927 | connection->setRateLimit(request.maximum_bytes_per_second); 928 | sendBridgeInfo(); 929 | } 930 | } 931 | 932 | if(request.name.empty() || request.return_maximum_bytes_per_second > 0) // we don't know the remote name, so send a connection request 933 | { 934 | auto sequence_number = next_connection_internal_message_sequence_number_++; 935 | auto& connection_internal = pending_connections_[sequence_number]; 936 | connection_internal.message.connection_id = connection_id; 937 | connection_internal.message.sequence_number = sequence_number; 938 | connection_internal.message.operation = ConnectionInternal::OPERATION_CONNECT; 939 | connection_internal.message.return_host = request.return_address; 940 | connection_internal.message.return_port = request.return_port; 941 | connection_internal.message.return_maximum_bytes_per_second = request.return_maximum_bytes_per_second; 942 | if(!request.address.empty()) 943 | { 944 | connection_internal.connection = std::make_shared(connection_id, request.address, request.port); 945 | connection_internal.connection->setRateLimit(request.maximum_bytes_per_second); 946 | } 947 | RemoteConnectionsList rcl; 948 | if(request.name.empty()) 949 | rcl[""].push_back(std::to_string(sequence_number)); 950 | else 951 | rcl[request.name].push_back(connection_id); 952 | send(connection_internal.message, rcl, true); 953 | } 954 | 955 | return true; 956 | } 957 | 958 | bool UDPBridge::listRemotes(udp_bridge::ListRemotes::Request &request, udp_bridge::ListRemotes::Response &response) 959 | { 960 | auto now = ros::Time::now(); 961 | for(auto remote: remote_nodes_) 962 | if(remote.second) 963 | { 964 | Remote r; 965 | r.name = remote.first; 966 | r.topic_name = remote.second->topicName(); 967 | for(auto connection: remote.second->connections()) 968 | if(connection) 969 | { 970 | RemoteConnection rc; 971 | rc.connection_id = connection->id(); 972 | rc.host = connection->host(); 973 | rc.port = connection->port(); 974 | rc.ip_address = connection->ip_address(); 975 | rc.return_host = connection->returnHost(); 976 | rc.return_port = connection->returnPort(); 977 | rc.source_ip_address = connection->sourceIPAddress(); 978 | rc.source_port = connection->sourcePort(); 979 | rc.maximum_bytes_per_second = connection->rateLimit(); 980 | auto receive_rates = connection->data_receive_rate(now.toSec()); 981 | rc.received_bytes_per_second = receive_rates.first + receive_rates.second; 982 | rc.duplicate_bytes_per_second = receive_rates.second; 983 | rc.message = connection->data_sent_rate(now, PacketSendCategory::message); 984 | rc.overhead = connection->data_sent_rate(now, PacketSendCategory::overhead); 985 | rc.resend = connection->data_sent_rate(now, PacketSendCategory::resend); 986 | r.connections.push_back(rc); 987 | } 988 | response.remotes.push_back(r); 989 | } 990 | return true; 991 | } 992 | 993 | UDPBridge::RemoteConnectionsList UDPBridge::allRemotes() const 994 | { 995 | RemoteConnectionsList ret; 996 | for(auto remote: remote_nodes_) 997 | if(remote.second) 998 | ret[remote.first]; 999 | return ret; 1000 | } 1001 | 1002 | void UDPBridge::maximumPacketSizeCallback(const std_msgs::Int32::ConstPtr& msg) 1003 | { 1004 | m_max_packet_size = msg->data; 1005 | } 1006 | 1007 | 1008 | } // namespace udp_bridge 1009 | -------------------------------------------------------------------------------- /src/udp_bridge_node.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/udp_bridge.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "udp_bridge_node"); 6 | 7 | udp_bridge::UDPBridge bridge; 8 | 9 | bridge.spin(); 10 | return 0; 11 | } 12 | 13 | -------------------------------------------------------------------------------- /src/udpbridge_ui.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "udp_bridge/TopicStatisticsArray.h" 4 | 5 | void statisticsCallback(udp_bridge::TopicStatisticsArray const &stats) 6 | { 7 | // if (!stats.remote_label.empty()) 8 | // return; // don't show stats from remotes 9 | 10 | std::vector headers {"source topic", "remote host", " messages", "message data", " send ok", " send fail", " dropped"}; 11 | std::vector column_widths; 12 | for(auto h: headers) 13 | column_widths.push_back(h.size()); 14 | 15 | for(auto c: stats.topics) 16 | { 17 | column_widths[0] = std::max(column_widths[0], int(c.source_topic.size())); 18 | column_widths[1] = std::max(column_widths[1], int(c.destination_node.size())); 19 | } 20 | 21 | std::cout << std::left; 22 | for(int i = 0; i < headers.size(); i++) 23 | std::cout << std::setw(column_widths[i]+1) << headers[i]; 24 | std::cout << std::endl; 25 | 26 | std::vector totals {0.0, 0.0, 0.0, 0.0, 0.0}; 27 | 28 | for(auto c: stats.topics) 29 | { 30 | std::cout << std::left; 31 | std::cout << std::setw(column_widths[0]+1) << c.source_topic; 32 | std::cout << std::setw(column_widths[1]+1) << c.destination_node; 33 | 34 | std::cout << std::fixed; 35 | std::cout << std::setprecision(1); 36 | std::cout << std::right; 37 | 38 | std::cout << std::setw(5) << c.messages_per_second << " msg/sec "; 39 | // bytes to kilobits, *8/100 -> /125 40 | std::cout << std::setw(7) << c.message_bytes_per_second/125.0 << " kbps "; 41 | 42 | std::cout << std::setw(7) << c.send.success_bytes_per_second/125.0 << " kbps "; 43 | std::cout << std::setw(7) << c.send.failed_bytes_per_second/125.0 << " kbps "; 44 | std::cout << std::setw(7) << c.send.dropped_bytes_per_second/125.0 << " kbps"; 45 | 46 | std::cout << std::endl; 47 | 48 | totals[0] += c.messages_per_second; 49 | totals[1] += c.message_bytes_per_second; 50 | totals[2] += c.send.success_bytes_per_second; 51 | totals[3] += c.send.failed_bytes_per_second; 52 | totals[4] += c.send.dropped_bytes_per_second; 53 | } 54 | std::cout << std::left; 55 | std::cout << std::setw(column_widths[0]+column_widths[1]+2) << "totals:"; 56 | std::cout << std::right; 57 | std::cout << std::setw(5) << totals[0] << " msg/sec "; 58 | std::cout << std::setw(7) << totals[1]/125.0 << " kbps "; 59 | std::cout << std::setw(7) << totals[2]/125.0 << " kbps "; 60 | std::cout << std::setw(7) << totals[3]/125.0 << " kbps"; 61 | std::cout << std::setw(7) << totals[4]/125.0 << " kbps"; 62 | std::cout << std::endl; 63 | 64 | std::cout << std::endl; 65 | 66 | } 67 | 68 | int main(int argc, char **argv) 69 | { 70 | ros::init(argc, argv, "udp_bridge_ui"); 71 | 72 | ros::NodeHandle nh; 73 | 74 | std::vector statsSubs; 75 | 76 | ros::master::V_TopicInfo topic_infos; 77 | ros::master::getTopics(topic_infos); 78 | 79 | ros::V_string nodes; 80 | ros::master::getNodes(nodes); 81 | 82 | for(auto ti:topic_infos) 83 | for(auto node: nodes) 84 | if(ti.name == node+"/topic_statistics" && ti.datatype == "udp_bridge/TopicStatisticsArray") 85 | statsSubs.push_back(nh.subscribe(ti.name, 10, &statisticsCallback)); 86 | 87 | ros::spin(); 88 | 89 | return 0; 90 | } 91 | 92 | -------------------------------------------------------------------------------- /src/utilities.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/utilities.h" 2 | 3 | namespace udp_bridge 4 | { 5 | 6 | std::string topicString(const std::string& topic, bool keep_slashes, char legal_first, char legal_char) 7 | { 8 | std::string ret = topic; 9 | for(int i = 0; i < ret.size(); i++) 10 | if(!(std::isalnum(ret[i]) || ret[i] == '_' || (ret[i] == '/' && keep_slashes))) 11 | ret[i] = legal_char; 12 | /// \todo check for more than just the first char, also check following /'s if they are kept 13 | if(!(std::isalpha(ret[0]) || ret[0] == '/')) 14 | ret = legal_first+ret; 15 | return ret; 16 | } 17 | 18 | } 19 | -------------------------------------------------------------------------------- /src/wrapped_packet.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/wrapped_packet.h" 2 | 3 | namespace udp_bridge 4 | { 5 | 6 | WrappedPacket::WrappedPacket(uint64_t packet_num, const std::vector& data) 7 | { 8 | memset(this, 0, sizeof(SequencedPacketHeader)); 9 | packet_number = packet_num; 10 | packet_size = sizeof(SequencedPacketHeader)+data.size(); 11 | timestamp = ros::Time::now(); 12 | type = PacketType::WrappedPacket; 13 | packet.resize(packet_size); 14 | memcpy(packet.data(), this, sizeof(SequencedPacketHeader)); 15 | memcpy(reinterpret_cast(packet.data())->packet, data.data(), data.size()); 16 | } 17 | 18 | WrappedPacket::WrappedPacket(const WrappedPacket& other, std::string src_node, std::string cid) 19 | { 20 | type = PacketType::WrappedPacket; 21 | packet_number = other.packet_number; 22 | packet_size = other.packet_size; 23 | timestamp = other.timestamp; 24 | packet = other.packet; 25 | 26 | memset(&source_node, 0, maximum_node_name_size); 27 | memcpy(&source_node, src_node.c_str(), std::min(src_node.size(), std::size_t(maximum_node_name_size-1))); 28 | 29 | memset(&connection_id, 0, maximum_connection_id_size); 30 | memcpy(&connection_id, cid.c_str(), std::min(cid.size(), std::size_t(maximum_connection_id_size-1))); 31 | 32 | memcpy(packet.data(), this, sizeof(SequencedPacketHeader)); 33 | } 34 | 35 | 36 | } // namespace udp_bridge 37 | -------------------------------------------------------------------------------- /srv/AddRemote.srv: -------------------------------------------------------------------------------- 1 | string name 2 | string connection_id 3 | string address 4 | uint16 port 5 | string return_address 6 | uint16 return_port 7 | uint32 maximum_bytes_per_second 8 | uint32 return_maximum_bytes_per_second 9 | --- -------------------------------------------------------------------------------- /srv/ListRemotes.srv: -------------------------------------------------------------------------------- 1 | --- 2 | Remote[] remotes -------------------------------------------------------------------------------- /srv/Subscribe.srv: -------------------------------------------------------------------------------- 1 | string remote 2 | string connection_id 3 | string source_topic 4 | string destination_topic 5 | uint32 queue_size 6 | 7 | # Allows limiting message rate by specifying minimum period in seconds between sent messages 8 | # 0 -> no limits, negative -> no messages sent, positive -> time in seconds between sent messages 9 | float32 period 10 | --- 11 | -------------------------------------------------------------------------------- /test/mininet/multilink.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Simulate a operator-robot network with multiple unreliable links. 5 | """ 6 | 7 | from mininet.cli import CLI 8 | from mininet.net import Mininet 9 | import mininet.link 10 | import mininet.log 11 | import mininet.node 12 | import mininet.util 13 | 14 | # specify link characteristics. bw is bandwidth in Mb/s and loss is in percent. 15 | links = [ 16 | {'bw':5, 'delay':'100ms', 'jitter':'50ms', 'loss':5, 'max_queue_size':250}, 17 | {'bw':1, 'delay':'500ms', 'jitter':'250ms', 'loss':2.5, 'max_queue_size':250}, 18 | {'bw':0.5, 'delay':'1000ms', 'jitter':'750ms', 'loss':1, 'max_queue_size':500} 19 | ] 20 | 21 | mininet.log.setLogLevel('info') 22 | 23 | net = Mininet(build=False, controller=mininet.node.RemoteController, link=mininet.link.TCLink, topo=None, xterms=False) 24 | 25 | robot = net.addHost('robot', ip=None) 26 | operator = net.addHost('operator', ip=None) 27 | 28 | for i in range(len(links)): 29 | si = str(i) 30 | net.addLink(operator, robot, intfName1='operator-eth'+si, intfName2='robot-eth'+si, **links[i]) 31 | 32 | robot.intf('robot-eth'+si).ip = '192.168.'+si+'.1' 33 | robot.intf('robot-eth'+si).prefixLen = 24 34 | robot.cmd('ip a a 192.168.'+si+'.1/24 dev robot-eth'+si) 35 | 36 | operator.intf('operator-eth'+si).ip = '192.168.'+si+'.2' 37 | operator.intf('operator-eth'+si).prefixLen = 24 38 | operator.cmd('ip a a 192.168.'+si+'.2/24 dev operator-eth'+si) 39 | 40 | net.build() 41 | 42 | robot.sendCmd('./robot.bash') 43 | print (robot.waitOutput()) 44 | 45 | operator.sendCmd('./operator.bash') 46 | print (operator.waitOutput()) 47 | 48 | CLI(net) 49 | 50 | net.stop() 51 | -------------------------------------------------------------------------------- /test/mininet/operator.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "Operator startup script" 4 | 5 | sudo -i -u "$SUDO_USER" bash << EOF 6 | cd "$(pwd)" 7 | xterm -T operator & 8 | EOF 9 | 10 | echo "Operator startup script done" 11 | -------------------------------------------------------------------------------- /test/mininet/operator_launch.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source ./operator_setup.bash 4 | xterm -e roscore & 5 | xterm -e rosrun rosmon rosmon --name=rosmon_operator_bridge udp_bridge test_mininet_multilink_operator.launch & 6 | -------------------------------------------------------------------------------- /test/mininet/operator_setup.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | export ROS_IP=192.168.0.2 4 | export ROS_MASTER_URI=http://192.168.0.2:11311 5 | -------------------------------------------------------------------------------- /test/mininet/robot.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "Robot startup script" 4 | 5 | sudo -i -u "$SUDO_USER" bash << EOF 6 | cd "$(pwd)" 7 | xterm -T robot & 8 | EOF 9 | 10 | echo "Robot startup script done" 11 | -------------------------------------------------------------------------------- /test/mininet/robot_launch.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source ./robot_setup.bash 4 | xterm -e roscore & 5 | xterm -e rosrun rosmon rosmon --name=rosmon_robot_bridge udp_bridge test_mininet_multilink_robot.launch & 6 | -------------------------------------------------------------------------------- /test/mininet/robot_setup.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | export ROS_IP=192.168.0.1 4 | export ROS_MASTER_URI=http://192.168.0.1:11311 5 | -------------------------------------------------------------------------------- /test/utest.cpp: -------------------------------------------------------------------------------- 1 | #include "udp_bridge/packet.h" 2 | #include "udp_bridge/connection.h" 3 | #include 4 | #include "ros/ros.h" 5 | 6 | TEST(UDPBridge_Packet, compressionTest) 7 | { 8 | std::string test_in {"foobar foobar foobar foobar"}; 9 | std::vector data_in(test_in.begin(),test_in.end()); 10 | auto compressed_data = udp_bridge::compress(data_in); 11 | 12 | udp_bridge::CompressedPacket *compressed_packet = reinterpret_cast(compressed_data.data()); 13 | EXPECT_EQ(compressed_packet->type, udp_bridge::PacketType::Compressed); 14 | EXPECT_LE(compressed_packet->uncompressed_size, test_in.size()); 15 | 16 | EXPECT_LT(compressed_data.size(), data_in.size()); 17 | 18 | auto decompressed_data = udp_bridge::uncompress(compressed_data); 19 | EXPECT_EQ(data_in.size(), decompressed_data.size()); 20 | EXPECT_EQ(data_in, decompressed_data); 21 | 22 | std::string decompressed_string(decompressed_data.begin(), decompressed_data.end()); 23 | EXPECT_EQ(test_in, decompressed_string); 24 | } 25 | 26 | TEST(UDPBridge_Packet, addressToDottedTest) 27 | { 28 | sockaddr_in address {}; 29 | address.sin_addr.s_addr = htonl(0x7f000001); 30 | 31 | EXPECT_EQ(udp_bridge::addressToDotted(address),"127.0.0.1"); 32 | } 33 | 34 | 35 | int main(int argc, char **argv){ 36 | testing::InitGoogleTest(&argc, argv); 37 | //ros::init(argc, argv, "udp_bridge_tester"); 38 | //ros::NodeHandle nh; 39 | return RUN_ALL_TESTS(); 40 | } 41 | --------------------------------------------------------------------------------