Merge branch 'master' of https://github.com/OSURoboticsClub/Rover_2017_2018

software/ros_packages/Readme.md

@@ -0,0 +1,10 @@
+# Rover Software
+This code is what runs on the NUC onboard the Rover.
+This handles everything from processing vision data to actually sending drive commands to the wheels.
+
+## Requirements
+* Python 3.X
+* ROS (Version TBD)
+
+## How to Launch
+TBD

software/ros_packages/ground_station/CMakeLists.txt

@@ -0,0 +1,197 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ground_station)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ground_station
+#  CATKIN_DEPENDS rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ground_station.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/ground_station_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ground_station.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

software/ros_packages/ground_station/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ground_station</name>
+  <version>0.0.0</version>
+  <description>The ground_station package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="caperren@todo.todo">caperren</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ground_station</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

software/ros_packages/ground_station/scripts/ground_station_launch.sh

@@ -0,0 +1,17 @@
+#!/usr/bin/env bash
+
+current_folder_name="scripts"
+current_folder_name_length=`expr length $current_folder_name`
+
+launch_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+launch_dir_length=`expr length $launch_dir`
+
+launch_dir_length_without_current_folder=$(($launch_dir_length-$current_folder_name_length))
+
+script_launch_path="${launch_dir:0:$launch_dir_length_without_current_folder}/src"
+cd $script_launch_path
+
+sleep 5
+
+export DISPLAY=:0
+python ground_station.py

software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py

@@ -0,0 +1,187 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+from inputs import devices, GamePad
+from time import time
+
+import rospy
+from rover_control.msg import DriveCommandMessage
+
+#####################################
+# Global Variables
+#####################################
+GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
+
+DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive"
+
+DRIVE_COMMAND_HERTZ = 15
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class LogitechJoystick(QtCore.QThread):
+    def __init__(self):
+        super(LogitechJoystick, self).__init__()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+        self.setup_controller_flag = True
+        self.data_acquisition_and_broadcast_flag = True
+        self.controller_acquired = False
+
+        # ########## Class Variables ##########
+        self.gamepad = None  # type: GamePad
+
+        self.controller_states = {
+            "left_stick_x_axis": 0,
+            "y_axis": 512,
+            "left_stick_center_pressed": 0,
+
+            "right_stick_x_axis": 0,
+            "right_stick_y_axis": 0,
+            "right_stick_center_pressed": 0,
+
+            "left_trigger_z_axis": 0,
+            "left_bumper_pressed": 0,
+
+            "z_axis": 128,
+            "right_bumper_pressed": 0,
+
+            "dpad_x": 0,
+            "dpad_y": 0,
+
+            "select_pressed": 0,
+            "start_pressed": 0,
+            "home_pressed": 0,
+
+            "a_pressed": 0,
+            "b_pressed": 0,
+            "x_pressed": 0,
+            "y_pressed": 0
+        }
+
+        self.raw_mapping_to_class_mapping = {
+            "ABS_X": "left_stick_x_axis",
+            "ABS_Y": "y_axis",
+            "BTN_THUMBL": "left_stick_center_pressed",
+
+            "ABS_RX": "right_stick_x_axis",
+            "ABS_RY": "right_stick_y_axis",
+            "BTN_THUMBR": "right_stick_center_pressed",
+
+            "ABS_Z": "left_trigger_z_axis",
+            "BTN_TL": "left_bumper_pressed",
+
+            "ABS_RZ": "z_axis",
+            "BTN_TR": "right_bumper_pressed",
+
+            "ABS_HAT0X": "dpad_x",
+            "ABS_HAT0Y": "dpad_y",
+
+            "BTN_SELECT": "select_pressed",
+            "BTN_START": "start_pressed",
+            "BTN_MODE": "home_pressed",
+
+            "BTN_SOUTH": "a_pressed",
+            "BTN_EAST": "b_pressed",
+            "BTN_NORTH": "x_pressed",
+            "BTN_WEST": "y_pressed"
+        }
+
+        self.ready = False
+
+        self.start()
+
+    def run(self):
+
+        while self.run_thread_flag:
+            if self.setup_controller_flag:
+                self.controller_acquired = self.__setup_controller()
+                self.setup_controller_flag = False
+            if self.data_acquisition_and_broadcast_flag:
+                self.__get_controller_data()
+
+    def __setup_controller(self):
+        for device in devices.gamepads:
+            print device
+            if device.name == GAME_CONTROLLER_NAME:
+                self.gamepad = device
+
+                return True
+        return False
+
+    def __get_controller_data(self):
+        if self.controller_acquired:
+            events = self.gamepad.read()
+
+            for event in events:
+                if event.code in self.raw_mapping_to_class_mapping:
+                    self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
+            self.ready = True
+            # print "Logitech: %s" % self.controller_states
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class RoverDriveSender(QtCore.QThread):
+    def __init__(self, shared_objects):
+        super(RoverDriveSender, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.right_screen = self.shared_objects["screens"]["right_screen"]
+        self.primary_video_display_label = self.right_screen.primary_video_label  # type:QtWidgets.QLabel
+        self.secondary_video_display_label = self.right_screen.secondary_video_label  # type:QtWidgets.QLabel
+        self.tertiary_video_display_label = self.right_screen.tertiary_video_label  # type:QtWidgets.QLabel
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        self.joystick = LogitechJoystick()
+
+        # ########## Class Variables ##########
+        # Publishers
+        self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
+
+        self.wait_time = 1 / DRIVE_COMMAND_HERTZ
+
+    def run(self):
+        while self.run_thread_flag:
+
+            start_time = time()
+
+            self.__update_and_publish()
+
+            time_diff = time() - start_time
+
+            self.msleep(max(self.wait_time - time_diff, 0))
+
+    def connect_signals_and_slots(self):
+        pass
+
+    def __update_and_publish(self):
+        drive_message = DriveCommandMessage()
+
+        drive_message.drive_twist.linear.x = -(self.joystick.controller_states["y_axis"] - 512) / 1024.0
+        drive_message.drive_twist.angular.z = -(self.joystick.controller_states["z_axis"] - 128) / 255.0
+        self.drive_command_publisher.publish(drive_message)
+        # print self.joystick.controller_states["y_axis"], self.joystick.controller_states["z_axis"]
+
+    def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+    def on_kill_threads_requested__slot(self):
+        self.run_thread_flag = False

software/ros_packages/ground_station/src/Framework/LoggingSystems/Logger.py

@@ -0,0 +1,127 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore
+from os import makedirs, rename, walk, unlink
+from os.path import exists, getmtime
+from os import environ
+import logging
+from datetime import datetime
+
+#####################################
+# Global Variables
+#####################################
+MAX_NUM_LOG_FILES = 30
+
+
+#####################################
+# Logger Definition
+#####################################
+class Logger(QtCore.QObject):
+    def __init__(self, console_output=True):
+        super(Logger, self).__init__()
+
+        # ########## Local class variables ##########
+        self.console_output = console_output
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Set variables with useful paths ##########
+        self.appdata_base_directory = environ["HOME"] + "/.groundstation"
+        self.log_directory = self.appdata_base_directory + "//logs"
+        self.log_file_path = self.log_directory + "//log.txt"
+
+        # ########## Cleanup old log files ##########
+        self.__cleanup_log_files()
+
+        # ########## Set up logger with desired settings ##########
+        self.__setup_logger()
+
+        # ########## Place divider in log file to see new program launch ##########
+        self.__add_startup_log_buffer_text()
+
+    def __setup_logger(self):
+        # Get the appdata directory and make the log path if it doesn't exist
+        if not exists(self.log_directory):
+            makedirs(self.log_directory)
+
+        # Set the debugging level
+        self.logger.setLevel(logging.DEBUG)
+
+        # Make a formatter with the log line format wanted
+        formatter = logging.Formatter(fmt='%(levelname)s : %(asctime)s : %(message)s', datefmt='%m/%d/%y %H:%M:%S')
+
+        # Set up a file handler so everything can be saved and attach it to the logger
+        file_handler = logging.FileHandler(filename=self.log_file_path)
+        file_handler.setFormatter(formatter)
+        file_handler.setLevel(logging.DEBUG)
+        self.logger.addHandler(file_handler)
+
+        # Enable console output if requested
+        if self.console_output:
+            console_handler = logging.StreamHandler()
+            console_handler.setFormatter(formatter)
+            console_handler.setLevel(logging.DEBUG)
+            self.logger.addHandler(console_handler)
+
+    def __cleanup_log_files(self):
+        # This copies the existing log.txt file to an old version with a datetime stamp
+        # It then checks if there are too many log files, and if so, deletes the oldest
+        if exists(self.log_directory):
+            # Get the number of log files
+            num_log_files = self.__get_num_files_in_directory(self.log_directory)
+
+            # Check that we actually have log files
+            if num_log_files > 0:
+                date_time = datetime.now().strftime("%Y%m%d-%H%M%S")
+
+                # If we do, move the current logfile to a backup in the format old_log_datetime
+                if exists(self.log_file_path):
+                    rename(self.log_file_path, self.log_directory + "\\old_log_" + date_time + ".txt")
+
+                # If we have more than the max log files delete the oldest one
+                if num_log_files >= MAX_NUM_LOG_FILES:
+                    unlink(self.__get_name_of_oldest_file(self.log_directory))
+
+    def __add_startup_log_buffer_text(self):
+        # Prints a header saying when the program started
+        self.logger.info("########## Application Starting ##########")
+
+    @staticmethod
+    def __get_name_of_oldest_file(input_path):
+        oldest_file_path = None
+        previous_oldest_time = 0
+
+        # Walk the directory passed in to get all folders and files
+        for dir_path, dir_names, file_names in walk(input_path):
+            # Go through all of the filenames found
+            for file in file_names:
+                # Recreate the full path and get the modified time of the file
+                current_path = dir_path + "\\" + file
+                time = getmtime(current_path)
+
+                # Default case for if the variables above have not been initially set
+                if previous_oldest_time == 0:
+                    previous_oldest_time = time
+                    oldest_file_path = current_path
+
+                # Saves the oldest time and file path of the current file if it's older (lower timestamp) than the
+                # last file saved in the variables
+                if time < previous_oldest_time:
+                    previous_oldest_time = time
+                    oldest_file_path = current_path
+
+        # Returns the path to the oldest file after checking all the files
+        return oldest_file_path
+
+    @staticmethod
+    def __get_num_files_in_directory(input_path):
+        # Walk the directory passed in to get all the files
+        for _, _, file_names in walk(input_path):
+            # Return the number of files found in the directory
+            return len(file_names)

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py

@@ -0,0 +1,426 @@
+'''
+Mapping.py: Objected Orientated Google Maps for Python
+ReWritten by Chris Pham
+
+Copyright OSURC, orginal code from GooMPy by Alec Singer and Simon D. Levy
+
+This code is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as
+published by the Free Software Foundation, either version 3 of the
+License, or (at your option) any later version.
+This code is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+You should have received a copy of the GNU Lesser General Public License
+along with this code.  If not, see <http://www.gnu.org/licenses/>.
+'''
+
+#####################################
+# Imports
+#####################################
+# Python native imports
+import math
+import urllib2
+from io import StringIO, BytesIO
+import os
+import time
+import PIL.ImageDraw
+import signing
+import RoverMapHelper as MapHelper
+
+#####################################
+# Constants
+#####################################
+_KEYS = []
+# Number of pixels in half the earth's circumference at zoom = 21
+_EARTHPIX = 268435456
+# Number of decimal places for rounding coordinates
+_DEGREE_PRECISION = 4
+# Larget tile we can grab without paying
+_TILESIZE = 640
+# Fastest rate at which we can download tiles without paying
+_GRABRATE = 4
+# Pixel Radius of Earth for calculations
+_PIXRAD = _EARTHPIX / math.pi
+_DISPLAYPIX = _EARTHPIX / 2000
+
+file_pointer = open('key', 'r')
+for i in file_pointer:
+    _KEYS.append(i.rstrip())
+file_pointer.close()
+
+
+class GMapsStitcher(object):
+    def __init__(self, width, height,
+                 latitude, longitude, zoom,
+                 maptype, radius_meters=None, num_tiles=4, debug=False):
+        self.helper = MapHelper.MapHelper()
+        self.latitude = latitude
+        self.longitude = longitude
+        self.start_latitude = latitude
+        self.start_longitude = longitude
+        self.width = width
+        self.height = height
+        self.zoom = zoom
+        self.maptype = maptype
+        self.radius_meters = radius_meters
+        self.num_tiles = num_tiles
+        self.display_image = self.helper.new_image(width, height)
+        self.debug = debug
+
+        # Get the big image here
+        self._fetch()
+        self.center_display(latitude, longitude)
+
+    def __str__(self):
+        """
+        This string returns when used in a print statement
+        Useful for debugging and to print current state
+
+        returns STRING
+        """
+        string_builder = ""
+        string_builder += ("Center of the displayed map: %4f, %4f\n" %
+                           (self.center_x, self.center_y))
+        string_builder += ("Center of the big map: %4fx%4f\n" %
+                           (self.start_longitude, self.start_longitude))
+        string_builder += ("Current latitude is: %4f, %4f\n" %
+                           (self.longitude, self.latitude))
+        string_builder += ("The top-left of the box: %dx%d\n" %
+                           (self.left_x, self.upper_y))
+        string_builder += ("Number of tiles genreated: %dx%d\n" %
+                           (self.num_tiles, self.num_tiles))
+        string_builder += "Map Type: %s\n" % (self.maptype)
+        string_builder += "Zoom Level: %s\n" % (self.zoom)
+        string_builder += ("Dimensions of Big Image: %dx%d\n" %
+                           (self.big_image.size[0], self.big_image.size[1]))
+        string_builder += ("Dimensions of Displayed Image: %dx%d\n" %
+                           (self.width, self.height))
+        string_builder += ("LatLong of Northwest Corner: %4f, %4f\n" %
+                           (self.northwest))
+        string_builder += ("LatLong of Southeast Corner: %4f, %4f\n" %
+                           (self.southeast))
+        return string_builder
+
+    def _grab_tile(self, longitude, latitude, sleeptime=0):
+        """
+        This will return the tile at location longitude x latitude.
+        Includes a sleep time to allow for free use if there is no API key
+
+        returns PIL.IMAGE OBJECT
+        """
+        # Make the url string for polling
+        # GET request header gets appended to the string
+        urlbase = 'https://maps.googleapis.com/maps/api/staticmap?'
+        urlbase += 'center=%.4f,%.4f&zoom=%d&maptype=%s'
+        urlbase += '&size=%dx%d&format=png&key=%s'
+
+        # Fill the formatting
+        specs = (self.helper.fast_round(latitude, _DEGREE_PRECISION),
+                 self.helper.fast_round(longitude, _DEGREE_PRECISION),
+                 self.zoom, self.maptype, _TILESIZE, _TILESIZE, _KEYS[0])
+        filename = 'Resources/Maps/' + ('%.4f_%.4f_%d_%s_%d_%d_%s' % specs)
+        filename += '.png'
+
+        # Tile Image object
+        tile_object = None
+
+        if os.path.isfile(filename):
+            tile_object = PIL.Image.open(filename)
+
+        # If file on filesystem
+        else:
+            # make the url
+            url = urlbase % specs
+            url = signing.sign_url(url, _KEYS[1])
+            result = urllib2.urlopen(urllib2.Request(url)).read()
+            tile_object = PIL.Image.open(BytesIO(result))
+            if not os.path.exists('Resources/Maps'):
+                os.mkdir('Resources/Maps')
+            tile_object.save(filename)
+            # Added to prevent timeouts on Google Servers
+            time.sleep(sleeptime)
+
+        return tile_object
+
+    def _pixels_to_lon(self, iterator, lon_pixels):
+        """
+        This converts pixels to degrees to be used in
+        fetching squares and generate correct squares
+
+        returns FLOAT(degrees)
+        """
+        # Magic Lines, no idea
+        degrees = self.helper.pixels_to_degrees(
+            (iterator - self.num_tiles / 2) * _TILESIZE, self.zoom)
+        return math.degrees((lon_pixels + degrees - _EARTHPIX) / _PIXRAD)
+
+    def _pixels_to_lat(self, iterator, lat_pixels):
+        """
+        This converts pixels to latitude using meridian projection
+        to get the latitude to generate squares
+
+        returns FLOAT(degrees)
+        """
+        # Magic Lines
+        return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels +
+                            self.helper.pixels_to_degrees(
+                                (iterator - self.num_tiles /
+                                 2) * _TILESIZE, self.zoom)) -
+                                 _EARTHPIX) / _PIXRAD)))
+
+    def fetch_tiles(self):
+        """
+        Function that handles fetching of files from init'd variables
+
+        returns PIL.IMAGE OBJECT, (WEST, NORTH), (EAST, SOUTH)
+
+        North/East/South/West are in FLOAT(degrees)
+        """
+        # cap floats to precision amount
+        self.latitude = self.helper.fast_round(self.latitude,
+                                               _DEGREE_PRECISION)
+        self.longitude = self.helper.fast_round(self.longitude,
+                                                _DEGREE_PRECISION)
+
+        # number of tiles required to go from center
+        # latitude to desired radius in meters
+        if self.radius_meters is not None:
+            self.num_tiles = (int(
+                              round(2 * self.helper.pixels_to_meters(
+                               self.latitude, self.zoom) /
+                               (_TILESIZE / 2. / self.radius_meters))))
+
+        lon_pixels = _EARTHPIX + self.longitude * math.radians(_PIXRAD)
+
+        sin_lat = math.sin(math.radians(self.latitude))
+        lat_pixels = _EARTHPIX - _PIXRAD * math.log((1+sin_lat)/(1-sin_lat))/2
+        self.big_size = self.num_tiles * _TILESIZE
+        big_image = self.helper.new_image(self.big_size, self.big_size)
+
+        for j in range(self.num_tiles):
+            lon = self._pixels_to_lon(j, lon_pixels)
+            for k in range(self.num_tiles):
+                lat = self._pixels_to_lat(k, lat_pixels)
+                tile = self._grab_tile(lon, lat)
+                big_image.paste(tile, (j * _TILESIZE, k * _TILESIZE))
+
+        west = self._pixels_to_lon(0, lon_pixels)
+        east = self._pixels_to_lon(self.num_tiles - 1, lon_pixels)
+
+        north = self._pixels_to_lat(0, lat_pixels)
+        south = self._pixels_to_lat(self.num_tiles - 1, lat_pixels)
+        return big_image, (north, west), (south, east)
+
+    def move_pix(self, dx, dy):
+        """
+        Function gets change in x and y (dx, dy)
+        then displaces the displayed map that amount
+
+        NO RETURN
+        """
+        self._constrain_x(dx)
+        self._constrain_y(dy)
+        self.update()
+
+    def _constrain_x(self, diff):
+        """
+        Helper for move_pix
+        """
+        new_value = self.left_x - diff
+
+        if ((not new_value > 0) and
+           (new_value < self.big_image.size[0] - self.width)):
+            return self.left_x
+        else:
+            return new_value
+
+    def _constrain_y(self, diff):
+        """
+        Helper for move_pix
+        """
+        new_value = self.upper_y - diff
+
+        if ((not new_value > 0) and
+           (new_value < self.big_image.size[1] - self.height)):
+            return self.upper_y
+        else:
+            return new_value
+
+    def update(self):
+        """
+        Function remakes display image using top left corners
+        """
+        self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
+        # self.display_image.resize((self.image_zoom, self.image_zoom))
+
+    def _fetch(self):
+        """
+        Function generates big image
+        """
+        self.big_image, self.northwest, self.southeast = self.fetch_tiles()
+
+    def move_latlon(self, lat, lon):
+        """
+        Function to move the object/rover
+        """
+        x, y = self._get_cartesian(lat, lon)
+        self._constrain_x(self.center_x-x)
+        self._constrain_y(self.center_y-y)
+        self.update()
+
+    def _get_cartesian(self, lat, lon):
+        """
+        Helper for getting the x, y given lat and lon
+
+        returns INT, INT (x, y)
+        """
+        viewport_lat_nw, viewport_lon_nw = self.northwest
+        viewport_lat_se, viewport_lon_se = self.southeast
+        # print "Lat:", viewport_lat_nw, viewport_lat_se
+        # print "Lon:", viewport_lon_nw, viewport_lon_se
+
+        viewport_lat_diff = viewport_lat_nw - viewport_lat_se
+        viewport_lon_diff = viewport_lon_se - viewport_lon_nw
+
+        # print viewport_lon_diff, viewport_lat_diff
+
+        bigimage_width = self.big_image.size[0]
+        bigimage_height = self.big_image.size[1]
+
+        pixel_per_lat = bigimage_height / viewport_lat_diff
+        pixel_per_lon = bigimage_width / viewport_lon_diff
+        # print "Pixel per:", pixel_per_lat, pixel_per_lon
+
+        new_lat_gps_range_percentage = (viewport_lat_nw - lat)
+        new_lon_gps_range_percentage = (lon - viewport_lon_nw)
+        # print lon, viewport_lon_se
+
+        x = new_lon_gps_range_percentage * pixel_per_lon
+        y = new_lat_gps_range_percentage * pixel_per_lat
+
+        return int(x), int(y)
+
+    def add_gps_location(self, lat, lon, shape, size, fill):
+        """
+        Function adds a shape at lat x lon
+        """
+        x, y = self._get_cartesian(lat, lon)
+        draw = PIL.ImageDraw.Draw(self.big_image)
+        if shape is "ellipsis":
+            draw.ellipsis((x-size, y-size, x+size, y+size), fill)
+        else:
+            draw.rectangle([x-size, y-size, x+size, y+size], fill)
+        self.update()
+
+    def center_display(self, lat, lon):
+        """
+        Function centers the display image
+        """
+        x, y = self._get_cartesian(lat, lon)
+        self.center_x = x
+        self.center_y = y
+
+        self.left_x = (self.center_x - (self.width/2))
+        self.upper_y = (self.center_y - (self.height/2))
+        self.update()
+
+    # def update_rover_map_location(self, lat, lon):
+    #     print "I did nothing"
+
+    # def draw_circle(self, lat, lon, radius, fill):
+    #     print "I did nothing"
+
+    def connect_signals_and_slots(self):
+        pass
+
+
+class OverlayImage(object):
+    def __init__(self, latitude, longitude, northwest, southeast,
+                 big_width, big_height, width, height):
+        self.northwest = northwest
+        self.southeast = southeast
+        self.latitude = latitude
+        self.longitude = longitude
+        self.big_width = big_width
+        self.big_height = big_height
+        self.width = width
+        self.height = height
+        self.big_image = None
+        self.display_image = None
+        self.helper = MapHelper.MapHelper()
+
+        x, y = self._get_cartesian(latitude, longitude)
+        self.center_x = x
+        self.center_y = y
+
+        self.left_x = (self.center_x - (self.width/2))
+        self.upper_y = (self.center_y - (self.height/2))
+
+    def generate_image_files(self):
+        """
+        Creates big_image and display image sizes
+
+        Returns NONE
+        """
+        self.big_image = self.helper.new_image(self.big_width, self.big_height,
+                                               True)
+        self.display_image = self.helper.new_image(self.width, self.height,
+                                                   True)
+
+    def _get_cartesian(self, lat, lon):
+        """
+        Helper for getting the x, y given lat and lon
+
+        returns INT, INT (x, y)
+        """
+        viewport_lat_nw, viewport_lon_nw = self.northwest
+        viewport_lat_se, viewport_lon_se = self.southeast
+        # print "Lat:", viewport_lat_nw, viewport_lat_se
+        # print "Lon:", viewport_lon_nw, viewport_lon_se
+
+        viewport_lat_diff = viewport_lat_nw - viewport_lat_se
+        viewport_lon_diff = viewport_lon_se - viewport_lon_nw
+
+        # print viewport_lon_diff, viewport_lat_diff
+
+        bigimage_width = self.width
+        bigimage_height = self.height
+
+        pixel_per_lat = bigimage_height / viewport_lat_diff
+        pixel_per_lon = bigimage_width / viewport_lon_diff
+        # print "Pixel per:", pixel_per_lat, pixel_per_lon
+
+        new_lat_gps_range_percentage = (viewport_lat_nw - lat)
+        new_lon_gps_range_percentage = (lon - viewport_lon_nw)
+        # print lon, viewport_lon_se
+
+        x = new_lon_gps_range_percentage * pixel_per_lon
+        y = new_lat_gps_range_percentage * pixel_per_lat
+
+        return int(x), int(y)
+
+    def update_new_location(self, latitude, longitude, compass):
+        self._draw_rover(latitude, longitude, 10, compass)
+        self.update()
+
+        return self.display_image
+
+    def _draw_rover(self, lat, lon, size, scaler):
+        x, y = self._get_cartesian(lat, lon)
+        draw = PIL.ImageDraw.Draw(self.big_image)
+        draw.ellipsis((x-size, y-size, x+size, y+size), (255, 255, 255, 0))
+        draw.line(
+            math.cos(x-2*size * scaler),
+            math.sin(y * scaler),
+            math.cos(x * scaler),
+            math.sin(y+2*size * scaler), (255, 255, 255, 0), 25)
+        draw.line(
+            math.cos(x+2*size * scaler),
+            math.sin(y * scaler),
+            math.cos(x * scaler),
+            math.sin(y+2*size * scaler), (255, 255, 255, 0), 25)
+
+    def update(self):
+        self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py

@@ -0,0 +1,104 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets, QtGui
+from PIL.ImageQt import ImageQt
+
+import logging
+
+import rospy
+
+# Custom Imports
+import RoverMap
+
+#####################################
+# Global Variables
+#####################################
+# put some stuff here later so you can remember
+
+
+class RoverMapCoordinator(QtCore.QThread):
+    pixmap_ready_signal = QtCore.pyqtSignal()
+
+    def __init__(self, shared_objects):
+        super(RoverMapCoordinator, self).__init__()
+
+        self.shared_objects = shared_objects
+        self.left_screen = self.shared_objects["screens"]["left_screen"]
+        self.mapping_label = self.left_screen.mapping_label
+
+        self.setings = QtCore.QSettings()
+
+        self.logger = logging.getLogger("groundstation")
+
+        self.run_thread_flag = True
+        self.setup_map_flag = True
+
+        self.google_maps_object = None
+        self.map_image = None
+        self.overlay_image = None
+        self.overlay_image_object = None
+
+        self.map_pixmap = None
+
+    def run(self):
+        self.logger.debug("Starting Map Coordinator Thread")
+
+        while self.run_thread_flag:
+            if self.setup_map_flag:
+                self._map_setup()
+                self.setup_map_flag = False
+            else:
+                self._get_map_image()
+            self.msleep(3)
+
+        self.logger.debug("Stopping Map Coordinator Thread")
+
+    def _setup_map_threads(self):
+        self.google_maps_object = RoverMap.GMapsStitcher(1280,
+                                                         720,
+                                                         44.567161,
+                                                         -123.278432,
+                                                         18,
+                                                         'terrain',
+                                                         None, 20)
+
+    def _map_setup(self):
+        self.google_maps_object = RoverMap.GMapsStitcher(1280,
+                                                         720,
+                                                         44.567161,
+                                                         -123.278432,
+                                                         18,
+                                                         'terrain',
+                                                         None, 20)
+        self.overlay_image_object = (
+            RoverMap.OverlayImage(44.567161, -123.278432,
+                                  self.google_maps_object.northwest,
+                                  self.google_maps_object.southeast,
+                                  self.google_maps_object.big_image[0],
+                                  self.google_maps_object.big_image[1],
+                                  1280, 720))
+
+    def _get_map_image(self):
+        self.map_image = self.google_maps_object.display_image
+        self.map_image.paste(self.overlay_image_object.display_image)
+        # get overlay here
+        qim = ImageQt(self.map_image)
+        self.map_pixmap = QtGui.QPixmap.fromImage(qim)
+        self.pixmap_ready_signal.emit()
+
+    def connect_signals_and_slots(self):
+        self.pixmap_ready_signal.connect(self.pixmap_ready__slot)
+
+    def on_kill_threads_requested_slot(self):
+        self.run_thread_flag = False
+
+    def setup_signals(self, start_signal, signals_and_slots_signal,
+                      kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested_slot)
+
+    def pixmap_ready__slot(self):
+        self.mapping_label.setPixmap(self.map_pixmap)

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapHelper.py

@@ -0,0 +1,46 @@
+import PIL.Image
+import math
+
+
+class MapHelper(object):
+
+    @staticmethod
+    def new_image(width, height, alpha=False):
+        """
+        Generates a new image using PIL.Image module
+
+        returns PIL.IMAGE OBJECT
+        """
+        if alpha is True:
+            return PIL.Image.new('RGBA', (width, height), (0, 0, 0, 255))
+        else:
+            return PIL.Image.new('RGBA', (width, height))
+
+    @staticmethod
+    def fast_round(value, precision):
+        """
+        Function to round values instead of using python's
+
+        return INT
+        """
+        return int(value * 10 ** precision) / 10. ** precision
+
+    @staticmethod
+    def pixels_to_degrees(pixels, zoom):
+        """
+        Generates pixels to be expected at zoom levels
+
+        returns INT
+        """
+        return pixels * 2 ** (21-zoom)
+
+    @staticmethod
+    def pixels_to_meters(latitude, zoom):
+        """
+        Function generates how many pixels per meter it
+        should be from the projecction
+
+        returns FLOAT
+        """
+        # https://groups.google.com/forum/#!topic/google-maps-js-api-v3/hDRO4oHVSeM
+        return 2 ** zoom / (156543.03392 * math.cos(math.radians(latitude)))

software/ros_packages/ground_station/src/Framework/MapSystems/signing.py

@@ -0,0 +1,53 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+""" Signs a URL using a URL signing secret """
+
+import hashlib
+import hmac
+import base64
+import urlparse
+
+def sign_url(input_url=None, secret=None):
+  """ Sign a request URL with a URL signing secret.
+
+      Usage:
+      from urlsigner import sign_url
+
+      signed_url = sign_url(input_url=my_url, secret=SECRET)
+
+      Args:
+      input_url - The URL to sign
+      secret    - Your URL signing secret
+
+      Returns:
+      The signed request URL
+  """
+
+  if not input_url or not secret:
+    raise Exception("Both input_url and secret are required")
+
+  url = urlparse.urlparse(input_url)
+
+  # We only need to sign the path+query part of the string
+  url_to_sign = url.path + "?" + url.query
+
+  # Decode the private key into its binary format
+  # We need to decode the URL-encoded private key
+  decoded_key = base64.urlsafe_b64decode(secret)
+
+  # Create a signature using the private key and the URL-encoded
+  # string using HMAC SHA1. This signature will be binary.
+  signature = hmac.new(decoded_key, url_to_sign, hashlib.sha1)
+
+  # Encode the binary signature into base64 for use within a URL
+  encoded_signature = base64.urlsafe_b64encode(signature.digest())
+
+  original_url = url.scheme + "://" + url.netloc + url.path + "?" + url.query
+
+  # Return signed URL
+  return original_url + "&signature=" + encoded_signature
+
+if __name__ == "__main__":
+  input_url = raw_input("URL to Sign: ")
+  secret = raw_input("URL signing secret: ")
+  print "Signed URL: " + sign_url(input_url, secret)

software/ros_packages/ground_station/src/Framework/StartupSystems/ROSMasterChecker.py

@@ -0,0 +1,45 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtGui, QtWidgets
+import time
+import logging
+import socket
+
+import rospy
+
+# Custom Imports
+
+#####################################
+# Global Variables
+#####################################
+
+
+#####################################
+# RoverVideoReceiver Class Definition
+#####################################
+class ROSMasterChecker(QtCore.QThread):
+    def __init__(self):
+        super(ROSMasterChecker, self).__init__()
+
+        # ########## Class Variables ##########
+        self.ros_master_present = False
+
+        self.start_time = time.time()
+        self.start()
+
+    def run(self):
+        try:
+            master = rospy.get_master()
+            master.getPid()
+            self.ros_master_present = True
+        except socket.error:
+            return
+
+    def master_present(self, timeout):
+        while self.isRunning() and (time.time() - self.start_time) < timeout:
+            self.msleep(100)
+
+        return self.ros_master_present
+

software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py

@@ -0,0 +1,199 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+
+import rospy
+
+# Custom Imports
+import RoverVideoReceiver
+
+#####################################
+# Global Variables
+#####################################
+CAMERA_TOPIC_PATH = "/cameras/"
+EXCLUDED_CAMERAS = ["zed"]
+#
+PRIMARY_LABEL_MAX = (640, 360)
+SECONDARY_LABEL_MAX = (256, 144)
+TERTIARY_LABEL_MAX = (256, 144)
+
+# PRIMARY_LABEL_MAX = (1280, 720)
+# SECONDARY_LABEL_MAX = (640, 360)
+# TERTIARY_LABEL_MAX = (640, 360)
+
+
+#####################################
+# RoverVideoCoordinator Class Definition
+#####################################
+class RoverVideoCoordinator(QtCore.QThread):
+    pixmap_ready_signal = QtCore.pyqtSignal(str)
+
+    def __init__(self, shared_objects):
+        super(RoverVideoCoordinator, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.right_screen = self.shared_objects["screens"]["right_screen"]
+        self.primary_video_display_label = self.right_screen.primary_video_label  # type:QtWidgets.QLabel
+        self.secondary_video_display_label = self.right_screen.secondary_video_label  # type:QtWidgets.QLabel
+        self.tertiary_video_display_label = self.right_screen.tertiary_video_label  # type:QtWidgets.QLabel
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        self.setup_cameras_flag = True
+
+        # ########## Class Variables ##########
+        # Camera variables
+        self.camera_threads = {}
+        self.valid_cameras = []
+        self.disabled_cameras = []
+
+        # Setup cameras
+        self.__get_cameras()
+        self.__setup_video_threads()
+
+        self.primary_label_current_setting = 0
+        self.secondary_label_current_setting = 0
+        self.tertiary_label_current_setting = 0
+
+        self.primary_label_max_resolution = -1
+        self.secondary_label_max_resolution = -1
+        self.tertiary_label_max_resolution = -1
+
+        self.first_image_received = False
+
+    def run(self):
+        self.logger.debug("Starting Video Coordinator Thread")
+
+        self.__set_max_resolutions()  # Do this initially so we don't try to disable cameras before they're set up
+        self.msleep(500)
+
+        while self.run_thread_flag:
+            self.__set_max_resolutions()
+            self.__toggle_background_cameras_if_needed()
+            self.msleep(10)
+
+        self.__wait_for_camera_threads()
+
+        self.logger.debug("Stopping Video Coordinator Thread")
+
+    def __set_max_resolutions(self):
+        self.primary_label_max_resolution = self.camera_threads[
+            self.valid_cameras[self.primary_label_current_setting]].current_max_resolution
+        self.secondary_label_max_resolution = self.camera_threads[
+            self.valid_cameras[self.secondary_label_current_setting]].current_max_resolution
+        self.tertiary_label_max_resolution = self.camera_threads[
+            self.valid_cameras[self.tertiary_label_current_setting]].current_max_resolution
+
+        if self.primary_label_max_resolution != PRIMARY_LABEL_MAX:
+            self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].change_max_resolution_setting(
+                PRIMARY_LABEL_MAX)
+
+        if self.secondary_label_max_resolution != SECONDARY_LABEL_MAX and self.secondary_label_current_setting != self.primary_label_current_setting:
+            self.camera_threads[self.valid_cameras[self.secondary_label_current_setting]].change_max_resolution_setting(
+                SECONDARY_LABEL_MAX)
+
+        if self.tertiary_label_max_resolution != TERTIARY_LABEL_MAX and self.tertiary_label_current_setting != self.primary_label_current_setting:
+            self.camera_threads[self.valid_cameras[self.tertiary_label_current_setting]].change_max_resolution_setting(
+                TERTIARY_LABEL_MAX)
+
+    def __toggle_background_cameras_if_needed(self):
+        if self.first_image_received:
+            enabled = list({self.primary_label_current_setting, self.secondary_label_current_setting,
+                            self.tertiary_label_current_setting})
+
+            for camera_index, camera_name in enumerate(self.valid_cameras):
+                if camera_index not in enabled and camera_index not in self.disabled_cameras:
+                    self.camera_threads[camera_name].toggle_video_display()
+                    self.disabled_cameras.append(camera_index)
+                elif camera_index in enabled and camera_index in self.disabled_cameras:
+                    self.camera_threads[camera_name].toggle_video_display()
+                    self.disabled_cameras.remove(camera_index)
+
+    def __get_cameras(self):
+        topics = rospy.get_published_topics(CAMERA_TOPIC_PATH)
+
+        names = []
+
+        for topics_group in topics:
+            main_topic = topics_group[0]
+            if "heartbeat" in main_topic:
+                continue
+            camera_name = main_topic.split("/")[2]
+            names.append(camera_name)
+
+        names = set(names)
+
+        for camera in EXCLUDED_CAMERAS:
+            if camera in names:
+                names.remove(camera)
+
+        self.valid_cameras = list(names)
+
+    def __setup_video_threads(self):
+        for camera in self.valid_cameras:
+            self.camera_threads[camera] = RoverVideoReceiver.RoverVideoReceiver(camera)
+
+    def __wait_for_camera_threads(self):
+        for camera in self.camera_threads:
+            self.camera_threads[camera].wait()
+
+    def connect_signals_and_slots(self):
+        for thread in self.camera_threads:
+            self.camera_threads[thread].image_ready_signal.connect(self.pixmap_ready__slot)
+
+        self.primary_video_display_label.mousePressEvent = self.__change_display_source_primary_mouse_press_event
+        self.secondary_video_display_label.mousePressEvent = self.__change_display_source_secondary_mouse_press_event
+        self.tertiary_video_display_label.mousePressEvent = self.__change_display_source_tertiary_mouse_press_event
+
+    def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+        for camera in self.camera_threads:
+            self.camera_threads[camera].setup_signals(start_signal, signals_and_slots_signal, kill_signal)
+
+    def __change_display_source_primary_mouse_press_event(self, event):
+        if event.button() == QtCore.Qt.LeftButton:
+            self.primary_label_current_setting = (self.primary_label_current_setting + 1) % len(self.valid_cameras)
+        elif event.button() == QtCore.Qt.RightButton:
+            self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].toggle_video_display()
+
+    def __change_display_source_secondary_mouse_press_event(self, event):
+        if event.button() == QtCore.Qt.LeftButton:
+            self.secondary_label_current_setting = (self.secondary_label_current_setting + 1) % len(self.valid_cameras)
+        elif event.button() == QtCore.Qt.RightButton:
+            self.camera_threads[self.valid_cameras[self.secondary_label_current_setting]].toggle_video_display()
+
+    def __change_display_source_tertiary_mouse_press_event(self, event):
+        if event.button() == QtCore.Qt.LeftButton:
+            self.tertiary_label_current_setting = (self.tertiary_label_current_setting + 1) % len(self.valid_cameras)
+        elif event.button() == QtCore.Qt.RightButton:
+            self.camera_threads[self.valid_cameras[self.tertiary_label_current_setting]].toggle_video_display()
+
+    def pixmap_ready__slot(self, camera):
+        if not self.first_image_received:
+            self.first_image_received = True
+
+        if self.valid_cameras[self.primary_label_current_setting] == camera:
+            self.primary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_1280x720_image)
+
+        if self.valid_cameras[self.secondary_label_current_setting] == camera:
+            self.secondary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
+
+        if self.valid_cameras[self.tertiary_label_current_setting] == camera:
+            self.tertiary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
+
+    def on_kill_threads_requested__slot(self):
+        self.run_thread_flag = False

software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py

@@ -0,0 +1,256 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtGui, QtWidgets
+import logging
+import cv2
+import numpy as np
+import qimage2ndarray
+
+import rospy
+import dynamic_reconfigure.client
+from cv_bridge import CvBridge
+from sensor_msgs.msg import CompressedImage
+
+# Custom Imports
+
+#####################################
+# Global Variables
+#####################################
+CAMERA_TOPIC_PATH = "/cameras/"
+
+QUALITY_MAX = 80
+QUALITY_MIN = 15
+
+
+#####################################
+# RoverVideoReceiver Class Definition
+#####################################
+class RoverVideoReceiver(QtCore.QThread):
+    image_ready_signal = QtCore.pyqtSignal(str)
+
+    def __init__(self, camera_name):
+        super(RoverVideoReceiver, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.camera_name = camera_name
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.camera_title_name = self.camera_name.replace("_", " ").title()
+
+        self.topic_base_path = CAMERA_TOPIC_PATH + self.camera_name
+        self.camera_topics = {}
+
+        self.current_max_resolution = None
+
+        self.current_camera_settings = {
+            "resolution": None,
+            "quality_setting": QUALITY_MAX,
+
+        }
+
+        self.previous_camera_settings = self.current_camera_settings.copy()
+
+        self.temp_topic_path = CAMERA_TOPIC_PATH + self.camera_name + "/image_640x360/compressed"
+
+        # Subscription variables
+        self.video_subscriber = None  # type: rospy.Subscriber
+
+        # Image variables
+        self.raw_image = None
+        self.opencv_1280x720_image = None
+        self.opencv_640x360_image = None
+
+        self.pixmap_1280x720_image = None
+        self.pixmap_640x360_image = None
+
+        # Processing variables
+        self.bridge = CvBridge()  # OpenCV ROS Video Data Processor
+        self.video_enabled = True
+        self.new_frame = False
+
+        # Text Drawing Variables
+        self.font = cv2.FONT_HERSHEY_TRIPLEX
+        self.font_thickness = 1
+        self.font_baseline = 0
+
+        self.camera_name_opencv_1280x720_image = None
+        self.camera_name_opencv_640x360_image = None
+
+        # ROS Dynamic Reconfigure Client
+        self.reconfigure_clients = {}
+
+        # Initial class setup to make text images and get camera resolutions
+        self.__create_camera_name_opencv_images()
+        self.__get_camera_available_resolutions()
+        #self.__setup_reconfigure_clients()
+
+    def run(self):
+        self.logger.debug("Starting \"%s\" Camera Thread" % self.camera_title_name)
+
+        while self.run_thread_flag:
+            if self.video_enabled:
+                self.__show_video_enabled()
+            else:
+                self.__show_video_disabled()
+
+            self.msleep(10)
+
+        self.logger.debug("Stopping \"%s\" Camera Thread" % self.camera_title_name)
+
+    def __perform_quality_check_and_adjust(self):
+        self.__set_jpeg_quality(self.current_camera_settings["quality_setting"])
+
+    def __set_jpeg_quality(self, quality_setting):
+        self.reconfigure_clients[self.current_camera_settings["resolution"]].update_configuration({"jpeg_quality": quality_setting})
+
+    def __setup_reconfigure_clients(self):
+        for resolution_group in self.camera_topics:
+            image_topic_string = "image_%sx%s" % resolution_group
+            full_topic = self.topic_base_path + "/" + image_topic_string + "/compressed"
+            self.reconfigure_clients[resolution_group] = dynamic_reconfigure.client.Client(full_topic)
+
+    def __get_camera_available_resolutions(self):
+        topics = rospy.get_published_topics(self.topic_base_path)
+
+        resolution_options = []
+
+        for topics_group in topics:
+            main_topic = topics_group[0]
+            if "heartbeat" in main_topic:
+                continue
+            camera_name = main_topic.split("/")[3]
+            resolution_options.append(camera_name)
+
+        resolution_options = list(set(resolution_options))
+
+        for resolution in resolution_options:
+            # Creates a tuple in (width, height) format that we can use as the key
+            group = int(resolution.split("image_")[1].split("x")[0]), int(resolution.split("image_")[1].split("x")[1])
+            self.camera_topics[group] = self.topic_base_path + "/" + resolution + "/compressed"
+
+    def __update_camera_subscription_and_settings(self):
+        if self.current_camera_settings["resolution"] != self.previous_camera_settings["resolution"]:
+
+            if self.video_subscriber:
+                self.video_subscriber.unregister()
+            new_topic = self.camera_topics[self.current_camera_settings["resolution"]]
+            self.video_subscriber = rospy.Subscriber(new_topic, CompressedImage, self.__image_data_received_callback)
+
+            self.new_frame = False
+            while not self.new_frame:
+                self.msleep(10)
+
+            self.previous_camera_settings["resolution"] = self.current_camera_settings["resolution"]
+
+    def __show_video_enabled(self):
+        self.__update_camera_subscription_and_settings()
+
+        if self.new_frame and self.current_camera_settings["resolution"]:
+            # self.__perform_quality_check_and_adjust()
+
+            opencv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
+
+            self.__create_final_pixmaps(opencv_image)
+
+            self.image_ready_signal.emit(self.camera_name)
+            self.new_frame = False
+
+    def __show_video_disabled(self):
+        blank_frame = np.zeros((720, 1280, 3), np.uint8)
+
+        self.__create_final_pixmaps(blank_frame)
+
+        self.image_ready_signal.emit(self.camera_name)
+
+    def __create_final_pixmaps(self, opencv_image):
+        height, width, _ = opencv_image.shape
+
+        if width != 1280 and height != 720:
+            self.opencv_1280x720_image = cv2.resize(opencv_image, (1280, 720))
+        else:
+            self.opencv_1280x720_image = opencv_image
+
+        if width != 640 and height != 360:
+            self.opencv_640x360_image = cv2.resize(opencv_image, (640, 360))
+        else:
+            self.opencv_640x360_image = opencv_image
+
+        self.__apply_camera_name(self.opencv_1280x720_image, self.camera_name_opencv_1280x720_image)
+        self.__apply_camera_name(self.opencv_640x360_image, self.camera_name_opencv_640x360_image)
+
+        self.pixmap_1280x720_image = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(
+            self.opencv_1280x720_image))
+        self.pixmap_640x360_image = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(
+            self.opencv_640x360_image))
+
+    def __image_data_received_callback(self, raw_image):
+        self.raw_image = raw_image
+        self.new_frame = True
+
+    def __create_camera_name_opencv_images(self):
+        camera_name_text_width, camera_name_text_height = \
+            cv2.getTextSize(self.camera_title_name, self.font, self.font_thickness, self.font_baseline)[0]
+
+        camera_name_width_buffered = camera_name_text_width + 10
+        camera_name_height_buffered = camera_name_text_height + 20
+
+        camera_name_opencv_image = np.zeros(
+            (camera_name_height_buffered, camera_name_width_buffered, 3), np.uint8)
+
+        cv2.putText(
+            camera_name_opencv_image,
+            self.camera_title_name,
+            ((camera_name_width_buffered - camera_name_text_width) / 2, int((camera_name_height_buffered * 2) / 3)),
+            self.font,
+            1,
+            (255, 255, 255),
+            1,
+            cv2.LINE_AA)
+
+        self.camera_name_opencv_1280x720_image = \
+            cv2.resize(camera_name_opencv_image, (camera_name_width_buffered, camera_name_height_buffered))
+
+        self.camera_name_opencv_640x360_image = \
+            cv2.resize(camera_name_opencv_image, (camera_name_width_buffered / 2, camera_name_height_buffered / 2))
+
+    def change_max_resolution_setting(self, resolution_max):
+        self.current_max_resolution = resolution_max
+        self.current_camera_settings["resolution"] = resolution_max
+
+    def toggle_video_display(self):
+        if self.video_enabled:
+            if self.video_subscriber:
+                self.video_subscriber.unregister()
+            self.new_frame = True
+            self.video_enabled = False
+        else:
+            new_topic = self.camera_topics[self.current_camera_settings["resolution"]]
+            self.video_subscriber = rospy.Subscriber(new_topic, CompressedImage, self.__image_data_received_callback)
+            self.video_enabled = True
+
+    def connect_signals_and_slots(self):
+        pass
+
+    def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+    def on_kill_threads_requested__slot(self):
+        self.run_thread_flag = False
+
+    @staticmethod
+    def __apply_camera_name(opencv_image, font_opencv_image):
+        opencv_image[0:0 + font_opencv_image.shape[0], 0:0 + font_opencv_image.shape[1]] = \
+            font_opencv_image

software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiverOld.py

@@ -0,0 +1,134 @@
+import sys
+from PyQt5 import QtWidgets, QtCore, QtGui, uic
+import signal
+import rospy
+import time
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import qimage2ndarray
+import numpy as np
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import CompressedImage
+#from sensor_msgs.msg import Image, CompressedImage
+
+
+class VideoTest(QtCore.QThread):
+    publish_message_signal = QtCore.pyqtSignal()
+    image_ready_signal = QtCore.pyqtSignal()
+
+    def __init__(self, shared_objects, screen_label, video_size=None, sub_path=None):
+        super(VideoTest, self).__init__()
+
+        self.not_abort = True
+
+        self.shared_objects = shared_objects
+
+        self.right_screen_label = screen_label  # type: QtGui.QPixmap
+        self.video_size = video_size
+
+        self.message = None
+
+        self.publisher = rospy.Subscriber(sub_path, CompressedImage, self.__receive_message)
+
+        self.raw_image = None
+        self.cv_image = None
+        self.pixmap = None
+        self.bridge = CvBridge()
+        # self.bridge.com
+
+        self.new_frame = False
+        self.frame_count = 0
+        self.last_frame_time = time.time()
+        self.fps = 0
+
+        self.name = sub_path.split("/")[2].replace("_", " ").title()
+
+        self.font = cv2.FONT_HERSHEY_TRIPLEX
+
+        thickness = 1
+        baseline = 0
+
+        text_size = cv2.getTextSize(self.name, self.font, thickness, baseline)
+        print text_size
+
+        text_width, text_height = text_size[0]
+
+        width = text_width + 10
+        height = text_height + 20
+
+        self.blank_image = np.zeros((height, width, 3), np.uint8)
+        cv2.putText(self.blank_image, self.name, ((width - text_width) / 2, int((height * 2) / 3)), self.font, 1, (255, 255, 255), 1, cv2.LINE_AA)
+        self.blank_image = cv2.resize(self.blank_image, (width / 2, height / 2))
+
+    def run(self):
+        # TODO: Thread starting message here
+        y_offset = 0
+        x_offset = 0
+
+        while self.not_abort:
+            if self.raw_image and self.new_frame:
+                self.cv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
+
+                self.cv_image = self.__show_fps(self.cv_image)
+
+                self.cv_image[y_offset:y_offset + self.blank_image.shape[0], x_offset:x_offset + self.blank_image.shape[1]] = self.blank_image
+
+                if self.video_size:
+                    self.cv_image = cv2.resize(self.cv_image, self.video_size)
+                self.pixmap = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(self.cv_image))
+                self.image_ready_signal.emit()
+                self.new_frame = False
+
+            if (time.time() - self.last_frame_time) >= 0.5:
+                self.fps = int(self.frame_count / (time.time() - self. last_frame_time))
+
+                self.last_frame_time = time.time()
+                self.frame_count = 0
+
+            self.msleep(18)
+        # TODO: Thread ending message here
+
+    def __show_fps(self, image):
+        thickness = 1
+        baseline = 0
+
+        fps_string = str(self.fps)
+
+        text_size = cv2.getTextSize(fps_string, self.font, thickness, baseline)
+
+        text_width, text_height = text_size[0]
+
+        width = text_width + 10
+        height = text_height + 20
+
+        fps_image = np.zeros((height, width, 3), np.uint8)
+
+        cv2.putText(fps_image, fps_string, ((width - text_width) / 2, int((height * 2) / 3)), self.font, 1, (255, 255, 255), 1, cv2.LINE_AA)
+        fps_image = cv2.resize(fps_image, (width / 2, height / 2))
+
+        y_offset = 0
+        x_offset = (image.shape[1] - fps_image.shape[1]) / 2
+
+        image[y_offset:y_offset + fps_image.shape[0], x_offset:x_offset + fps_image.shape[1]] = fps_image
+
+        return image
+
+    def __on_image_update_ready(self):
+        self.right_screen_label.setPixmap(self.pixmap)
+
+    def __receive_message(self, message):
+        self.raw_image = message
+        self.new_frame = True
+        self.frame_count += 1
+
+    def connect_signals_and_slots(self):
+        self.image_ready_signal.connect(self.__on_image_update_ready)
+
+    def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+    def on_kill_threads_requested__slot(self):
+        self.not_abort = False

software/ros_packages/ground_station/src/Readme.md

@@ -0,0 +1,14 @@
+# Ground Station Software
+This code is for the ground station side of the Mars Rover project.
+
+## Requirements
+* Python 2.X
+* PyQt5
+* ROS Kinetic
+
+Python Libraries
+* inputs
+
+## How to Launch
+Navigate to this directory on a linux system with the requirements listed above installed.
+Run "python RoverGroundStation.py" or ./RoverGroundStation.py

software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui

@@ -0,0 +1,345 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>1920</width>
+    <height>1080</height>
+   </rect>
+  </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
+  <property name="minimumSize">
+   <size>
+    <width>1920</width>
+    <height>1080</height>
+   </size>
+  </property>
+  <property name="maximumSize">
+   <size>
+    <width>1920</width>
+    <height>1080</height>
+   </size>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <property name="windowOpacity">
+   <double>1.000000000000000</double>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <layout class="QHBoxLayout" name="horizontalLayout">
+    <property name="spacing">
+     <number>0</number>
+    </property>
+    <property name="sizeConstraint">
+     <enum>QLayout::SetDefaultConstraint</enum>
+    </property>
+    <property name="margin">
+     <number>0</number>
+    </property>
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout_2">
+      <property name="spacing">
+       <number>0</number>
+      </property>
+      <item>
+       <layout class="QVBoxLayout" name="verticalLayout_4">
+        <property name="spacing">
+         <number>0</number>
+        </property>
+        <item>
+         <widget class="QWidget" name="arm_visualization_widget" native="true">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="minimumSize">
+           <size>
+            <width>640</width>
+            <height>720</height>
+           </size>
+          </property>
+          <property name="maximumSize">
+           <size>
+            <width>640</width>
+            <height>720</height>
+           </size>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_4">
+          <property name="spacing">
+           <number>0</number>
+          </property>
+          <property name="sizeConstraint">
+           <enum>QLayout::SetNoConstraint</enum>
+          </property>
+          <item>
+           <widget class="QWidget" name="heading_widget" native="true">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>320</width>
+              <height>360</height>
+             </size>
+            </property>
+            <property name="maximumSize">
+             <size>
+              <width>320</width>
+              <height>360</height>
+             </size>
+            </property>
+            <property name="styleSheet">
+             <string notr="true">background-color:black;;</string>
+            </property>
+            <layout class="QGridLayout" name="gridLayout">
+             <property name="margin">
+              <number>0</number>
+             </property>
+             <property name="spacing">
+              <number>0</number>
+             </property>
+             <item row="0" column="0">
+              <widget class="QLabel" name="compass_label">
+               <property name="text">
+                <string/>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </widget>
+          </item>
+          <item>
+           <widget class="QWidget" name="speed_limit_widget" native="true">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="minimumSize">
+             <size>
+              <width>320</width>
+              <height>360</height>
+             </size>
+            </property>
+            <property name="maximumSize">
+             <size>
+              <width>320</width>
+              <height>360</height>
+             </size>
+            </property>
+            <layout class="QVBoxLayout" name="verticalLayout">
+             <item>
+              <widget class="QLabel" name="label">
+               <property name="font">
+                <font>
+                 <pointsize>22</pointsize>
+                 <weight>75</weight>
+                 <bold>true</bold>
+                </font>
+               </property>
+               <property name="text">
+                <string>Current Speed</string>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLabel" name="label_2">
+               <property name="font">
+                <font>
+                 <pointsize>22</pointsize>
+                </font>
+               </property>
+               <property name="text">
+                <string>2.4 m/s</string>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLabel" name="label_3">
+               <property name="font">
+                <font>
+                 <pointsize>22</pointsize>
+                 <weight>75</weight>
+                 <bold>true</bold>
+                </font>
+               </property>
+               <property name="text">
+                <string>Speed Limit</string>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QProgressBar" name="progressBar">
+               <property name="value">
+                <number>50</number>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
+               <property name="orientation">
+                <enum>Qt::Horizontal</enum>
+               </property>
+               <property name="invertedAppearance">
+                <bool>false</bool>
+               </property>
+               <property name="textDirection">
+                <enum>QProgressBar::TopToBottom</enum>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLabel" name="label_4">
+               <property name="font">
+                <font>
+                 <pointsize>22</pointsize>
+                 <weight>75</weight>
+                 <bold>true</bold>
+                </font>
+               </property>
+               <property name="text">
+                <string>Tank Drive Output</string>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QProgressBar" name="progressBar_3">
+               <property name="value">
+                <number>0</number>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QProgressBar" name="progressBar_2">
+               <property name="value">
+                <number>0</number>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </widget>
+          </item>
+         </layout>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </item>
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout_3">
+      <property name="spacing">
+       <number>0</number>
+      </property>
+      <item>
+       <widget class="QLabel" name="primary_video_label">
+        <property name="minimumSize">
+         <size>
+          <width>1280</width>
+          <height>720</height>
+         </size>
+        </property>
+        <property name="maximumSize">
+         <size>
+          <width>1280</width>
+          <height>720</height>
+         </size>
+        </property>
+        <property name="styleSheet">
+         <string notr="true">background-color: darkblue;</string>
+        </property>
+        <property name="text">
+         <string/>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <layout class="QHBoxLayout" name="horizontalLayout_3">
+        <property name="spacing">
+         <number>0</number>
+        </property>
+        <item>
+         <widget class="QLabel" name="secondary_video_label">
+          <property name="minimumSize">
+           <size>
+            <width>640</width>
+            <height>360</height>
+           </size>
+          </property>
+          <property name="maximumSize">
+           <size>
+            <width>640</width>
+            <height>360</height>
+           </size>
+          </property>
+          <property name="styleSheet">
+           <string notr="true">background-color:darkgreen;</string>
+          </property>
+          <property name="text">
+           <string/>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QLabel" name="tertiary_video_label">
+          <property name="minimumSize">
+           <size>
+            <width>640</width>
+            <height>360</height>
+           </size>
+          </property>
+          <property name="maximumSize">
+           <size>
+            <width>640</width>
+            <height>360</height>
+           </size>
+          </property>
+          <property name="styleSheet">
+           <string notr="true">background-color:maroon;</string>
+          </property>
+          <property name="text">
+           <string/>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </item>
+   </layout>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

software/ros_packages/ground_station/src/ground_station.py

@@ -0,0 +1,185 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+import sys
+from PyQt5 import QtWidgets, QtCore, QtGui, uic
+import signal
+import rospy
+import logging
+import qdarkstyle
+import PIL.Image
+from PIL.ImageQt import ImageQt
+
+# Custom Imports
+import Framework.StartupSystems.ROSMasterChecker as ROSMasterChecker
+import Framework.LoggingSystems.Logger as Logger
+import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator
+import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator
+import Framework.DriveSystems.RoverDriveSender as RoverDriveSender
+
+#####################################
+# Global Variables
+#####################################
+UI_FILE_LEFT = "Resources/Ui/left_screen.ui"
+UI_FILE_RIGHT = "Resources/Ui/right_screen.ui"
+
+#####################################
+# Class Organization
+#####################################
+# Class Name:
+#   "init"
+#   "run (if there)" - personal pref
+#   "private methods"
+#   "public methods, minus slots"
+#   "slot methods"
+#   "static methods"
+#   "run (if there)" - personal pref
+
+
+#####################################
+# ApplicationWindow Class Definition
+#####################################
+class ApplicationWindow(QtWidgets.QMainWindow):
+    exit_requested_signal = QtCore.pyqtSignal()
+
+    kill_threads_signal = QtCore.pyqtSignal()
+
+    def __init__(self, parent=None, ui_file_path=None):
+        super(ApplicationWindow, self).__init__(parent)
+
+        uic.loadUi(ui_file_path, self)
+
+        QtWidgets.QShortcut(QtGui.QKeySequence("Ctrl+Q"), self, self.exit_requested_signal.emit)
+
+
+#####################################
+# GroundStation Class Definition
+#####################################
+class GroundStation(QtCore.QObject):
+    LEFT_SCREEN_ID = 1
+    RIGHT_SCREEN_ID = 0
+
+    exit_requested_signal = QtCore.pyqtSignal()
+
+    start_threads_signal = QtCore.pyqtSignal()
+    connect_signals_and_slots_signal = QtCore.pyqtSignal()
+    kill_threads_signal = QtCore.pyqtSignal()
+
+    def __init__(self, parent=None,):
+        # noinspection PyArgumentList
+        super(GroundStation, self).__init__(parent)
+
+        # ##### Setup the Logger Immediately #####
+        self.logger_setup_class = Logger.Logger(console_output=True)  # Doesn't need to be shared
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        self.shared_objects = {
+            "screens": {},
+            "regular_classes": {},
+            "threaded_classes": {}
+        }
+
+        # ###### Instantiate Left And Right Screens ######
+        self.shared_objects["screens"]["left_screen"] = \
+            self.create_application_window(UI_FILE_LEFT, "Rover Ground Station Left Screen",
+                                           self.LEFT_SCREEN_ID)  # type: ApplicationWindow
+
+        self.shared_objects["screens"]["right_screen"] = \
+            self.create_application_window(UI_FILE_RIGHT, "Rover Ground Station Right Screen",
+                                           self.RIGHT_SCREEN_ID)  # type: ApplicationWindow
+
+        # ###### Initialize the Ground Station Node ######
+        rospy.init_node("ground_station")
+
+        # ##### Instantiate Regular Classes ######
+
+        # ##### Instantiate Threaded Classes ######
+        self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
+        self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects))
+        self.__add_thread("Rover Drive Sender", RoverDriveSender.RoverDriveSender(self.shared_objects))
+        self.connect_signals_and_slots_signal.emit()
+        self.__connect_signals_to_slots()
+        self.start_threads_signal.emit()
+        
+        compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300))  # PIL.Image     
+        self.shared_objects["screens"]["right_screen"].compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image))) 
+
+    def ___ros_master_running(self):
+        checker = ROSMasterChecker.ROSMasterChecker()
+
+        if not checker.master_present(5):
+            self.logger.debug("ROS Master Not Found!!!! Exiting!!!")
+            QtGui.QGuiApplication.exit()
+            return False
+        return True
+
+    def __add_thread(self, thread_name, instance):
+        self.shared_objects["threaded_classes"][thread_name] = instance
+        instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
+                               self.kill_threads_signal)
+
+    def __connect_signals_to_slots(self):
+        self.shared_objects["screens"]["left_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)
+        self.shared_objects["screens"]["right_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)
+
+    def on_exit_requested__slot(self):
+        self.kill_threads_signal.emit()
+
+        # Wait for Threads
+        for thread in self.shared_objects["threaded_classes"]:
+            self.shared_objects["threaded_classes"][thread].wait()
+
+        QtGui.QGuiApplication.exit()
+
+    @staticmethod
+    def create_application_window(ui_file_path, title, display_screen):
+        system_desktop = QtWidgets.QDesktopWidget()  # This gets us access to the desktop geometry
+
+        app_window = ApplicationWindow(parent=None, ui_file_path=ui_file_path)  # Make a window in this application
+        app_window.setWindowTitle(title)  # Sets the window title
+
+        app_window.setWindowFlags(app_window.windowFlags() |  # Sets the windows flags to:
+                                  QtCore.Qt.FramelessWindowHint |  # remove the border and frame on the application,
+                                  QtCore.Qt.WindowStaysOnTopHint |  # and makes the window stay on top of all others
+                                  QtCore.Qt.X11BypassWindowManagerHint)  # This is needed to show fullscreen in gnome
+
+        app_window.setGeometry(
+            system_desktop.screenGeometry(display_screen))  # Sets the window to be on the first screen
+
+        app_window.showFullScreen()  # Shows the window in full screen mode
+
+        return app_window
+
+
+#####################################
+# Main Definition
+#####################################
+if __name__ == "__main__":
+    signal.signal(signal.SIGINT, signal.SIG_DFL)  # This allows the keyboard interrupt kill to work properly
+
+    # ########## Start the QApplication Framework ##########
+    application = QtWidgets.QApplication(sys.argv)  # Create the ase qt gui application
+    application.setStyleSheet(qdarkstyle.load_stylesheet_pyqt5())
+
+    # ########## Set Organization Details for QSettings ##########
+    QtCore.QCoreApplication.setOrganizationName("OSURC")
+    QtCore.QCoreApplication.setOrganizationDomain("http://osurobotics.club/")
+    QtCore.QCoreApplication.setApplicationName("groundstation")
+
+    # ########## Check ROS Master Status ##########
+    master_checker = ROSMasterChecker.ROSMasterChecker()
+
+    if not master_checker.master_present(5):
+        message_box = QtWidgets.QMessageBox()
+        message_box.setWindowTitle("Rover Ground Station")
+        message_box.setText("Connection to ROS Master Failed!!!\n" +
+                            "Ensure ROS master is running or check for network issues.")
+        message_box.exec_()
+        exit()
+
+    # ########## Start Ground Station If Ready ##########
+    ground_station = GroundStation()
+    application.exec_()  # Execute launching of the application

software/ros_packages/nimbro_topic_transport/CMakeLists.txt

@@ -0,0 +1,201 @@
+cmake_minimum_required(VERSION 2.8)
+
+project(nimbro_topic_transport)
+
+find_package(catkin REQUIRED COMPONENTS
+	roscpp
+	topic_tools
+	rostest
+	message_generation
+)
+
+add_message_files(FILES
+	CompressedMsg.msg
+	ReceiverStats.msg
+	SenderStats.msg
+	TopicBandwidth.msg
+)
+
+generate_messages(DEPENDENCIES
+	std_msgs
+)
+
+catkin_package()
+include_directories(${catkin_INCLUDE_DIRS})
+
+# Some optional components of the NimbRo software framework
+find_package(plot_msgs)
+if(plot_msgs_FOUND)
+	include_directories(${plot_msgs_INCLUDE_DIRS})
+	add_definitions(-DWITH_PLOTTING=1)
+endif()
+
+find_package(config_server)
+if(config_server_FOUND)
+	include_directories(${config_server_INCLUDE_DIRS})
+	add_definitions(-DWITH_CONFIG_SERVER=1)
+endif()
+
+find_package(catch_ros)
+
+# If OpenFEC is available, we can enable FEC for the UDP sender
+set(DEFAULT_OPENFEC_PATH /opt/openfec_v1.4.2)
+set(OPENFEC_PATH "" CACHE PATH "Path to OpenFEC (optional)")
+if(NOT OPENFEC_PATH AND IS_DIRECTORY ${DEFAULT_OPENFEC_PATH})
+	set(OPENFEC_PATH ${DEFAULT_OPENFEC_PATH})
+endif()
+if(OPENFEC_PATH)
+	include_directories(${OPENFEC_PATH}/src/lib_common)
+	set(OPENFEC_LIBRARY ${OPENFEC_PATH}/bin/Release/libopenfec.so)
+	add_definitions(-DWITH_OPENFEC=1)
+	message(STATUS "Found and using OpenFEC at: ${OPENFEC_PATH}")
+else()
+	set(OPENFEC_LIBRARY "")
+endif()
+
+find_library(BZ2_LIBRARY bz2 REQUIRED)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
+add_executable(udp_sender
+	src/udp/topic_sender.cpp
+	src/udp/udp_sender.cpp
+	src/topic_info.cpp
+)
+add_dependencies(udp_sender
+	${PROJECT_NAME}_generate_messages_cpp
+)
+
+target_link_libraries(udp_sender
+	${catkin_LIBRARIES}
+	${BZ2_LIBRARY}
+	${OPENFEC_LIBRARY}
+	${config_server_LIBRARIES}
+)
+
+add_executable(udp_receiver
+	src/udp/udp_receiver.cpp
+	src/udp/topic_receiver.cpp
+	src/topic_info.cpp
+)
+add_dependencies(udp_receiver
+	${PROJECT_NAME}_generate_messages_cpp
+)
+
+target_link_libraries(udp_receiver
+	${catkin_LIBRARIES}
+	${BZ2_LIBRARY}
+	${OPENFEC_LIBRARY}
+)
+
+add_executable(tcp_sender
+	src/tcp/tcp_sender.cpp
+	src/topic_info.cpp
+)
+add_dependencies(tcp_sender
+	${PROJECT_NAME}_generate_messages_cpp
+)
+target_link_libraries(tcp_sender
+	${catkin_LIBRARIES}
+	${BZ2_LIBRARY}
+	${config_server_LIBRARIES}
+)
+
+add_executable(tcp_receiver
+	src/tcp/tcp_receiver.cpp
+	src/topic_info.cpp
+)
+add_dependencies(tcp_receiver
+	${PROJECT_NAME}_generate_messages_cpp
+)
+target_link_libraries(tcp_receiver
+	${catkin_LIBRARIES}
+	${BZ2_LIBRARY}
+)
+
+# Tools
+add_executable(action_proxy
+	src/action_proxy.cpp
+	src/topic_info.cpp
+)
+target_link_libraries(action_proxy
+	${catkin_LIBRARIES}
+)
+
+# GUI
+find_package(Qt4 REQUIRED)
+include(${QT_USE_FILE})
+
+qt4_wrap_cpp(MOC_SRCS
+	src/gui/topic_gui.h
+	src/gui/dot_widget.h
+)
+
+qt4_wrap_cpp(BAND_MOC_SRCS
+	src/gui/bandwidth_gui.h
+	src/gui/contrib/qcustomplot/qcustomplot.h
+)
+
+add_library(topic_gui
+	${MOC_SRCS}
+	src/gui/topic_gui.cpp
+	src/gui/dot_widget.cpp
+)
+
+add_library(bandwidth_gui
+	${BAND_MOC_SRCS}
+	src/gui/bandwidth_gui.cpp
+	src/gui/contrib/qcustomplot/qcustomplot.cpp
+)
+
+
+add_dependencies(topic_gui
+	${PROJECT_NAME}_generate_messages_cpp
+)
+
+target_link_libraries(topic_gui
+	${QT_LIBRARIES}
+)
+
+add_dependencies(bandwidth_gui
+	${PROJECT_NAME}_generate_messages_cpp
+)
+
+target_link_libraries(bandwidth_gui
+	${QT_LIBRARIES}
+	yaml-cpp
+)
+
+# Tests
+if(catch_ros_FOUND)
+	include_directories(${catch_ros_INCLUDE_DIRS})
+
+	catch_add_rostest_node(test_comm
+		test/test_comm.cpp
+	)
+	target_link_libraries(test_comm
+		${catkin_LIBRARIES}
+	)
+
+	add_rostest(test/topic_transport.test ARGS protocol:=udp)
+	add_rostest(test/topic_transport.test ARGS protocol:=tcp)
+
+	if(OPENFEC_PATH)
+		add_rostest(test/topic_transport.test ARGS protocol:=udp fec:=true)
+	endif()
+
+	catch_add_rostest_node(test_bidirectional
+		test/test_bidirectional.cpp
+	)
+	target_link_libraries(test_bidirectional
+		${catkin_LIBRARIES}
+	)
+
+	add_rostest(test/bidirectional.test ARGS allow_bidirectional:=true)
+	add_rostest(test/bidirectional.test ARGS allow_bidirectional:=false)
+endif()
+
+#install
+install(TARGETS udp_receiver tcp_receiver udp_sender tcp_sender
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

software/ros_packages/nimbro_topic_transport/README.md

@@ -0,0 +1,71 @@
+
+nimbro_topic_transport
+======================
+
+This package includes nodes for transmitting ROS topic messages over a network
+connection. For an overview over the available parameters, see
+`doc/configuration.md`.
+
+For a quick getting-started guide see `doc/getting_started.md`.
+
+The remainder of this document introduces the features of the topic transport
+and explains the choices you have in detail.
+
+The fundamental choice you have is whether you want to use the TCP or the UDP
+protocol.
+
+UDP
+---
+
+The UDP protocol is the right choice for most ROS topics. It is especially
+suited for lossy and/or delayed connections, where TCP has problems. Note
+however, that with our UDP protocol there is no guarantee that a message
+arrives.
+
+Typical message types transmitted using the UDP protocol are camera images,
+joystick commands, TF snapshots (see tf_throttle). In short, everything where
+retransmitting missed packets does not make sense, because you have newer data
+available already.
+
+For example launch files, see the launch directory.
+
+TCP
+---
+
+The TCP protocol is more useful for topics where you need a transmission
+guarantee. For example, it makes sense to transmit 3D laser scans via this
+method, because re-transmission of dropped packets is still faster than
+waiting for the next scan.
+
+For example launch files, see the launch directory.
+
+Visualization & diagnosis
+-------------------------
+
+The nimbro_topic_transport package provides GUI plugins for the `rqt` GUI:
+
+ - "nimbro_network/Topic Transport" shows a dot graph of all network connections
+ - "nimbro_network/Topic Bandwidth" shows detailed bandwidth usage for a
+   single connection.
+
+Note that it may be necessary to transmit the topics `/network/sender_stats` and
+`/network/receiver_stats` over the network to get the full amount of
+information.
+
+Relay mode
+----------
+
+The UDP transport supports a special "relay" mode that was used in the DARPA
+Robotics Challenge. Basically, it saturates a given target bitrate, repeating
+messages if necessary. In the DRC, the high bandwidth link was switched on for
+very short times (1 second). By saturating the available bandwidth, we made
+sure that we got the maximum amount of data across in that window.
+
+Forward error correction (FEC)
+------------------------------
+
+The UDP transport can do forward error correction if [OpenFEC][1] is available.
+A short guide on how to install and configure the system is available
+in `doc/FEC.md`.
+
+[1]: http://openfec.org/

software/ros_packages/nimbro_topic_transport/bandwidth_plugin.xml

@@ -0,0 +1,15 @@
+<library path="lib/libbandwidth_gui">
+	<class name="BandwidthGui" type="nimbro_topic_transport::BandwidthGui" base_class_type="rqt_gui_cpp::Plugin">
+		<description>
+			Display bandwidth information for transferred topics
+		</description>
+		<qtgui>
+			<group>
+				<label>nimbro_network</label>
+			</group>
+
+			<label>Topic Bandwidth</label>
+			<statustip>Diagnostic information</statustip>
+		</qtgui>
+	</class>
+</library>

software/ros_packages/nimbro_topic_transport/doc/FEC.md

@@ -0,0 +1,18 @@
+Forward Error Correction
+========================
+
+This is a short guide on how to get FEC running. If you want to use FEC,
+you have to download [OpenFEC][OpenFEC] and compile it. Assuming you already
+have compiled `nimbro_topic_transport` at least once, you can then inform
+it about your OpenFEC installation:
+
+```
+cd my_catkin_workspace/build/nimbro_topic_transport
+cmake -DOPENFEC_PATH=/path/to/openfec .
+make
+```
+
+Afterwards, you can use the `fec` parameter on sender & receiver side as
+described in `configuration.md`.
+
+[OpenFEC]: http://openfec.org/

software/ros_packages/nimbro_topic_transport/doc/configuration.md

@@ -0,0 +1,74 @@
+
+Configuration
+=============
+
+`nimbro_topic_transport` is configured through ROS parameters.
+
+Receiver parameters
+-------------------
+
+`tcp_receiver` and `udp_receiver` accept the following parameters:
+
+Required:
+ - `port` (int): UDP/TCP port to bind to (required)
+
+Optional:
+ - `drop_repeated_msgs` (bool): If a message with the same sequence number
+   arrives twice, drop it. Needed in conjunction with the relay mode.
+   (UDP only, default true)
+ - `fec` (bool): Enable Forward Error correction (UDP only, default false)
+ - `keep_compressed` (bool): Do not uncompress compressed topics, instead
+   publish them as the type `nimbro_topic_transport/CompressedMsg`
+   (default false)
+ - `label` (string): Display a label in the visualization GUIs
+ - `topic_prefix` (string): prepend topic_prefix before advertised topic names
+ - `warn_drop_incomplete` (bool): Display a warning every time an incomplete
+   packet is dropped (UDP only, default true)
+
+Sender parameters
+-----------------
+
+`tcp_sender` and `udp_sender` accept the following parameters:
+
+Required:
+ - `destination_addr` (string): Hostname or IP address of the destination
+   machine (required)
+ - `destination_port` (int): Port number to connect to (required)
+ - `source_port` (int): Source port to bind to. If not specified, the port is
+   chosen by the OS (TODO: true for udp_sender!)
+ - `topics` (list): List of topics to be transmitted (see below)
+
+Optional:
+ - `fec` (float): If non-zero, this is the proportion of repair packets sent for
+   Forward Error Correction (0.5 -> Send 50% more data). This needs support for
+   FEC compiled in, see README.md (default 0.0)
+ - `label` (string): Display a label in the visualization GUIs
+ - `relay_mode` (bool): Enable relay mode, see README.md
+   (UDP only, default false)
+ - `relay_target_bitrate` (float): Target bitrate for relay mode (UDP only)
+ - `relay_control_rate` (float): Check if new packets can be sent in relay mode
+   at this rate (UDP only)
+ - `ignored_publishers` (list of string): Names of nodes whose messages should be 
+   ignored if received by this sender. This should be used on both senders when 
+   messages are to be sent to a topic both ways (always specify the name of the
+   receiver belonging to the other sender). See `launch/bidirectional_topics.launch`
+   for an example setup (TCP only)
+
+Topic configuration
+-------------------
+
+Configuration of topics to be transmitted is done on the parameter server of
+the sender's side. See the example launch files for the usual setup.
+
+Here is a list of parameters that are available per topic. The only mandatory
+parameter is `name`.
+
+ - `name`: Name of the topic to be sent over.
+ - `rate`: Rate limit on messages / sec (floating point). Messages over the
+   rate limit are silently dropped on the sender side. The default is 0.0
+   (no rate limit).
+   Note: Limiting only works well for lower rates (<20 Hz).
+   (UDP only)
+ - `resend`: If the sender does not get a message 1.0/`rate` after the last one,
+   it will re-send the last received one. (UDP only)
+ - `compress`: If true, compress the data on the wire with bz2.

software/ros_packages/nimbro_topic_transport/doc/getting_started.md

@@ -0,0 +1,26 @@
+
+Getting started
+===============
+
+We assume that we have two machines, machineA and machineB. Both will be running
+roscores, and we will transport some topics from machineA to machineB.
+
+On machineA:
+
+```bash
+roslaunch nimbro_topic_transport udp_sender.launch target:=machineB
+rostopic pub -r 1.0 /my_first_topic std_msgs/String "Hello World"
+```
+
+Instead of using the host name `machineB`, you can also use an IP address.
+
+On machineB:
+
+```bash
+roslaunch nimbro_topic_transport udp_receiver.launch
+rostopic echo /my_first_topic
+```
+
+You should see the `Hello World` messages arriving.
+
+For customization, you should copy the launch files into your own package.

software/ros_packages/nimbro_topic_transport/launch/bidirectional_machine1.launch

@@ -0,0 +1,30 @@
+<launch>
+
+    <arg name="target" default="localhost" />
+    <arg name="allow_bidirectional" default="true" />
+
+    <!-- The TCP sender node -->
+    <node name="tcp_sender_machine1" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
+
+        <!-- The destination host name or IP address -->
+        <param name="destination_addr" value="$(arg target)" />
+        <param name="destination_port" value="17001" />
+
+        <!-- Load the list of topics from a YAML file -->
+        <rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics_machine1.yaml" />
+
+        <!-- If bidirectional traffic over a topic is expected, fill this parameter.
+             See details in bidirectional_topics.launch -->
+        <rosparam param="ignored_publishers" if="$(arg allow_bidirectional)">["/tcp_receiver_machine1"]</rosparam>
+        <rosparam param="ignored_publishers" unless="$(arg allow_bidirectional)">[]</rosparam>
+    </node>
+
+    <node name="tcp_receiver_machine1" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
+        <param name="port" value="17002" />
+
+        <!-- Remap topics so that sender & receiver do not clash if run on the
+          same machine. This is not necessary in a typical setup :-)
+         -->
+        <remap from="/recv/my_first_topic" to="/my_first_topic" />
+    </node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/bidirectional_machine2.launch

@@ -0,0 +1,29 @@
+<launch>
+    <arg name="target" default="localhost" />
+    <arg name="allow_bidirectional" default="true" />
+
+    <!-- The TCP sender node -->
+    <node name="tcp_sender_machine2" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
+
+        <!-- The destination host name or IP address -->
+        <param name="destination_addr" value="$(arg target)" />
+        <param name="destination_port" value="17002" />
+
+        <!-- Load the list of topics from a YAML file -->
+        <rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics_machine2.yaml" />
+
+        <!-- If bidirectional traffic over a topic is expected, fill this parameter.
+             See details in bidirectional_topics.launch -->
+        <rosparam param="ignored_publishers" if="$(arg allow_bidirectional)">["/tcp_receiver_machine2"]</rosparam>
+        <rosparam param="ignored_publishers" unless="$(arg allow_bidirectional)">[]</rosparam>
+    </node>
+
+    <node name="tcp_receiver_machine2" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
+        <param name="port" value="17001" />
+
+        <!-- Remap topics so that sender & receiver do not clash if run on the
+          same machine. This is not necessary in a typical setup :-)
+         -->
+        <remap from="/my_first_topic" to="/recv/my_first_topic" />
+    </node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/bidirectional_topics.launch

@@ -0,0 +1,22 @@
+<launch>
+    <arg name="allow_bidirectional" default="true" />
+
+    <!--
+    If you set up transports for a topic in both directions, don't forget to
+    provide the `ignored_publishers` parameter to both senders. Otherwise,
+    nimbro_network would enter an infinite republishing loop. You can test it
+    by playing with the `allow_bidirectional` argument and publishing to one
+    of the topics.
+
+    The `ignored_publishers` sender parameter is a list of node-names of
+    all receivers receiving any of the topics this publisher publishes.
+    -->
+
+    <include file="$(find nimbro_topic_transport)/launch/bidirectional_machine1.launch">
+        <arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
+    </include>
+
+    <include file="$(find nimbro_topic_transport)/launch/bidirectional_machine2.launch">
+        <arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
+    </include>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/camera_topics.yaml

@@ -0,0 +1,4 @@
+topics:
+ - name: "/cameras/camera_undercarriage/image_1280x720/compressed"
+   compress: false # enable bz2 compression
+   rate: 30.0 # rate limit at 1Hz

software/ros_packages/nimbro_topic_transport/launch/camera_topics2.yaml

@@ -0,0 +1,10 @@
+topics:
+ - name: "/cameras/camera_chassis/image_640x360/compressed"
+   compress: false # enable bz2 compression
+   rate: 30.0 # rate limit at 1Hz
+ - name: "/cameras/camera_undercarriage/image_640x360/compressed"
+   compress: false # enable bz2 compression
+   rate: 30.0 # rate limit at 1Hz
+ - name: "/cameras/main_navigation/image_640x360/compressed"
+   compress: false # enable bz2 compression
+   rate: 30.0 # rate limit at 1Hz

software/ros_packages/nimbro_topic_transport/launch/camera_topics3.yaml

@@ -0,0 +1,4 @@
+topics:
+ - name: "/cameras/main_navigation/image_1280x720/compressed"
+   compress: false # enable bz2 compression
+   rate: 30.0 # rate limit at 1Hz

software/ros_packages/nimbro_topic_transport/launch/tcp_receiver.launch

@@ -0,0 +1,11 @@
+<launch>
+	<node name="tcp_receiver" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
+		<param name="port" value="17001" />
+
+		<!-- Remap topics so that sender & receiver do not clash if run on the
+		  same machine. This is not necessary in a typical setup :-)
+		 -->
+		<remap from="/my_first_topic" to="/recv/my_first_topic" />
+		<remap from="/my_second_topic" to="/recv/my_second_topic" />
+	</node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/tcp_sender.launch

@@ -0,0 +1,14 @@
+<launch>
+	<arg name="target" default="localhost" />
+
+	<!-- The UDP sender node -->
+	<node name="tcp_sender" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17001" />
+
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
+	</node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/topics.yaml

@@ -0,0 +1,5 @@
+topics:
+ - name: "/drive/motoroneandtwo"
+   compress: true # enable bz2 compression
+   rate: 20.0 # rate limit at 1Hz
+ - name: "/my_second_topic"

software/ros_packages/nimbro_topic_transport/launch/topics_machine1.yaml

@@ -0,0 +1,2 @@
+topics:
+ - name: "/my_first_topic"

software/ros_packages/nimbro_topic_transport/launch/topics_machine2.yaml

@@ -0,0 +1,2 @@
+topics:
+ - name: "/recv/my_first_topic"

software/ros_packages/nimbro_topic_transport/launch/udp_camera_receiver.launch

@@ -0,0 +1,37 @@
+<launch>
+    <!--<node name="udp_receiver" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">-->
+        <!--<param name="port" value="17001" />-->
+    <!--</node>-->
+
+    <!--<node name="udp_receiver2" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">-->
+        <!--<param name="port" value="17002" />-->
+    <!--</node>-->
+
+    <!--<node name="udp_receiver3" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">-->
+        <!--<param name="port" value="17003" />-->
+    <!--</node>-->
+
+    <node name="udp_receiver4" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17004" />
+    </node>
+
+    <node name="udp_receiver5" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17005" />
+    </node>
+
+    <node name="udp_receiver6" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17006" />
+    </node>
+
+    <node name="udp_receiver7" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17007" />
+    </node>
+
+    <node name="udp_receiver8" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17008" />
+    </node>
+
+    <node name="udp_receiver9" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17009" />
+    </node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/udp_camera_sender.launch

@@ -0,0 +1,81 @@
+<launch>
+	<arg name="target" default="192.168.1.15" />
+
+    	<!-- The UDP sender node -->
+	<node name="udp_sender4" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17004" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/cameras/camera_chassis/image_640x360/compressed", compress: false, rate: 30.0}]
+        </rosparam>
+
+	</node>
+
+    	<!-- The UDP sender node -->
+	<node name="udp_sender5" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17005" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/cameras/camera_undercarriage/image_640x360/compressed", compress: false, rate: 30.0}]
+        </rosparam>
+
+	</node>
+
+    	<!-- The UDP sender node -->
+	<node name="udp_sender6" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17006" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/cameras/main_navigation/image_640x360/compressed", compress: false, rate: 30.0}]
+        </rosparam>
+
+	</node>
+
+    	<!-- The UDP sender node -->
+	<node name="udp_sender7" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17007" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/cameras/camera_chassis/image_256x144/compressed", compress: false, rate: 30.0}]
+        </rosparam>
+
+	</node>
+
+    	<!-- The UDP sender node -->
+	<node name="udp_sender8" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17008" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/cameras/camera_undercarriage/image_256x144/compressed", compress: false, rate: 30.0}]
+        </rosparam>
+
+	</node>
+
+    	<!-- The UDP sender node -->
+	<node name="udp_sender9" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17009" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 30.0}]
+        </rosparam>
+
+	</node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/udp_drive_command_receiver.launch

@@ -0,0 +1,5 @@
+<launch>
+    <node name="udp_receiver_drive" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+        <param name="port" value="17020" />
+    </node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/udp_drive_command_sender.launch

@@ -0,0 +1,17 @@
+<launch>
+	<arg name="target" default="192.168.1.10" />
+
+   	<!-- The UDP sender node -->
+	<node name="udp_sender4" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17020" />
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam param="topics">
+            [{name: "/command_control/groundstation_drive", compress: true, rate: 15.0}]
+        </rosparam>
+
+	</node>
+
+</launch>

software/ros_packages/nimbro_topic_transport/launch/udp_receiver.launch

@@ -0,0 +1,19 @@
+<launch>
+	<!--
+	  This launch file runs a udp_receiver node, which receives topics
+	  over the network on port 17001 and publishes them on the local roscore.
+
+	  See udp_sender.launch for the sender part.
+	-->
+
+	<node name="udp_receiver" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
+		<!-- The port to receive packets on -->
+		<param name="port" value="17001" />
+
+		<!-- Remap topics so that sender & receiver do not clash if run on the
+		  same machine. This is not necessary in a typical setup :-)
+		 -->
+		<remap from="/my_first_topic" to="/recv/my_first_topic" />
+		<remap from="/my_second_topic" to="/recv/my_second_topic" />
+	</node>
+</launch>

software/ros_packages/nimbro_topic_transport/launch/udp_sender.launch

@@ -0,0 +1,26 @@
+<launch>
+	<!--
+	  This launch file runs a udp_sender node, which sends topics from the local
+	  roscore over the network on port 17001.
+
+	  By default, this launch file sends topics to your local machine for
+	  testing purposes. If you want to send to another machine, use
+	    roslaunch nimbro_topic_transport udp_sender.launch target:=other_host
+	  where other_host can be a host name or IP address.
+
+	  See udp_receiver.launch for the receiving part.
+	-->
+
+	<arg name="target" default="localhost" />
+
+	<!-- The UDP sender node -->
+	<node name="udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17001" />
+
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
+	</node>
+</launch>

software/ros_packages/nimbro_topic_transport/msg/CompressedMsg.msg

@@ -0,0 +1,8 @@
+
+# Topic type (e.g. 'std_msgs/String')
+string type
+
+# Topic md5 sum
+uint32[4] md5
+
+uint8[] data

software/ros_packages/nimbro_topic_transport/msg/ReceiverStats.msg

@@ -0,0 +1,31 @@
+Header header
+
+# Node name
+string node
+
+# Protocol ("TCP" or "UDP")
+string protocol
+
+# Connection label
+string label
+
+# Local hostname
+string host
+
+# Remote hostname (if resolvable, otherwise IP)
+string remote
+
+# Port
+uint16 local_port
+
+# Port
+uint16 remote_port
+
+# is Forward Error Correction enabled?
+bool fec
+
+# Bandwidth in bit/s
+float32 bandwidth
+
+# Drop rate (estimated, percentage of missing packets in a fixed time interval)
+float32 drop_rate

software/ros_packages/nimbro_topic_transport/msg/SenderStats.msg

@@ -0,0 +1,31 @@
+Header header
+
+# Node name
+string node
+
+# Protocol ("TCP" or "UDP")
+string protocol
+
+# Connection label
+string label
+
+# Local hostname
+string host
+
+# Destination address
+string destination
+
+# Destination port
+uint16 destination_port
+
+# Source port
+uint16 source_port
+
+# Is Forward Error Correction enabled?
+bool fec
+
+# Bandwidth in bit/s
+float32 bandwidth
+
+# Topic specific bandwidth
+TopicBandwidth[] topics

software/ros_packages/nimbro_topic_transport/msg/TopicBandwidth.msg

@@ -0,0 +1,4 @@
+# Topic name
+string name
+# Bandwidth
+float32 bandwidth

software/ros_packages/nimbro_topic_transport/my_launch/udp_sender.launch

@@ -0,0 +1,26 @@
+<launch>
+	<!--
+	  This launch file runs a udp_sender node, which sends topics from the local
+	  roscore over the network on port 17001.
+
+	  By default, this launch file sends topics to your local machine for
+	  testing purposes. If you want to send to another machine, use
+	    roslaunch nimbro_topic_transport udp_sender.launch target:=other_host
+	  where other_host can be a host name or IP address.
+
+	  See udp_receiver.launch for the receiving part.
+	-->
+
+	<arg name="target" default="192.168.1.10" />
+
+	<!-- The UDP sender node -->
+	<node name="udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17001" />
+
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
+	</node>
+</launch>

software/ros_packages/nimbro_topic_transport/package.xml

@@ -0,0 +1,32 @@
+<package format="2">
+	<name>nimbro_topic_transport</name>
+	<version>1.0.0</version>
+	<description>Contains nodes for TCP/UDP communication between ROS masters</description>
+
+	<maintainer email="max.schwarz@uni-bonn.de">Max Schwarz</maintainer>
+
+	<license>GPLv2</license>
+
+	<buildtool_depend>catkin</buildtool_depend>
+	<depend>roscpp</depend>
+	<depend>topic_tools</depend>
+
+	<build_depend>message_generation</build_depend>
+
+	<!-- This is not a hard dependency, the package compiles without plot_msgs. -->
+	<depend>plot_msgs</depend>
+
+	<!-- This is not a hard dependency, the package compiles without config_server. -->
+	<depend>config_server</depend>
+
+	<!-- This is not a hard dependency, the package compiles without catch_ros -->
+	<depend>catch_ros</depend>
+
+	<depend>rqt_gui</depend>
+
+	<export>
+		<rqt_gui plugin="${prefix}/rqt_plugin.xml" />
+		<rqt_gui plugin="${prefix}/bandwidth_plugin.xml" />
+	</export>
+
+</package>

software/ros_packages/nimbro_topic_transport/rqt_plugin.xml

@@ -0,0 +1,15 @@
+<library path="lib/libtopic_gui">
+	<class name="topic_gui" type="nimbro_topic_transport::TopicGUI" base_class_type="rqt_gui_cpp::Plugin">
+		<description>
+			Display diagnostic information about the topic transport
+		</description>
+		<qtgui>
+			<group>
+				<label>nimbro_network</label>
+			</group>
+
+			<label>Topic Transport</label>
+			<statustip>Diagnostic information</statustip>
+		</qtgui>
+	</class>
+</library>

software/ros_packages/nimbro_topic_transport/src/action_proxy.cpp

@@ -0,0 +1,138 @@
+// Proxy node for action topics
+// actionlib expects that all action topics are handled by a single node
+// -- so republish / resubscribe them from here.
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include <actionlib_msgs/GoalID.h>
+#include <actionlib_msgs/GoalStatusArray.h>
+
+#include <topic_tools/shape_shifter.h>
+
+#include "topic_info.h"
+
+ros::Subscriber m_sub_goal;
+ros::Publisher m_pub_goal;
+
+void handleGoal(const topic_tools::ShapeShifter::ConstPtr& goal)
+{
+	m_pub_goal.publish(goal);
+}
+
+ros::Subscriber m_sub_cancel;
+ros::Publisher m_pub_cancel;
+
+void handleCancel(const actionlib_msgs::GoalID& id)
+{
+	m_pub_cancel.publish(id);
+}
+
+ros::Subscriber m_sub_result;
+ros::Publisher m_pub_result;
+
+void handleResult(const topic_tools::ShapeShifter::ConstPtr& result)
+{
+	m_pub_result.publish(result);
+}
+
+ros::Subscriber m_sub_status;
+ros::Publisher m_pub_status;
+
+void handleStatus(const actionlib_msgs::GoalStatusArray& status)
+{
+	m_pub_status.publish(status);
+}
+
+ros::Subscriber m_sub_feedback;
+ros::Publisher m_pub_feedback;
+
+void handleFeedback(const topic_tools::ShapeShifter::ConstPtr& fb)
+{
+	m_pub_feedback.publish(fb);
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "action_proxy");
+
+	ros::NodeHandle nh("~");
+
+	std::string type;
+	if(!nh.getParam("type", type))
+	{
+		ROS_FATAL("need type parameter");
+		return 1;
+	}
+
+	std::string input;
+	if(!nh.getParam("input", input))
+	{
+		ROS_FATAL("need input parameter");
+		return 1;
+	}
+
+	std::string output;
+	if(!nh.getParam("output", output))
+	{
+		ROS_FATAL("need output parameter");
+		return 1;
+	}
+
+	{
+		ros::AdvertiseOptions ops;
+		ops.topic = input + "/goal";
+		ops.datatype = type + "ActionGoal";
+		ops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(ops.datatype);
+
+		m_pub_goal = nh.advertise(ops);
+
+		ros::SubscribeOptions subops;
+		subops.init<topic_tools::ShapeShifter>(output + "/goal", 10, boost::bind(&handleGoal, _1));
+		subops.datatype = type + "ActionGoal";
+		subops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(subops.datatype);
+		m_sub_goal = nh.subscribe(subops);
+	}
+
+	{
+		m_pub_cancel = nh.advertise<actionlib_msgs::GoalID>(input + "/cancel", 1);
+		m_sub_cancel = nh.subscribe(output + "/cancel", 10, &handleCancel);
+	}
+
+	{
+		ros::AdvertiseOptions ops;
+		ops.topic = output + "/result";
+		ops.datatype = type + "ActionResult";
+		ops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(ops.datatype);
+
+		m_pub_result = nh.advertise(ops);
+
+		ros::SubscribeOptions subops;
+		subops.init<topic_tools::ShapeShifter>(input + "/result", 10, boost::bind(&handleResult, _1));
+		subops.datatype = type + "ActionResult";
+		subops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(subops.datatype);
+		m_sub_result = nh.subscribe(subops);
+	}
+
+	{
+		ros::AdvertiseOptions ops;
+		ops.topic = output + "/feedback";
+		ops.datatype = type + "ActionFeedback";
+		ops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(ops.datatype);
+
+		m_pub_feedback = nh.advertise(ops);
+
+		ros::SubscribeOptions subops;
+		subops.init<topic_tools::ShapeShifter>(input + "/feedback", 10, boost::bind(&handleFeedback, _1));
+		subops.datatype = type + "ActionFeedback";
+		subops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(subops.datatype);
+		m_sub_feedback = nh.subscribe(subops);
+	}
+
+	{
+		m_pub_status = nh.advertise<actionlib_msgs::GoalStatusArray>(output + "/status", 10);
+		m_sub_status = nh.subscribe(input + "/status", 10, &handleStatus);
+	}
+
+	ros::spin();
+
+	return 0;
+}

software/ros_packages/nimbro_topic_transport/src/gui/bandwidth_gui.cpp

@@ -0,0 +1,353 @@
+// Bandwidth display for transferred topics
+// Author: Sebastian Schüller
+
+#include "bandwidth_gui.h"
+#include "contrib/qcustomplot/qcustomplot.h"
+
+#include <pluginlib/class_list_macros.h>
+#include <ros/node_handle.h>
+
+#include <yaml-cpp/yaml.h>
+
+#include <algorithm>
+
+#include <QLayout>
+#include <QComboBox>
+#include <QPushButton>
+#include <QTextEdit>
+#include <QMessageBox>
+
+Q_DECLARE_METATYPE(nimbro_topic_transport::SenderStatsConstPtr)
+
+static const int WINDOW_SIZE = 64;
+static const int BRUSH_VALUE = 180;
+static const int BRUSH_SATURATION = 150;
+static const std::string DEFAULT_GROUPS =
+	"cameras: [\"/camera_center/h264\", \"/camera_left/h264\", \"/camera_right/h264\"]\n"
+	"hand_cameras: [\"/camera_left_hand/h264\", \"/camera_right_hand/h264\"]\n"
+	"network_stats: [\"/network/sender_stats\", \"/network/receiver_stats\"]\n";
+
+inline double bpsToKbps(double bps)
+{
+	return bps/1000.0;
+}
+
+inline double bpsToMbps(double bps)
+{
+	return bpsToKbps(bps)/1000.0;
+}
+
+namespace nimbro_topic_transport
+{
+BandwidthGui::BandwidthGui()
+ : m_maxBandwidth(0)
+ , m_hue(175)
+{}
+
+BandwidthGui::~BandwidthGui()
+{}
+
+void BandwidthGui::initPlugin(qt_gui_cpp::PluginContext& ctx)
+{
+	m_plot = new QCustomPlot();
+	m_plot->legend->setVisible(true);
+	m_plot->legend->setIconBorderPen(Qt::NoPen);
+	m_plot->yAxis->setLabel("Kb/s");
+
+
+	QWidget* wrapper = new QWidget();
+	m_connectionBox = new QComboBox(wrapper);
+	QPushButton* groupBtn = new QPushButton("Group Settings", wrapper);
+	QGridLayout* gl = new QGridLayout(wrapper);
+	gl->addWidget(m_plot, 0, 0, 1, 2);
+	gl->addWidget(m_connectionBox, 1, 0);
+	gl->addWidget(groupBtn, 1, 1);
+	wrapper->setLayout(gl);
+	wrapper->setWindowTitle("Bandwidth");
+	ctx.addWidget(wrapper);
+
+	connect(
+		m_plot, SIGNAL(legendClick(QCPLegend *, QCPAbstractLegendItem *, QMouseEvent *)),
+		this, SLOT(handleClickedLegend(QCPLegend*, QCPAbstractLegendItem*, QMouseEvent*))
+	);
+
+	connect(
+		m_connectionBox, SIGNAL(currentIndexChanged(int)),
+		this, SLOT(clearPlot())
+	);
+
+	connect(
+		groupBtn, SIGNAL(pressed()),
+		this, SLOT(updateGroupInfo())
+	);
+
+	qRegisterMetaType<nimbro_topic_transport::SenderStatsConstPtr>();
+
+	connect(
+		this, SIGNAL(senderStatsReceived(nimbro_topic_transport::SenderStatsConstPtr)),
+		this, SLOT(handleSenderStats(nimbro_topic_transport::SenderStatsConstPtr)),
+		Qt::QueuedConnection
+	);
+
+	m_sub_senderStats = getPrivateNodeHandle().subscribe(
+		"/network/sender_stats", 1, &BandwidthGui::senderStatsReceived, this
+	);
+	m_plot->xAxis->setTickLabelType(QCPAxis::ltDateTime);
+	m_plot->xAxis->setDateTimeFormat("hh:mm:ss");
+	m_plot->xAxis->setAutoTickStep(true);
+	m_plot->yAxis->setRangeLower(0);
+	QCPLayoutGrid* subLayout = new QCPLayoutGrid();
+	QCPLayoutElement* dummy = new QCPLayoutElement();
+	m_plot->plotLayout()->addElement(0, 1, subLayout);
+	subLayout->addElement(0, 0, m_plot->legend);
+	subLayout->addElement(1, 0, dummy);
+	subLayout->setRowStretchFactor(0, 0.01);
+	m_plot->plotLayout()->setColumnStretchFactor(1, 0.01);
+
+	connect(
+		m_plot->xAxis, SIGNAL(rangeChanged(QCPRange)),
+		m_plot->xAxis2, SLOT(setRange(QCPRange))
+	);
+	connect(
+		m_plot->yAxis, SIGNAL(rangeChanged(QCPRange)),
+		m_plot->yAxis2, SLOT(setRange(QCPRange))
+	);
+
+	connect(
+		&m_plotTimer, SIGNAL(timeout()),
+		this, SLOT(updatePlot())
+	);
+	m_plotTimer.start(0);
+
+}
+
+void BandwidthGui::shutdownPlugin()
+{
+	m_sub_senderStats.shutdown();
+}
+
+void BandwidthGui::clearPlot()
+{
+	m_maxBandwidth = 0;
+	m_bandwidths.clear();
+	m_plot->clearGraphs();
+}
+
+void BandwidthGui::handleSenderStats(const nimbro_topic_transport::SenderStatsConstPtr& msg)
+{
+	// Update connection info
+	std::stringstream ss;
+	ss << "[" << msg->protocol << "] ";
+	ss << msg->host << ":" << msg->source_port << " -> ";
+	ss << msg->destination << ":" << msg->destination_port;
+	QString connection = QString::fromStdString(ss.str());
+	if(m_connectionBox->findText(connection) == -1)
+		m_connectionBox->addItem(connection);
+
+	if(connection != m_connectionBox->currentText())
+		return;
+
+	for(auto& gv : m_bandwidths)
+		gv.second.value = 0.0;
+
+	for(const auto& topic : msg->topics)
+	{
+		//Look up if topic is in group
+		std::string name = topic.name;
+		for(const auto& group : m_groupMap)
+		{
+			const auto& members = group.second;
+			if(std::find(members.begin(), members.end(), name) != members.end())
+			{
+				name = group.first;
+				break;
+			}
+		}
+
+		if(m_bandwidths.find(name) == m_bandwidths.end())
+		{
+			// Add and configure new graph to plot
+			auto graph = m_plot->addGraph();
+			graph->setName(QString::fromStdString(name));
+			graph->setPen(QPen(QColor::fromHsv(0,110,150)));
+			graph->setBrush(QBrush(QColor::fromHsv(m_hue, BRUSH_SATURATION, BRUSH_VALUE)));
+			m_hue = (m_hue + 137) % 360;
+			m_bandwidths[name].graph = graph;
+			m_bandwidths[name].last_timestamp = std::numeric_limits<double>::max();
+		}
+		m_bandwidths[name].value += bpsToKbps(topic.bandwidth);
+		m_bandwidths[name].timestamp = msg->header.stamp.toSec();
+	}
+}
+
+void BandwidthGui::updateGroupInfo()
+{
+	QString temp;
+	GroupDialog dial; //ToDo find correct parent
+	dial.setText(m_grpYamlString);
+
+	int result = dial.exec();
+	if (result != QDialog::Accepted)
+		return;
+	temp = dial.text();
+	if (m_grpYamlString == temp)
+		return;
+
+	GroupMap groupMap;
+	if(!groupFromYaml(temp.toStdString(), &groupMap))
+	{
+		QMessageBox::warning(m_plot->parentWidget(), "Warning!", "Malformed YAML");
+		return;
+	}
+	m_grpYamlString = temp;
+	clearPlot();
+	m_groupMap = groupMap;
+
+	return;
+}
+
+bool BandwidthGui::groupFromYaml(const std::string& yaml, GroupMap* groupMap)
+{
+	YAML::Node grpInfo = YAML::Load(yaml);
+	if (grpInfo.IsNull())
+	{
+		return true;
+	}
+	if (grpInfo.Type() != YAML::NodeType::Map)
+	{
+		return false;
+	}
+	for (const auto& pair : grpInfo)
+	{
+		if(pair.first.Type() != YAML::NodeType::Scalar)
+		{
+			return false;
+		}
+		std::string key = pair.first.as<std::string>();
+		for (const auto& element : pair.second)
+		{
+			if(element.Type() != YAML::NodeType::Scalar)
+			{
+				return false;
+			}
+			(*groupMap)[key].push_back(element.as<std::string>());
+		}
+	}
+	return true;
+}
+
+
+
+void BandwidthGui::updatePlot()
+{
+	double plotValue = 0;
+
+	QCPGraph* prevGraph = nullptr;
+
+	for(auto& bw : m_bandwidths)
+	{
+		auto& gv = bw.second;
+		plotValue += gv.value;
+
+		if(prevGraph)
+			gv.graph->setChannelFillGraph(prevGraph);
+		prevGraph = gv.graph;
+
+		gv.graph->addData(gv.timestamp, plotValue);
+		m_maxBandwidth = std::max(m_maxBandwidth, plotValue);
+		m_plot->yAxis->setRangeUpper(m_maxBandwidth + (m_maxBandwidth/50.0));
+		gv.graph->removeDataBefore(5*WINDOW_SIZE);
+		gv.last_timestamp = gv.timestamp;
+	}
+	m_plot->xAxis->setRange(ros::Time::now().toSec() + 0.25, WINDOW_SIZE, Qt::AlignRight);
+	m_plot->replot();
+}
+
+void BandwidthGui::handleClickedLegend(QCPLegend *legend, QCPAbstractLegendItem *item, QMouseEvent *event)
+{
+	QCPPlottableLegendItem* graphItem = dynamic_cast<QCPPlottableLegendItem*>(item);
+	if(!graphItem)
+		return;
+
+	auto color = graphItem->plottable()->brush().color();
+	auto hue = color.hue() + 180 % 360;
+	auto sat = color.saturation();
+	auto val = color.value();
+
+	if(sat == BRUSH_SATURATION)
+	{
+		sat = 255;
+		val = 255;
+	}
+	else
+	{
+		sat = BRUSH_SATURATION;
+		val = BRUSH_VALUE;
+	}
+	color.setHsv(hue, sat, val);
+	graphItem->plottable()->setBrush(QBrush(color));
+}
+
+void BandwidthGui::saveSettings(qt_gui_cpp::Settings& pluginSettings, qt_gui_cpp::Settings& instanceSettings) const
+{
+	instanceSettings.setValue("connection", m_connectionBox->currentText());
+	instanceSettings.setValue("groups", m_grpYamlString);
+}
+
+void BandwidthGui::restoreSettings(const qt_gui_cpp::Settings& pluginSettings, const qt_gui_cpp::Settings& instanceSettings)
+{
+	if(instanceSettings.contains("connection"))
+		m_connectionBox->addItem(instanceSettings.value("connection").toString());
+	if(instanceSettings.contains("groups"))
+	{
+		m_grpYamlString = instanceSettings.value("groups").toString();
+		GroupMap groupMap;
+		if(m_grpYamlString.isEmpty())
+			m_grpYamlString = QString::fromStdString(DEFAULT_GROUPS);
+
+		if(!groupFromYaml(m_grpYamlString.toStdString(), &groupMap))
+		{
+			m_grpYamlString = "";
+			return;
+		}
+		m_groupMap = groupMap;
+	}
+}
+
+
+////// Group Dialog
+GroupDialog::GroupDialog()
+ : m_tEdit(new QTextEdit(this))
+{
+	QPushButton* okBtn = new QPushButton("Accept", this);
+	QPushButton* cancelBtn = new QPushButton("Cancel", this);
+	QGridLayout* gl = new QGridLayout(this);
+	gl->addWidget(m_tEdit, 0, 0, 1, 2);
+	gl->addWidget(cancelBtn, 1, 0);
+	gl->addWidget(okBtn, 1, 1);
+	this->setLayout(gl);
+
+	connect(
+		okBtn, SIGNAL(pressed()),
+		this, SLOT(accept())
+	);
+
+	connect(
+		cancelBtn, SIGNAL(pressed()),
+		this, SLOT(reject())
+	);
+}
+
+void GroupDialog::setText(QString text)
+{
+	m_tEdit->setPlainText(text);
+}
+
+QString GroupDialog::text()
+{
+	return m_tEdit->toPlainText();
+}
+
+}
+
+PLUGINLIB_EXPORT_CLASS(nimbro_topic_transport::BandwidthGui, rqt_gui_cpp::Plugin)

software/ros_packages/nimbro_topic_transport/src/gui/bandwidth_gui.h

@@ -0,0 +1,93 @@
+// Bandwidth display for transferred topics
+// Author: Sebastian Schüller
+
+#ifndef BANDWIDTH_GUI
+#define BANDWIDTH_GUI
+
+#include <rqt_gui_cpp/plugin.h>
+
+#include <ros/subscriber.h>
+#include <nimbro_topic_transport/SenderStats.h>
+#include <map>
+
+#include <QTimer>
+#include <QDialog>
+
+class QCustomPlot;
+class QCPGraph;
+class QCPLegend;
+class QCPAbstractLegendItem;
+class QMouseEvent;
+class QComboBox;
+class QTextEdit;
+
+typedef std::map<std::string, std::vector<std::string> > GroupMap;
+
+namespace nimbro_topic_transport
+{
+
+
+class BandwidthGui : public rqt_gui_cpp::Plugin
+{
+Q_OBJECT
+public:
+	BandwidthGui();
+	virtual ~BandwidthGui();
+
+	virtual void initPlugin(qt_gui_cpp::PluginContext & ) override;
+	virtual void shutdownPlugin() override;
+	virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const override;
+	virtual void restoreSettings(const qt_gui_cpp::Settings& pluginSettings, const qt_gui_cpp::Settings& instanceSettings) override;
+
+Q_SIGNALS:
+	// Bounce SenderStats msg from ros thread to GUI thread
+	void senderStatsReceived(const nimbro_topic_transport::SenderStatsConstPtr& msg);
+
+private Q_SLOTS:
+	void handleSenderStats(const nimbro_topic_transport::SenderStatsConstPtr& msg);
+	void updateGroupInfo();
+	void updatePlot();
+	void clearPlot();
+	void handleClickedLegend(QCPLegend *legend, QCPAbstractLegendItem *item, QMouseEvent *event);
+
+private:
+	struct GraphValue
+	{
+		float value; // bandwidth value
+		double timestamp; // in seconds since epoch
+		double last_timestamp; // in seconds, timestamp of last point
+		QCPGraph* graph;
+	};
+
+	bool groupFromYaml(const std::string& yaml, GroupMap* map);
+
+	QCustomPlot* m_plot;
+	QComboBox* m_connectionBox;
+	QTimer m_plotTimer;
+
+
+	ros::Subscriber m_sub_senderStats;
+	std::map<std::string, GraphValue> m_bandwidths;
+	double m_maxBandwidth;
+	int m_hue;
+
+	std::vector<std::string> m_connections;
+
+	QString m_grpYamlString;
+	GroupMap m_groupMap;
+};
+
+// Text Dialog to change Group Settings
+class GroupDialog : public QDialog
+{
+Q_OBJECT
+public:
+	GroupDialog();
+	void setText(QString text);
+	QString text();
+private:
+	QTextEdit* m_tEdit;
+};
+
+}
+#endif

software/ros_packages/nimbro_topic_transport/src/gui/contrib/qcustomplot/GPL.txt

@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    <program>  Copyright (C) <year>  <name of author>
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<http://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.

software/ros_packages/nimbro_topic_transport/src/gui/contrib/qcustomplot/changelog.txt

@@ -0,0 +1,416 @@
+#### Version 1.3.1 released on 25.04.15 ####
+
+  Bugfixes:
+    - Fixed bug that prevented automatic axis rescaling when some graphs/curves had only NaN data points
+    - Improved QCPItemBracket selection boundaries, especially bsCurly and bsCalligraphic
+    - Fixed bug of axis rect and colorscale background shifted downward by one logical pixel (visible in scaled png and pdf export)
+    - Replot upon mouse release is now only performed if a selection change has actually happened (improves responsivity on particularly complex plots)
+    - Fixed bug that allowed scatter-only graphs to be selected by clicking the non-existent line between scatters
+    - Fixed crash when trying to select a scatter-only QCPGraph whose only points in the visible key range are at identical key coordinates and vertically off-screen, with adaptive sampling enabled
+    - Fixed pdf export of QCPColorMap with enabled interpolation (didn't appear interpolated in pdf)
+    - Reduced QCPColorMap jitter of internal cell boundaries for small sized maps when viewed with high zoom, by applying oversampling factors dependant on map size
+    - Fixed bug of QCPColorMap::fill() not causing the buffered internal image map to be updated, and thus the change didn't become visible immediately
+    - Axis labels with size set in pixels (setPixelSize) instead of points now correctly calculate the exponent's font size if beautifully typeset powers are enabled
+    - Fixed QCPColorMap appearing at the wrong position for logarithmic axes and color map spanning larger ranges
+    
+  Other:
+    - Pdf export used to embed entire QCPColorMaps, potentially leading to large files. Now only the visible portion of the map is embedded in the pdf
+    - Many documentation fixes and extensions, style modernization
+    - Reduced documentation file size (and thus full package size) by automatically reducing image palettes during package build
+    - Fixed MSVC warning message (at warning level 4) due to temporary QLists in some foreach statements
+
+#### Version 1.3.0 released on 27.12.14 ####
+
+  Added features:
+    - New plottable class QCPFinancial allows display of candlestick/ohlc data
+    - New class QCPBarsGroup allows horizontal grouping of multiple QCPBars plottables
+    - Added QCPBars feature allowing non-zero base values (see property QCPBars::setBaseValue)
+    - Added QCPBars width type, for more flexible bar widths (see property QCPBars::setWidthType)
+    - New QCPCurve optimization algorithm, fixes bug which caused line flicker at deep zoom into curve segment
+    - Item positions can now have different position types and anchors for their x and y coordinates (QCPItemPosition::setTypeX/Y, setParentAnchorX/Y)
+    - QCPGraph and QCPCurve can now display gaps in their lines, when inserting quiet NaNs as values (std::numeric_limits<double>::quiet_NaN())
+    - QCPAxis now supports placing the tick labels inside the axis rect, for particularly space saving plots (QCPAxis::setTickLabelSide)
+  Added features after beta:
+    - Made code compatible with QT_NO_CAST_FROM_ASCII, QT_NO_CAST_TO_ASCII
+    - Added compatibility with QT_NO_KEYWORDS after sending code files through a simple reg-ex script
+    - Added possibility to inject own QCPAxis(-subclasses) via second, optional QCPAxisRect::addAxis parameter
+    - Added parameter to QCPItemPixmap::setScaled to specify transformation mode
+  
+  Bugfixes:
+    - Fixed bug in QCPCurve rendering of very zoomed-in curves (via new optimization algorithm)
+    - Fixed conflict with MSVC-specific keyword "interface" in text-document-integration example
+    - Fixed QCPScatterStyle bug ignoring the specified pen in the custom scatter shape constructor
+    - Fixed bug (possible crash) during QCustomPlot teardown, when a QCPLegend that has no parent layout (i.e. was removed from layout manually) gets deleted
+  Bugfixes after beta:
+    - Fixed bug of QCPColorMap/QCPColorGradient colors being off by one color sampling step (only noticeable in special cases)
+    - Fixed bug of QCPGraph adaptive sampling on vertical key axis, causing staggered look
+    - Fixed low (float) precision in QCPCurve optimization algorithm, by not using QVector2D anymore
+  
+  Other:
+    - Qt 5.3 and Qt 5.4 compatibility
+
+#### Version 1.2.1 released on 07.04.14 ####
+
+  Bugfixes:
+    - Fixed regression which garbled date-time tick labels on axes, if setTickLabelType is ltDateTime and setNumberFormat contains the "b" option
+
+#### Version 1.2.0 released on 14.03.14 ####
+
+  Added features:
+    - Adaptive Sampling for QCPGraph greatly improves performance for high data densities (see QCPGraph::setAdaptiveSampling)
+    - QCPColorMap plottable with QCPColorScale layout element allows plotting of 2D color maps
+    - QCustomPlot::savePdf now has additional optional parameters pdfCreator and pdfTitle to set according PDF metadata fields
+    - QCustomPlot::replot now allows specifying whether the widget update is immediate (repaint) or queued (update)
+    - QCPRange operators +, -, *, / with double operand for range shifting and scaling, and ==, != for range comparison
+    - Layers now have a visibility property (QCPLayer::setVisible)
+    - static functions QCPAxis::opposite and QCPAxis::orientation now offer more convenience when handling axis types
+    - added notification signals for selectability change (selectableChanged) on all objects that have a selected/selectable property
+    - added notification signal for QCPAxis scaleType property
+    - added notification signal QCPLayerable::layerChanged
+  
+  Bugfixes:
+    - Fixed assert halt, when QCPAxis auto tick labels not disabled but nevertheless a custom non-number tick label ending in "e" given
+    - Fixed painting glitches when QCustomPlot resized inside a QMdiArea or under certain conditions inside a QLayout
+    - If changing QCPAxis::scaleType and thus causing range sanitizing and a range modification, rangeChanged wouldn't be emitted
+    - Fixed documentation bug that caused indentation to be lost in code examples
+  Bugfixes after beta:
+    - Fixed bug that caused crash if clicked-on legend item is removed in mousePressEvent.
+    - On some systems, font size defaults to -1, which used to cause a debug output in QCPAxisPainterPrivate::TickLabelDataQCP. Now it's checked before setting values based on the default font size.
+    - When using multiple axes on one side, setting one to invisible didn't properly compress the freed space.
+    - Fixed bug that allowed selection of plottables when clicking in the bottom or top margin of a QCPAxisRect (outside the inner rect)
+    
+  Other:
+    - In method QCPAbstractPlottable::getKeyRange/getValueRange, renamed parameter "validRange" to "foundRange", to better reflect its meaning (and contrast it from QCPRange::validRange)
+    - QCPAxis low-level axis painting methods exported to QCPAxisPainterPrivate
+
+#### Version 1.1.1 released on 09.12.13 ####
+
+  Bugfixes:
+    - Fixed bug causing legends blocking input events from reaching underlying axis rect even if legend is invisible
+    - Added missing Q_PROPERTY for QCPAxis::setDateTimeSpec
+    - Fixed behaviour of QCPAxisRect::setupFullAxesBox (now transfers more properties from bottom/left to top/right axes and sets visibility of bottom/left axes to true)
+    - Made sure PDF export doesn't default to grayscale output on some systems
+  
+  Other:
+    - Plotting hint QCP::phForceRepaint is now enabled on all systems (and not only on windows) by default
+    - Documentation improvements
+
+#### Version 1.1.0 released on 04.11.13 ####
+
+  Added features:
+    - Added QCPRange::expand and QCPRange::expanded
+    - Added QCPAxis::rescale to rescale axis to all associated plottables
+    - Added QCPAxis::setDateTimeSpec/dateTimeSpec to allow axis labels either in UTC or local time
+    - QCPAxis now additionally emits a rangeChanged signal overload that provides the old range as second parameter
+    
+  Bugfixes:
+    - Fixed QCustomPlot::rescaleAxes not rescaling properly if first plottable has an empty range
+    - QCPGraph::rescaleAxes/rescaleKeyAxis/rescaleValueAxis are no longer virtual (never were in base class, was a mistake)
+    - Fixed bugs in QCPAxis::items and QCPAxisRect::items not properly returning associated items and potentially stalling
+    
+  Other:
+    - Internal change from QWeakPointer to QPointer, thus got rid of deprecated Qt functionality
+    - Qt5.1 and Qt5.2 (beta1) compatibility
+    - Release packages now extract to single subdirectory and don't place multiple files in current working directory
+
+#### Version 1.0.1 released on 05.09.13 ####
+
+  Bugfixes:
+    - using define flag QCUSTOMPLOT_CHECK_DATA caused debug output when data was correct, instead of invalid (fixed QCP::isInvalidData)
+    - documentation images are now properly shown when viewed with Qt Assistant
+    - fixed various documentation mistakes
+    
+  Other:
+    - Adapted documentation style sheet to better match Qt5 documentation
+
+#### Version 1.0.0 released on 01.08.13 ####
+
+  Quick Summary:
+    - Layout system for multiple axis rects in one plot
+    - Multiple axes per side
+    - Qt5 compatibility
+    - More flexible and consistent scatter configuration with QCPScatterStyle
+    - Various interface cleanups/refactoring
+    - Pixmap-cached axis labels for improved replot performance
+
+  Changes that break backward compatibility:
+    - QCustomPlot::axisRect() changed meaning due to the extensive changes to how axes and axis rects are handled
+      it now returns a pointer to a QCPAxisRect and takes an integer index as parameter.
+    - QCPAxis constructor changed to now take QCPAxisRect* as parent
+    - setAutoMargin, setMarginLeft/Right/Top/Bottom removed due to the axis rect changes (see QCPAxisRect::setMargins/setAutoMargins)
+    - setAxisRect removed due to the axis rect changes
+    - setAxisBackground(-Scaled/-ScaledMode) now moved to QCPAxisRect as setBackground(-Scaled/ScaledMode) (access via QCustomPlot::axisRects())
+    - QCPLegend now is a QCPLayoutElement
+    - QCPAbstractPlottable::drawLegendIcon parameter "rect" changed from QRect to QRectF
+    - QCPAbstractLegendItem::draw second parameter removed (position/size now handled via QCPLayoutElement base class)
+    - removed QCPLegend::setMargin/setMarginLeft/Right/Top/Bottom (now inherits the capability from QCPLayoutElement::setMargins)
+    - removed QCPLegend::setMinimumSize (now inherits the capability from QCPLayoutElement::setMinimumSize)
+    - removed enum QCPLegend::PositionStyle, QCPLegend::positionStyle/setPositionStyle/position/setPosition (replaced by capabilities of QCPLayoutInset)
+    - QCPLegend transformed to work with new layout system (almost everything changed)
+    - removed entire title interface: QCustomPlot::setTitle/setTitleFont/setTitleColor/setTitleSelected/setTitleSelectedFont/setTitleSelectedColor and
+      the QCustomPlot::iSelectTitle interaction flag (all functionality is now given by the layout element "QCPPlotTitle" which can be added to the plot layout)
+    - selectTest functions now take two additional parameters: bool onlySelectable and QVariant *details=0
+    - selectTest functions now ignores visibility of objects and (if parameter onlySelectable is true) does not anymore ignore selectability of the object
+    - moved QCustomPlot::Interaction/Interactions to QCP namespace as QCP::Interaction/Interactions
+    - moved QCustomPlot::setupFullAxesBox() to QCPAxisRect::setupFullAxesBox. Now also accepts parameter to decide whether to connect opposite axis ranges
+    - moved range dragging/zooming interface from QCustomPlot to QCPAxisRect (setRangeDrag, setRangeZoom, setRangeDragAxes, setRangeZoomAxes,...)
+    - rangeDrag/Zoom is now set to Qt::Horizontal|Qt::Vertical instead of 0 by default, on the other hand, iRangeDrag/Zoom is unset in interactions by
+      default (this makes enabling dragging/zooming easier by just adding the interaction flags)
+    - QCPScatterStyle takes over everything related to handling scatters in all plottables
+    - removed setScatterPen/Size on QCPGraph and QCPCurve, removed setOutlierPen/Size on QCPStatisticalBox (now handled via QCPScatterStyle)
+    - modified setScatterStyle on QCPGraph and QCPCurve, and setOutlierStyle on QCPStatisticalBox, to take QCPScatterStyle
+    - axis grid and subgrid are now reachable via the QCPGrid *QCPAxis::grid() method. (e.g. instead of xAxis->setGrid(true), write xAxis->grid()->setVisible(true))
+
+  Added features:
+    - Axis tick labels are now pixmap-cached, thus increasing replot performance (in usual setups by about 24%). See plotting hint phCacheLabels which is set by default
+    - Advanced layout system, including the classes QCPLayoutElement, QCPLayout, QCPLayoutGrid, QCPLayoutInset, QCPAxisRect
+    - QCustomPlot::axisRects() returns all the axis rects in the QCustomPlot.
+    - QCustomPlot::plotLayout() returns the top level layout (initially a QCPLayoutGrid with one QCPAxisRect inside)
+    - QCPAxis now may have an offset to the axis rect (setOffset)
+    - Multiple axes per QCPAxisRect side are now supported (see QCPAxisRect::addAxis)
+    - QCustomPlot::toPixmap renders the plot into a pixmap and returns it
+    - When setting tick label rotation to +90 or -90 degrees on a vertical axis, the labels are now centered vertically on the tick height
+      (This allows space saving vertical tick labels by having the text direction parallel to the axis)
+    - Substantially increased replot performance when using very large manual tick vectors (> 10000 ticks) via QCPAxis::setTickVector
+    - QCPAxis and QCPAxisRect now allow easy access to all plottables(), graphs() and items() that are associated with them
+    - Added QCustomPlot::hasItem method for consistency with plottable interface, hasPlottable
+    - Added QCPAxisRect::setMinimumMargins as replacement for hardcoded minimum axis margin (15 px) when auto margin is enabled
+    - Added Flags type QCPAxis::AxisTypes (from QCPAxis::AxisType), used in QCPAxisRect interface
+    - Automatic margin calculation can now be enabled/disabled on a per-side basis, see QCPAxisRect::setAutoMargins
+    - QCPAxisRect margins of multiple axis rects can be coupled via QCPMarginGroup
+    - Added new default layers "background" and "legend" (QCPAxisRect draws its background on the "background" layer, QCPLegend is on the "legend" layer by default)
+    - Custom scatter style via QCP::ssCustom and respective setCustomScatter functions that take a QPainterPath
+    - Filled scatters via QCPScatterStyle::setBrush
+  Added features after beta:
+    - Added QCustomPlot::toPainter method, to allow rendering with existing painter
+    - QCPItemEllipse now provides a center anchor
+
+  Bugfixes:
+    - Fixed compile error on ARM
+    - Wrong legend icons were displayed if using pixmaps for scatters that are smaller than the legend icon rect
+    - Fixed clipping inaccuracy for rotated tick labels (were hidden too early, because the non-rotated bounding box was used)
+    - Fixed bug that caused wrong clipping of axis ticks and subticks when the ticks were given manually by QCPAxis::setTickVector
+    - Fixed Qt5 crash when dragging graph out of view (iterator out of bounds in QCPGraph::getVisibleDataBounds)
+    - Fixed QCPItemText not scaling properly when using scaled raster export
+  Bugfixes after beta:
+    - Fixed bug that clipped the rightmost pixel column of tick labels when caching activated (only visible on windows for superscript exponents)
+    - Restored compatibility to Qt4.6
+    - Restored support for -no-RTTI compilation
+    - Empty manual tick labels are handled more gracefully (no QPainter qDebug messages anymore)
+    - Fixed type ambiguity in QCPLineEnding::draw causing compile error on ARM
+    - Fixed bug of grid layouts not propagating the minimum size from their child elements to the parent layout correctly
+    - Fixed bug of child elements (e.g. axis rects) of inset layouts not properly receiving mouse events
+
+  Other:
+    - Opened up non-amalgamated project structure to public via git repository
+
+#### Version released on 09.06.12 ####
+
+  Quick Summary:
+    - Items (arrows, text,...)
+    - Layers (easier control over rendering order)
+    - New antialiasing system (Each objects controls own antialiasing with setAntialiased)
+    - Performance Improvements
+    - improved pixel-precise drawing
+    - easier shared library creation/usage
+
+  Changes that (might) break backward compatibility:
+    - enum QCPGraph::ScatterSymbol was moved to QCP namespace (now QCP::ScatterSymbol).
+      This replace should fix your code: "QCPGraph::ss" -> "QCP::ss"
+    - enum QCustomPlot::AntialiasedElement and flag QCustomPlot::AntialiasedElements was moved to QCP namespace
+      This replace should fix your code: "QCustomPlot::ae" -> "QCP::ae"
+    - the meaning of QCustomPlot::setAntialiasedElements has changed slightly: It is now an override to force elements to be antialiased. If you want to force
+      elements to not be drawn antialiased, use the new setNotAntialiasedElements. If an element is mentioned in neither of those functions, it now controls
+      its antialiasing itself via its "setAntialiased" function(s). (e.g. QCPAxis::setAntialiased(bool), QCPAbstractPlottable::setAntialiased(bool),
+      QCPAbstractPlottable::setAntialiasedScatters(bool), etc.)
+    - QCPAxis::setTickVector and QCPAxis::setTickVectorLabels no longer take a pointer but a const reference of the respective QVector as parameter.
+      (handing over a pointer didn't give any noticeable performance benefits but was inconsistent with the rest of the interface)
+    - Equally QCPAxis::tickVector and QCPAxis::tickVectorLabels don't return by pointer but by value now
+    - QCustomPlot::savePngScaled was removed, its purpose is now included as optional parameter "scale" of savePng.
+    - If you have derived from QCPAbstractPlottable: all selectTest functions now consistently take the argument "const QPointF &pos" which is the test point in pixel coordinates.
+      (the argument there was "double key, double value" in plot coordinates, before).
+    - QCPAbstractPlottable, QCPAxis and QCPLegend now inherit from QCPLayerable
+    - If you have derived from QCPAbstractPlottable: the draw method signature has changed from "draw (..) const" to "draw (..)", i.e. the method
+      is not const anymore. This allows the draw function of your plottable to perform buffering/caching operations, if necessary.
+
+  Added features:
+    - Item system: QCPAbstractItem, QCPItemAnchor, QCPItemPosition, QCPLineEnding. Allows placing of lines, arrows, text, pixmaps etc.
+      - New Items: QCPItemStraightLine, QCPItemLine, QCPItemCurve, QCPItemEllipse, QCPItemRect, QCPItemPixmap, QCPItemText, QCPItemBracket, QCPItemTracer
+      - QCustomPlot::addItem/itemCount/item/removeItem/selectedItems
+      - signals QCustomPlot::itemClicked/itemDoubleClicked
+      - the QCustomPlot interactions property now includes iSelectItems (for selection of QCPAbstractItem)
+      - QCPLineEnding. Represents the different styles a line/curve can end (e.g. different arrows, circle, square, bar, etc.), see e.g. QCPItemCurve::setHead
+    - Layer system: QCPLayerable, QCPLayer. Allows more sophisticated control over drawing order and a kind of grouping.
+      - QCPAbstractPlottable, QCPAbstractItem, QCPAxis, QCPGrid, QCPLegend are layerables and derive from QCPLayerable
+      - QCustomPlot::addLayer/moveLayer/removeLayer/setCurrentLayer/layer/currentLayer/layerCount
+      - Initially there are three layers: "grid", "main", and "axes". The "main" layer is initially empty and set as current layer, so new plottables/items are put there.
+    - QCustomPlot::viewport now makes the previously inaccessible viewport rect read-only-accessible (needed that for item-interface)
+    - PNG export now allows transparent background by calling QCustomPlot::setColor(Qt::transparent) before savePng
+    - QCPStatisticalBox outlier symbols may now be all scatter symbols, not only hardcoded circles.
+    - perfect precision of scatter symbol/error bar drawing and clipping in both antialiased and non-antialiased mode, by introducing QCPPainter
+      that works around some QPainter bugs/inconveniences. Further, more complex symbols like ssCrossSquare used to look crooked, now they look good.
+    - new antialiasing control system: Each drawing element now has its own "setAntialiased" function to control whether it is drawn antialiased.
+      - QCustomPlot::setAntialiasedElements and QCustomPlot::setNotAntialiasedElements can be used to override the individual settings.
+      - Subclasses of QCPAbstractPlottable can now use the convenience functions like applyFillAntialiasingHint or applyScattersAntialiasingHint to
+        easily make their drawing code comply with the overall antialiasing system.
+    - QCustomPlot::setNoAntialiasingOnDrag allows greatly improved performance and responsiveness by temporarily disabling all antialiasing while
+      the user is dragging axis ranges
+    - QCPGraph can now show scatter symbols at data points and hide its line (see QCPGraph::setScatterStyle, setScatterSize, setScatterPixmap, setLineStyle)
+    - Grid drawing code was sourced out from QCPAxis to QCPGrid. QCPGrid is mainly an internal class and every QCPAxis owns one. The grid interface still
+      works through QCPAxis and hasn't changed. The separation allows the grid to be drawn on a different layer as the axes, such that e.g. a graph can
+      be above the grid but below the axes.
+    - QCustomPlot::hasPlottable(plottable), returns whether the QCustomPlot contains the plottable
+    - QCustomPlot::setPlottingHint/setPlottingHints, plotting hints control details about the plotting quality/speed
+    - export to jpg and bmp added (QCustomPlot::saveJpg/saveBmp), as well as control over compression quality for png and jpg
+    - multi-select-modifier may now be specified with QCustomPlot::setMultiSelectModifier and is not fixed to Ctrl anymore
+
+  Bugfixes:
+    - fixed QCustomPlot ignores replot after it had size (0,0) even if size becomes valid again
+    - on Windows, a repaint used to be delayed during dragging/zooming of a complex plot, until the drag operation was done.
+      This was fixed, i.e. repaints are forced after a replot() call. See QCP::phForceRepaint and setPlottingHints.
+    - when using the raster paintengine and exporting to scaled PNG, pen widths are now scaled correctly (QPainter bug workaround via QCPPainter)
+    - PDF export now respects QCustomPlot background color (QCustomPlot::setColor), also Qt::transparent
+    - fixed a bug on QCPBars and QCPStatisticalBox where auto-rescaling of axis would fail when all data is very small (< 1e-11)
+    - fixed mouse event propagation bug that prevented range dragging from working on KDE (GNU/Linux)
+    - fixed a compiler warning on 64-bit systems due to pointer cast to int instead of quintptr in a qDebug output
+
+  Other:
+    - Added support for easier shared library creation (including examples for compiling and using QCustomPlot as shared library)
+    - QCustomPlot now has the Qt::WA_OpaquePaintEvent widget attribute (gives slightly improved performance).
+    - QCP::aeGraphs (enum QCP::AntialiasedElement, previously QCustomPlot::aeGraphs) has been marked deprecated since version 02.02.12 and
+      was now removed. Use QCP::aePlottables instead.
+    - optional performance-quality-tradeoff for solid graph lines (see QCustomPlot::setPlottingHints).
+    - marked data classes and QCPRange as Q_MOVABLE_TYPE
+    - replaced usage of own macro FUNCNAME with Qt macro Q_FUNC_INFO
+    - QCustomPlot now returns a minimum size hint of 50*50
+
+#### Version released on 31.03.12 ####
+
+  Changes that (might) break backward compatibility:
+  - QCPAbstractLegendItem now inherits from QObject
+  - mousePress, mouseMove and mouseRelease signals are now emitted before and not after any QCustomPlot processing (range dragging, selecting, etc.)
+
+  Added features:
+  - Interaction system: now allows selecting of objects like plottables, axes, legend and plot title, see QCustomPlot::setInteractions documentation
+  - Interaction system for plottables:
+    - setSelectable, setSelected, setSelectedPen, setSelectedBrush, selectTest on QCPAbstractPlottable and all derived plottables
+    - setSelectionTolerance on QCustomPlot
+    - selectedPlottables and selectedGraphs on QCustomPlot (returns the list of currently selected plottables/graphs)
+  - Interaction system for axes:
+    - setSelectable, setSelected, setSelectedBasePen, setSelectedTickPen, setSelectedSubTickPen, setSelectedLabelFont, setSelectedTickLabelFont,
+      setSelectedLabelColor, setSelectedTickLabelColor, selectTest on QCPAxis
+    - selectedAxes on QCustomPlot (returns a list of the axes that currently have selected parts)
+  - Interaction system for legend:
+    - setSelectable, setSelected, setSelectedBorderPen, setSelectedIconBorderPen, setSelectedBrush, setSelectedFont, setSelectedTextColor, selectedItems on QCPLegend
+    - setSelectedFont, setSelectedTextColor, setSelectable, setSelected on QCPAbstractLegendItem
+    - selectedLegends on QCustomPlot
+  - Interaction system for title:
+    - setSelectedTitleFont, setSelectedTitleColor, setTitleSelected on QCustomPlot
+  - new signals in accordance with the interaction system:
+    - selectionChangedByUser on QCustomPlot
+    - selectionChanged on QCPAbstractPlottable
+    - selectionChanged on QCPAxis
+    - selectionChanged on QCPLegend and QCPAbstractLegendItem
+    - plottableClick, legendClick, axisClick, titleClick, plottableDoubleClick, legendDoubleClick, axisDoubleClick, titleDoubleClick on QCustomPlot
+  - QCustomPlot::deselectAll (deselects everything, i.e. axes and plottables)
+  - QCPAbstractPlottable::pixelsToCoords (inverse function to the already existing coordsToPixels function)
+  - QCPRange::contains(double value)
+  - QCPAxis::setLabelColor and setTickLabelColor
+  - QCustomPlot::setTitleColor
+  - QCustomPlot now emits beforeReplot and afterReplot signals. Note that it is safe to make two customPlots mutually call eachothers replot functions
+    in one of these slots, it will not cause an infinite loop. (usefull for synchronizing axes ranges between two customPlots, because setRange alone doesn't replot)
+  - If the Qt version is 4.7 or greater, the tick label strings in date-time-mode now support sub-second accuracy (e.g. with format like "hh:mm:ss.zzz").
+
+  Bugfixes:
+  - tick labels/margins should no longer oscillate by one pixel when dragging range or replotting repeatedly while changing e.g. data. This
+    was caused by a bug in Qt's QFontMetrics::boundingRect function when the font has an integer point size (probably some rounding problem).
+    The fix hence consists of creating a temporary font (only for bounding-box calculation) which is 0.05pt larger and thus avoiding the
+    jittering rounding outcome.
+  - tick label, axis label and plot title colors used to be undefined. This was fixed by providing explicit color properties.
+    
+  Other:
+  - fixed some glitches in the documentation
+  - QCustomPlot::replot and QCustomPlot::rescaleAxes are now slots
+
+#### Version released on 02.02.12 ####
+
+  Changes that break backward compatibility:
+  - renamed all secondary classes from QCustomPlot[...] to QCP[...]:
+    QCustomPlotAxis -> QCPAxis
+    QCustomPlotGraph -> QCPGraph
+    QCustomPlotRange -> QCPRange
+    QCustomPlotData -> QCPData
+    QCustomPlotDataMap -> QCPDataMap
+    QCustomPlotLegend -> QCPLegend
+    QCustomPlotDataMapIterator -> QCPDataMapIterator
+    QCustomPlotDataMutableMapIterator -> QCPDataMutableMapIterator
+    A simple search and replace on all code files should make your code run again, e.g. consider the regex "QCustomPlot(?=[AGRDL])" -> "QCP".
+    Make sure not to just replace "QCustomPlot" with "QCP" because the main class QCustomPlot hasn't changed to QCP.
+    This change was necessary because class names became unhandy, pardon my bad naming decision in the beginning.
+  - QCPAxis::tickLength() and QCPAxis::subTickLength() now each split into two functions for inward and outward ticks (tickLengthIn/tickLengthOut).
+  - QCPLegend now uses QCPAbstractLegendItem to carry item data (before, the legend was passed QCPGraphs directly)
+  - QCustomPlot::addGraph() now doesn't return the index of the created graph anymore, but a pointer to the created QCPGraph.
+  - QCustomPlot::setAutoAddGraphToLegend is replaced by setAutoAddPlottableToLegend
+
+  Added features:
+  - Reversed axis range with QCPAxis::setRangeReversed(bool)
+  - Tick labels are now only drawn if not clipped by the viewport (widget border) on the sides (e.g. left and right on a horizontal axis).
+  - Zerolines. Like grid lines only with a separate pen (QCPAxis::setZeroLinePen), at tick position zero.
+  - Outward ticks. QCPAxis::setTickLength/setSubTickLength now accepts two arguments for inward and outward tick length. This doesn't break
+    backward compatibility because the second argument (outward) has default value zero and thereby a call with one argument hasn't changed its meaning.
+  - QCPGraph now inherits from QCPAbstractPlottable
+  - QCustomPlot::addPlottable/plottable/removePlottable/clearPlottables added to interface with the new QCPAbstractPlottable-based system. The simpler interface
+    which only acts on QCPGraphs (addGraph, graph, removeGraph, etc.) was adapted internally and is kept for backward compatibility and ease of use.
+  - QCPLegend items for plottables (e.g. graphs) can automatically wrap their texts to fit the widths, see QCPLegend::setMinimumSize and QCPPlottableLegendItem::setTextWrap.
+  - QCustomPlot::rescaleAxes. Adapts axis ranges to show all plottables/graphs, by calling QCPAbstractPlottable::rescaleAxes on all plottables in the plot.
+  - QCPCurve. For plotting of parametric curves.
+  - QCPBars. For plotting of bar charts.
+  - QCPStatisticalBox. For statistical box plots.
+
+  Bugfixes:
+  - Fixed QCustomPlot::removeGraph(int) not being able to remove graph index 0
+  - made QCustomPlot::replot() abort painting when painter initialization fails (e.g. because width/height of QCustomPlot is zero)
+  - The distance of the axis label from the axis ignored the tick label padding, this could have caused overlapping axis labels and tick labels
+  - fixed memory leak in QCustomPlot (dtor didn't delete legend)
+  - fixed bug that prevented QCPAxis::setRangeLower/Upper from setting the value to exactly 0.
+
+  Other:
+  - Changed default error bar handle size (QCustomPlotGraph::setErrorBarSize) from 4 to 6.
+  - Removed QCustomPlotDataFetcher. Was deprecated and not used class.
+  - Extended documentation, especially class descriptions.
+
+#### Version released on 15.01.12 ####
+
+  Changes that (might) break backward compatibility:
+  - QCustomPlotGraph now inherits from QObject
+  
+  Added features:
+  - Added axis background pixmap (QCustomPlot::setAxisBackground, setAxisBackgroundScaled, setAxisBackgroundScaledMode)
+  - Added width and height parameter on PDF export function QCustomPlot::savePdf(). This now allows PDF export to
+    have arbitrary dimensions, independent of the current geometry of the QCustomPlot.
+  - Added overload of QCustomPlot::removeGraph that takes QCustomPlotGraph* as parameter, instead the index of the graph
+  - Added all enums to the Qt meta system via Q_ENUMS(). The enums can now be transformed
+    to QString values easily with the Qt meta system, which makes saving state e.g. as XML
+    significantly nicer.
+  - added typedef QMapIterator<double,QCustomPlotData> QCustomPlotDataMapIterator
+    and typedef QMutableMapIterator<double,QCustomPlotData> QCustomPlotDataMutableMapIterator
+    for improved information hiding, when using iterators outside QCustomPlot code
+  
+  Bugfixes:
+  - Fixed savePngScaled. Axis/label drawing functions used to reset the painter transform
+    and thereby break savePngScaled. Now they buffer the current transform and restore it afterwards.
+  - Fixed some glitches in the doxygen comments (affects documentation only)
+  
+  Other:
+  - Changed the default tickLabelPadding of top axis from 3 to 6 pixels. Looks better.
+  - Changed the default QCustomPlot::setAntialiasedElements setting: Graph fills are now antialiased
+    by default. That's a bit slower, but makes fill borders look better.
+
+#### Version released on 19.11.11 ####
+
+  Changes that break backward compatibility:
+  - QCustomPlotAxis: tickFont and setTickFont renamed to tickLabelFont and setTickLabelFont (for naming consistency)
+
+  Other:
+  - QCustomPlotAxis: Added rotated tick labels, see setTickLabelRotation
+    

software/ros_packages/nimbro_topic_transport/src/gui/dot_widget.cpp

@@ -0,0 +1,139 @@
+// Display a .dot graph
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "dot_widget.h"
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdexcept>
+#include <sys/wait.h>
+
+
+#include <QPainter>
+
+static ssize_t runDot(const std::string& dot, int width, int height, uint8_t* pngOut, size_t pngOutSize)
+{
+	int inputPipe[2];
+	int outputPipe[2];
+
+	if(pipe(inputPipe) != 0 || pipe(outputPipe) != 0)
+		throw std::runtime_error("Could not create pipe");
+
+	int pid = fork();
+	if(pid == 0)
+	{
+		// Child
+		close(inputPipe[1]);
+		close(outputPipe[0]);
+
+		dup2(inputPipe[0], STDIN_FILENO);
+		dup2(outputPipe[1], STDOUT_FILENO);
+
+		char sizeBuf[256];
+		int dpi = 400;
+		snprintf(sizeBuf, sizeof(sizeBuf), "-Gsize=%f,%f", ((float)width)/dpi, ((float)height)/dpi);
+
+		if(execlp("dot", "dot", "-Tpng", sizeBuf, "-Gdpi=400", "/dev/stdin", (char*)NULL) != 0)
+		{
+			perror("execlp() failed");
+			throw std::runtime_error("Could not execute dot");
+			exit(1);
+		}
+
+		// Cannot reach this
+		return -1;
+	}
+	else
+	{
+		// Parent
+		close(inputPipe[0]);
+		close(outputPipe[1]);
+
+		size_t written = 0;
+		size_t len = dot.length();
+		while(written != len)
+		{
+			int ret = write(inputPipe[1], dot.c_str() + written, len - written);
+			if(ret <= 0)
+				perror("Could not write");
+
+			written += ret;
+		}
+		close(inputPipe[1]);
+
+		size_t readBytes = 0;
+		while(1)
+		{
+			if(pngOutSize == readBytes)
+			{
+				throw std::runtime_error("png buffer is to small");
+			}
+
+			int ret = read(outputPipe[0], pngOut + readBytes, pngOutSize - readBytes);
+			if(ret == 0)
+				break;
+			else if(ret < 0)
+			{
+				perror("Could not read");
+				throw std::runtime_error("Could not read");
+			}
+
+			readBytes += ret;
+		}
+		close(outputPipe[0]);
+
+		int status;
+		waitpid(pid, &status, 0);
+		if(status != 0)
+			fprintf(stderr, "dot exited with error status %d\n", status);
+
+		return readBytes;
+	}
+}
+
+namespace nimbro_topic_transport
+{
+
+DotWidget::DotWidget()
+ : m_pngBuf(1024 * 1024)
+{
+}
+
+DotWidget::~DotWidget()
+{
+}
+
+void DotWidget::paintEvent(QPaintEvent*)
+{
+	QPainter painter(this);
+	painter.fillRect(rect(), Qt::white);
+
+	painter.setRenderHint(QPainter::SmoothPixmapTransform);
+
+	float scale_x = ((float)width()) / m_pixmap.width();
+	float scale_y = ((float)height()) / m_pixmap.height();
+
+	if(scale_x > scale_y)
+	{
+		int newWidth = scale_y * m_pixmap.width();
+		QRect pixmapRect((width() - newWidth)/2, 0, newWidth, height());
+		painter.drawPixmap(pixmapRect, m_pixmap);
+	}
+	else
+	{
+		int newHeight = scale_x * m_pixmap.height();
+		QRect pixmapRect(0, (height() - newHeight)/2, width(), newHeight);
+		painter.drawPixmap(pixmapRect, m_pixmap);
+	}
+}
+
+void DotWidget::updateGraph(const std::string& dot)
+{
+	ssize_t pngSize = runDot(dot, width(), height(), m_pngBuf.data(), m_pngBuf.size());
+
+	m_pixmap.loadFromData(m_pngBuf.data(), pngSize, "png");
+
+	update();
+}
+
+}

software/ros_packages/nimbro_topic_transport/src/gui/dot_widget.h

@@ -0,0 +1,29 @@
+// Display a .dot graph
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef DOT_WIDGET_H
+#define DOT_WIDGET_H
+
+#include <QWidget>
+
+namespace nimbro_topic_transport
+{
+
+class DotWidget : public QWidget
+{
+Q_OBJECT
+public:
+	DotWidget();
+	virtual ~DotWidget();
+
+	void updateGraph(const std::string& dot);
+
+	virtual void paintEvent(QPaintEvent *) override;
+private:
+	QPixmap m_pixmap;
+	std::vector<uint8_t> m_pngBuf;
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/gui/topic_gui.cpp

@@ -0,0 +1,201 @@
+// Diagnostic GUI for nimbro_topic_transport
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "topic_gui.h"
+
+#include <QMetaType>
+
+#include <ros/node_handle.h>
+
+#include <set>
+
+#include <pluginlib/class_list_macros.h>
+
+#include <sstream>
+
+Q_DECLARE_METATYPE(nimbro_topic_transport::SenderStatsConstPtr)
+Q_DECLARE_METATYPE(nimbro_topic_transport::ReceiverStatsConstPtr)
+
+namespace nimbro_topic_transport
+{
+
+TopicGUI::TopicGUI()
+{
+}
+
+TopicGUI::~TopicGUI()
+{
+}
+
+void TopicGUI::initPlugin(qt_gui_cpp::PluginContext& ctx)
+{
+	m_w = new DotWidget;
+	ctx.addWidget(m_w);
+
+	qRegisterMetaType<nimbro_topic_transport::SenderStatsConstPtr>();
+	qRegisterMetaType<nimbro_topic_transport::ReceiverStatsConstPtr>();
+
+	connect(
+		this, SIGNAL(senderStatsReceived(nimbro_topic_transport::SenderStatsConstPtr)),
+		this, SLOT(handleSenderStats(nimbro_topic_transport::SenderStatsConstPtr)),
+		Qt::QueuedConnection
+	);
+
+	m_sub_senderStats = getPrivateNodeHandle().subscribe(
+		"/network/sender_stats", 1, &TopicGUI::senderStatsReceived, this
+	);
+
+	connect(
+		this, SIGNAL(receiverStatsReceived(nimbro_topic_transport::ReceiverStatsConstPtr)),
+		this, SLOT(handleReceiverStats(nimbro_topic_transport::ReceiverStatsConstPtr)),
+		Qt::QueuedConnection
+	);
+
+	m_sub_receiverStats = getPrivateNodeHandle().subscribe(
+		"/network/receiver_stats", 1, &TopicGUI::receiverStatsReceived, this
+	);
+
+	QTimer* updateTimer = new QTimer(this);
+	connect(updateTimer, SIGNAL(timeout()), SLOT(update()));
+	updateTimer->start(2000);
+}
+
+void TopicGUI::shutdownPlugin()
+{
+	m_sub_senderStats.shutdown();
+	m_sub_receiverStats.shutdown();
+}
+
+void TopicGUI::handleSenderStats(const SenderStatsConstPtr& msg)
+{
+	ConnectionIdentifier ident;
+	ident.dest = msg->destination;
+	ident.protocol = msg->protocol;
+	ident.sourcePort = msg->source_port;
+	ident.destPort = msg->destination_port;
+
+	m_senderStats[ident] = msg;
+
+	update();
+}
+
+void TopicGUI::handleReceiverStats(const ReceiverStatsConstPtr& msg)
+{
+	ConnectionIdentifier ident;
+	ident.dest = msg->host;
+	ident.protocol = msg->protocol;
+	ident.sourcePort = msg->remote_port;
+	ident.destPort = msg->local_port;
+
+	m_receiverStats[ident] = msg;
+
+	update();
+}
+
+static std::string sanitize(std::string arg)
+{
+	for(size_t i = 0; i < arg.length(); ++i)
+	{
+		if(!isalnum(arg[i]))
+			arg[i] = '_';
+	}
+
+	return arg;
+}
+
+void TopicGUI::update()
+{
+	ros::Time now = ros::Time::now();
+	ros::Duration timeoutDuration(4.0);
+
+	std::stringstream ss;
+	ss << "digraph G {\n";
+	ss << "K=0.6;\n";
+
+	std::set<ConnectionIdentifier> connections;
+	std::set<std::string> hosts;
+
+	for(auto pair : m_receiverStats)
+	{
+		if(now - pair.second->header.stamp > timeoutDuration)
+			continue;
+
+		connections.insert(pair.first);
+		hosts.insert(pair.second->remote);
+		hosts.insert(pair.first.dest);
+	}
+	for(auto pair : m_senderStats)
+	{
+		if(now - pair.second->header.stamp > timeoutDuration)
+			continue;
+
+		connections.insert(pair.first);
+		hosts.insert(pair.second->host);
+		hosts.insert(pair.first.dest);
+	}
+
+	for(auto& host : hosts)
+	{
+		ss << "node [ label = \"" << host << "\" ]; node_" << sanitize(host) << ";\n";
+	}
+
+	for(const auto& connection : connections)
+	{
+		auto sender_it = m_senderStats.find(connection);
+		auto receiver_it = m_receiverStats.find(connection);
+
+		SenderStatsConstPtr sender;
+		if(sender_it != m_senderStats.end())
+			sender = sender_it->second;
+
+		ReceiverStatsConstPtr receiver;
+		if(receiver_it != m_receiverStats.end())
+			receiver = receiver_it->second;
+
+		std::string source = sender ? sender->host : receiver->remote;
+
+		ss << "node_" << sanitize(source) << " -> node_" << sanitize(connection.dest) << "[ ";
+
+		ss << "label=\"";
+
+		std::string label;
+		if(sender_it != m_senderStats.end() && !sender_it->second->label.empty())
+			label = sender_it->second->label;
+		else if(receiver_it != m_receiverStats.end() && !receiver_it->second->label.empty())
+			label = receiver_it->second->label;
+
+		if(!label.empty())
+			ss << label << "\n";
+
+		ss << connection.protocol << " " << connection.sourcePort << " -> " << connection.destPort << "\n";
+
+		float bandwidth = 0.0;
+		int div = 0;
+		if(sender_it != m_senderStats.end() && now - sender_it->second->header.stamp < timeoutDuration)
+		{
+			bandwidth += sender_it->second->bandwidth;
+			div++;
+		}
+		if(receiver_it != m_receiverStats.end() && now - receiver_it->second->header.stamp < timeoutDuration)
+		{
+			bandwidth += receiver_it->second->bandwidth;
+			div++;
+		}
+
+		char bwBuf[100];
+		snprintf(bwBuf, sizeof(bwBuf), "%.2f MBit/s", bandwidth / 1024 / 1024);
+
+		ss << bwBuf;
+		ss << "\"";
+
+		ss << " ];\n";
+	}
+
+	ss << "}";
+
+	m_w->updateGraph(ss.str());
+}
+
+}
+
+PLUGINLIB_EXPORT_CLASS(nimbro_topic_transport::TopicGUI, rqt_gui_cpp::Plugin)

software/ros_packages/nimbro_topic_transport/src/gui/topic_gui.h

@@ -0,0 +1,87 @@
+// Diagnostic GUI for nimbro_topic_transport
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TOPIC_GUI_H
+#define TOPIC_GUI_H
+
+#include <ros/subscriber.h>
+
+#include <nimbro_topic_transport/SenderStats.h>
+#include <nimbro_topic_transport/ReceiverStats.h>
+
+#include <rqt_gui_cpp/plugin.h>
+
+#include <string>
+#include <map>
+
+#include <QTimer>
+
+#include "dot_widget.h"
+
+namespace nimbro_topic_transport
+{
+
+struct ConnectionIdentifier
+{
+	std::string dest;
+	std::string protocol;
+	int sourcePort;
+	int destPort;
+
+	bool operator==(const ConnectionIdentifier& other) const
+	{
+		return dest == other.dest
+			&& protocol == other.protocol
+			&& sourcePort == other.sourcePort && destPort == other.destPort;
+	}
+
+	bool operator<(const ConnectionIdentifier& other) const
+	{
+		if(dest < other.dest)
+			return true;
+		if(dest > other.dest)
+			return false;
+
+		if(protocol < other.protocol)
+			return true;
+		if(protocol > other.protocol)
+			return false;
+
+		if(sourcePort < other.sourcePort)
+			return true;
+		if(sourcePort > other.sourcePort)
+			return false;
+
+		return destPort < other.destPort;
+	}
+};
+
+class TopicGUI : public rqt_gui_cpp::Plugin
+{
+Q_OBJECT
+public:
+	TopicGUI();
+	virtual ~TopicGUI();
+
+	virtual void initPlugin(qt_gui_cpp::PluginContext & ) override;
+	virtual void shutdownPlugin() override;
+Q_SIGNALS:
+	void senderStatsReceived(const nimbro_topic_transport::SenderStatsConstPtr& msg);
+	void receiverStatsReceived(const nimbro_topic_transport::ReceiverStatsConstPtr& msg);
+private Q_SLOTS:
+	void handleSenderStats(const nimbro_topic_transport::SenderStatsConstPtr& msg);
+	void handleReceiverStats(const nimbro_topic_transport::ReceiverStatsConstPtr& msg);
+	void update();
+private:
+	ros::Subscriber m_sub_senderStats;
+	ros::Subscriber m_sub_receiverStats;
+
+	std::map<ConnectionIdentifier, SenderStatsConstPtr> m_senderStats;
+	std::map<ConnectionIdentifier, ReceiverStatsConstPtr> m_receiverStats;
+
+	DotWidget* m_w;
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/le_value.h

@@ -0,0 +1,105 @@
+// Little-Endian value
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef LE_VALUE_H
+#define LE_VALUE_H
+
+#include <stdint.h>
+#include <endian.h>
+
+namespace nimbro_topic_transport
+{
+
+/**
+ * The LEValue template provides automatic endianness correction. The bytes
+ * in memory are always ordered little-endian.
+ **/
+template<int Width>
+class LEValue
+{
+};
+
+template<>
+class LEValue<1>
+{
+public:
+	typedef uint8_t Type;
+
+	inline operator uint8_t() const
+	{ return m_value; }
+
+	inline uint8_t operator()() const
+	{ return m_value; }
+
+	inline uint8_t operator=(uint8_t value)
+	{ return m_value = value; }
+private:
+	uint8_t m_value;
+} __attribute__((packed));
+
+template<>
+class LEValue<2>
+{
+public:
+	typedef uint16_t Type;
+
+	inline operator uint16_t() const
+	{ return le16toh(m_value); }
+
+	inline uint16_t operator()() const
+	{ return le16toh(m_value); }
+
+	inline uint16_t operator=(uint16_t value)
+	{
+		m_value = htole16(value);
+		return value;
+	}
+private:
+	uint16_t m_value;
+} __attribute__((packed));
+
+template<>
+class LEValue<4>
+{
+public:
+	typedef uint32_t Type;
+
+	inline operator uint32_t() const
+	{ return le32toh(m_value); }
+
+	inline uint32_t operator()() const
+	{ return le32toh(m_value); }
+
+	inline uint32_t operator=(uint32_t value)
+	{
+		m_value = htole32(value);
+		return value;
+	}
+private:
+	uint32_t m_value;
+} __attribute__((packed));
+
+template<>
+class LEValue<8>
+{
+public:
+	typedef uint64_t Type;
+
+	inline operator uint64_t() const
+	{ return le64toh(m_value); }
+
+	inline uint64_t operator()() const
+	{ return le64toh(m_value); }
+
+	inline uint64_t operator=(uint64_t value)
+	{
+		m_value = htole64(value);
+		return value;
+	}
+private:
+	uint64_t m_value;
+} __attribute__((packed));
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/tcp/tcp_packet.h

@@ -0,0 +1,28 @@
+// TCP packet definition
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TCP_PACKET_H
+#define TCP_PACKET_H
+
+#include "../le_value.h"
+
+namespace nimbro_topic_transport
+{
+
+enum TCPFlag
+{
+	TCP_FLAG_COMPRESSED = (1 << 0)
+};
+
+struct TCPHeader
+{
+	LEValue<2> topic_len;
+	LEValue<2> type_len;
+	LEValue<4> data_len;
+	LEValue<4> topic_md5sum[4];
+	LEValue<4> flags;
+} __attribute__((packed));
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/tcp/tcp_receiver.cpp

@@ -0,0 +1,409 @@
+// TCP receiver
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "tcp_receiver.h"
+
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include "tcp_packet.h"
+#include "../topic_info.h"
+#include <topic_tools/shape_shifter.h>
+
+#include <bzlib.h>
+
+#include <nimbro_topic_transport/CompressedMsg.h>
+
+namespace nimbro_topic_transport
+{
+
+static bool sureRead(int fd, void* dest, ssize_t size)
+{
+	uint8_t* destWPtr = (uint8_t*)dest;
+	while(size != 0)
+	{
+		ssize_t ret = read(fd, destWPtr, size);
+
+		if(ret < 0)
+		{
+			ROS_ERROR("Could not read(): %s", strerror(errno));
+			return false;
+		}
+
+		if(ret == 0)
+		{
+			// Client has closed connection (ignore silently)
+			return false;
+		}
+
+		size -= ret;
+		destWPtr += ret;
+	}
+
+	return true;
+}
+
+
+TCPReceiver::TCPReceiver()
+ : m_nh("~")
+ , m_receivedBytesInStatsInterval(0)
+{
+	m_fd = socket(AF_INET, SOCK_STREAM, 0);
+	if(m_fd < 0)
+	{
+		ROS_FATAL("Could not create socket: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	int port;
+	m_nh.param("port", port, 5050);
+
+	sockaddr_in addr;
+	addr.sin_family = AF_INET;
+	addr.sin_addr.s_addr = INADDR_ANY;
+	addr.sin_port = htons(port);
+
+	int on = 1;
+	if(setsockopt(m_fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) != 0)
+	{
+		ROS_FATAL("Could not enable SO_REUSEADDR: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	ROS_DEBUG("Binding to :%d", port);
+
+	if(bind(m_fd, (sockaddr*)&addr, sizeof(addr)) != 0)
+	{
+		ROS_FATAL("Could not bind socket: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	if(listen(m_fd, 10) != 0)
+	{
+		ROS_FATAL("Could not listen: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	m_nh.param("keep_compressed", m_keepCompressed, false);
+
+	char hostnameBuf[256];
+	gethostname(hostnameBuf, sizeof(hostnameBuf));
+	hostnameBuf[sizeof(hostnameBuf)-1] = 0;
+
+	m_stats.node = ros::this_node::getName();
+	m_stats.protocol = "TCP";
+	m_stats.host = hostnameBuf;
+	m_stats.local_port = port;
+	m_stats.fec = false;
+
+	m_nh.param("label", m_stats.label, std::string());
+
+	m_pub_stats = m_nh.advertise<ReceiverStats>("/network/receiver_stats", 1);
+	m_statsInterval = ros::WallDuration(2.0);
+	m_statsTimer = m_nh.createWallTimer(m_statsInterval,
+		boost::bind(&TCPReceiver::updateStats, this)
+	);
+
+	m_nh.param("topic_prefix", m_topicPrefix, std::string());
+}
+
+TCPReceiver::~TCPReceiver()
+{
+}
+
+void TCPReceiver::run()
+{
+	fd_set fds;
+
+	while(ros::ok())
+	{
+		ros::spinOnce();
+
+		// Clean up any exited threads
+		std::list<ClientHandler*>::iterator it = m_handlers.begin();
+		while(it != m_handlers.end())
+		{
+			if(!(*it)->isRunning())
+			{
+				delete *it;
+				it = m_handlers.erase(it);
+			}
+			else
+				it++;
+		}
+
+		FD_ZERO(&fds);
+		FD_SET(m_fd, &fds);
+
+		timeval timeout;
+		timeout.tv_usec = 0;
+		timeout.tv_sec = 1;
+
+		int ret = select(m_fd+1, &fds, 0, 0, &timeout);
+		if(ret < 0)
+		{
+			if(errno == EINTR || errno == EAGAIN)
+				continue;
+
+			ROS_ERROR("Could not select(): %s", strerror(errno));
+			throw std::runtime_error("Could not select");
+		}
+		if(ret == 0)
+			continue;
+
+		sockaddr_storage remoteAddr;
+		socklen_t remoteAddrLen = sizeof(remoteAddr);
+
+		int client_fd = accept(m_fd, (sockaddr*)&remoteAddr, &remoteAddrLen);
+
+		{
+			// Perform reverse lookup
+			char nameBuf[256];
+			char serviceBuf[256];
+
+			ros::WallTime startLookup = ros::WallTime::now();
+			if(getnameinfo((sockaddr*)&remoteAddr, remoteAddrLen, nameBuf, sizeof(nameBuf), serviceBuf, sizeof(serviceBuf), NI_NUMERICSERV) == 0)
+			{
+				ROS_INFO("New remote: %s:%s", nameBuf, serviceBuf);
+				m_stats.remote = nameBuf;
+				m_stats.remote_port = atoi(serviceBuf);
+			}
+			else
+			{
+				ROS_ERROR("Could not resolve remote address to name");
+				m_stats.remote = "unknown";
+				m_stats.remote_port = -1;
+			}
+			ros::WallTime endLookup = ros::WallTime::now();
+
+			// Warn if lookup takes up time (otherwise the user does not know
+			// what is going on)
+			if(endLookup - startLookup > ros::WallDuration(1.0))
+			{
+				ROS_WARN("Reverse address lookup took more than a second. "
+					"Consider adding '%s' to /etc/hosts",
+					m_stats.remote.c_str()
+				);
+			}
+		}
+
+		ClientHandler* handler = new ClientHandler(client_fd, m_topicPrefix);
+		handler->setKeepCompressed(m_keepCompressed);
+
+		m_handlers.push_back(handler);
+	}
+}
+
+TCPReceiver::ClientHandler::ClientHandler(int fd, const std::string& topicPrefix)
+ : m_fd(fd)
+ , m_uncompressBuf(1024)
+ , m_running(true)
+ , m_keepCompressed(false)
+ , m_topicPrefix(topicPrefix)
+{
+	m_thread = boost::thread(boost::bind(&ClientHandler::start, this));
+}
+
+TCPReceiver::ClientHandler::~ClientHandler()
+{
+	close(m_fd);
+}
+
+class VectorStream
+{
+public:
+	VectorStream(std::vector<uint8_t>* vector)
+	 : m_vector(vector)
+	{}
+
+	inline const uint8_t* getData()
+	{ return m_vector->data(); }
+
+	inline size_t getLength()
+	{ return m_vector->size(); }
+private:
+	std::vector<uint8_t>* m_vector;
+};
+
+void TCPReceiver::ClientHandler::start()
+{
+	run();
+	m_running = false;
+}
+
+void TCPReceiver::ClientHandler::run()
+{
+	while(1)
+	{
+		TCPHeader header;
+
+		if(!sureRead(m_fd, &header, sizeof(header)))
+			return;
+
+		std::vector<char> buf(header.topic_len + 1);
+		if(!sureRead(m_fd, buf.data(), header.topic_len))
+			return;
+		buf[buf.size()-1] = 0;
+
+		std::string topic(buf.data());
+
+		buf.resize(header.type_len+1);
+		if(!sureRead(m_fd, buf.data(), header.type_len))
+			return;
+		buf[buf.size()-1] = 0;
+
+		std::string type(buf.data());
+
+		std::string md5;
+		topic_info::unpackMD5(header.topic_md5sum, &md5);
+
+		std::vector<uint8_t> data(header.data_len);
+		if(!sureRead(m_fd, data.data(), header.data_len))
+			return;
+
+		ROS_DEBUG("Got msg with flags: %d", header.flags());
+
+		if(m_keepCompressed && (header.flags() & TCP_FLAG_COMPRESSED))
+		{
+			CompressedMsg compressed;
+			compressed.type = type;
+			memcpy(compressed.md5.data(), header.topic_md5sum, sizeof(header.topic_md5sum));
+			compressed.data.swap(data);
+
+			std::map<std::string, ros::Publisher>::iterator it = m_pub.find(topic);
+			if(it == m_pub.end())
+			{
+				ros::NodeHandle nh;
+				ros::Publisher pub = nh.advertise<CompressedMsg>(m_topicPrefix + topic, 2);
+				m_pub[topic] = pub;
+
+				pub.publish(compressed);
+			}
+			else
+				it->second.publish(compressed);
+		}
+		else
+		{
+			topic_tools::ShapeShifter shifter;
+
+			if(header.flags() & TCP_FLAG_COMPRESSED)
+			{
+				int ret = 0;
+				unsigned int len = m_uncompressBuf.size();
+
+				while(1)
+				{
+					ret = BZ2_bzBuffToBuffDecompress((char*)m_uncompressBuf.data(), &len, (char*)data.data(), data.size(), 0, 0);
+
+					if(ret == BZ_OUTBUFF_FULL)
+					{
+						len = 4 * m_uncompressBuf.size();
+						ROS_INFO("Increasing buffer size to %d KiB", (int)len / 1024);
+						m_uncompressBuf.resize(len);
+						continue;
+					}
+					else
+						break;
+				}
+
+				if(ret != BZ_OK)
+				{
+					ROS_ERROR("Could not decompress bz2 data (reason %d), dropping msg", ret);
+					continue;
+				}
+
+				ROS_DEBUG("decompress %d KiB (%d) to %d KiB (%d)", (int)data.size() / 1024, (int)data.size(), (int)len / 1024, (int)len);
+				m_uncompressBuf.resize(len);
+
+				VectorStream stream(&m_uncompressBuf);
+				shifter.read(stream);
+			}
+			else
+			{
+				VectorStream stream(&data);
+				shifter.read(stream);
+			}
+
+			ROS_DEBUG("Got message from topic '%s' (type '%s', md5 '%s')", topic.c_str(), type.c_str(), md5.c_str());
+
+			shifter.morph(md5, type, "", "");
+
+			std::map<std::string, ros::Publisher>::iterator it = m_pub.find(topic);
+			if(it == m_pub.end())
+			{
+				ROS_DEBUG("Advertising new topic '%s'", (m_topicPrefix + topic).c_str());
+				std::string msgDef = topic_info::getMsgDef(type);
+
+				ros::NodeHandle nh;
+
+				ros::AdvertiseOptions options(
+					m_topicPrefix + topic,
+					2,
+					md5,
+					type,
+					topic_info::getMsgDef(type)
+				);
+
+				// It will take subscribers some time to connect to our publisher.
+				// Therefore, latch messages so they will not be lost.
+				// No, this is often unexpected. Instead, wait before publishing.
+	// 			options.latch = true;
+
+				m_pub[topic] = nh.advertise(options);
+				it = m_pub.find(topic);
+
+				sleep(1);
+			}
+
+			it->second.publish(shifter);
+		}
+
+		uint8_t ack = 1;
+		if(write(m_fd, &ack, 1) != 1)
+		{
+			ROS_ERROR("Could not write(): %s", strerror(errno));
+			return;
+		}
+	}
+}
+
+bool TCPReceiver::ClientHandler::isRunning() const
+{
+	return m_running;
+}
+
+void TCPReceiver::updateStats()
+{
+	m_stats.header.stamp = ros::Time::now();
+
+	uint64_t totalBytes = 0;
+	for(auto handler : m_handlers)
+	{
+		totalBytes += handler->bytesReceived();
+		handler->resetByteCounter();
+	}
+
+	m_stats.bandwidth = totalBytes / m_statsInterval.toSec();
+
+	m_stats.drop_rate = 0;
+
+	// If there is no connection yet, drop the stats msg
+	if(m_handlers.empty())
+		return;
+
+	m_pub_stats.publish(m_stats);
+}
+
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "tcp_receiver");
+
+	nimbro_topic_transport::TCPReceiver recv;
+
+	recv.run();
+
+	return 0;
+}

software/ros_packages/nimbro_topic_transport/src/tcp/tcp_receiver.h

@@ -0,0 +1,72 @@
+// TCP receiver
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TCP_RECEIVER_H
+#define TCP_RECEIVER_H
+
+#include <list>
+#include <ros/node_handle.h>
+#include <boost/thread.hpp>
+
+#include <nimbro_topic_transport/ReceiverStats.h>
+
+namespace nimbro_topic_transport
+{
+
+class TCPReceiver
+{
+public:
+	TCPReceiver();
+	~TCPReceiver();
+
+	void run();
+private:
+	class ClientHandler
+	{
+	public:
+		ClientHandler(int fd, const std::string& topicPrefix);
+		~ClientHandler();
+		void start();
+		void run();
+
+		void setKeepCompressed(bool keep)
+		{ m_keepCompressed = keep; }
+
+		bool isRunning() const;
+
+		inline uint64_t bytesReceived() const
+		{ return m_bytesReceived; }
+
+		inline void resetByteCounter()
+		{ m_bytesReceived = 0; }
+	private:
+		int m_fd;
+		boost::thread m_thread;
+		std::map<std::string, ros::Publisher> m_pub;
+		std::vector<uint8_t> m_uncompressBuf;
+		bool m_running;
+		bool m_keepCompressed;
+		uint64_t m_bytesReceived;
+		std::string m_topicPrefix;
+	};
+
+	void updateStats();
+
+	int m_fd;
+	std::list<ClientHandler*> m_handlers;
+	ros::NodeHandle m_nh;
+
+	bool m_keepCompressed;
+
+	nimbro_topic_transport::ReceiverStats m_stats;
+	uint64_t m_receivedBytesInStatsInterval;
+	ros::Publisher m_pub_stats;
+	ros::WallDuration m_statsInterval;
+	ros::WallTimer m_statsTimer;
+
+	std::string m_topicPrefix;
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/tcp/tcp_sender.cpp

@@ -0,0 +1,406 @@
+// TCP sender
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "tcp_sender.h"
+#include "../topic_info.h"
+
+#include <bzlib.h>
+
+#include <netinet/tcp.h>
+#include <boost/algorithm/string/replace.hpp>
+
+#include "ros/message_traits.h"
+
+#include <netdb.h>
+
+namespace nimbro_topic_transport
+{
+
+TCPSender::TCPSender()
+ : m_nh("~")
+ , m_fd(-1)
+ , m_sentBytesInStatsInterval(0)
+{
+	std::string addr;
+	if(!m_nh.getParam("destination_addr", addr) && !m_nh.getParam("address", addr))
+	{
+		ROS_FATAL("tcp_sender needs an 'destination_addr' parameter!");
+		throw std::runtime_error("tcp_sender needs an 'destination_addr' parameter!");
+	}
+
+	int port;
+	if(!m_nh.getParam("destination_port", port) && !m_nh.getParam("port", port))
+	{
+		ROS_FATAL("tcp_sender needs a 'destination_port' parameter!");
+		throw std::runtime_error("tcp_sender needs a 'destination_port' parameter!");
+	}
+
+	std::string portStr = boost::lexical_cast<std::string>(port);
+
+	if(m_nh.hasParam("source_port"))
+	{
+		if(!m_nh.getParam("source_port", m_sourcePort))
+		{
+			ROS_FATAL("Invalid source_port");
+			throw std::runtime_error("Invalid source port");
+		}
+	}
+	else
+		m_sourcePort = -1;
+
+	addrinfo* info = 0;
+	if(getaddrinfo(addr.c_str(), portStr.c_str(), 0, &info) != 0 || !info)
+	{
+		ROS_FATAL("Could not lookup host name '%s'", addr.c_str());
+		throw std::runtime_error("Could not lookup hostname");
+	}
+
+	m_addrFamily = info->ai_family;
+	memcpy(&m_addr, info->ai_addr, info->ai_addrlen);
+	m_addrLen = info->ai_addrlen;
+
+	freeaddrinfo(info);
+
+	XmlRpc::XmlRpcValue list;
+	m_nh.getParam("topics", list);
+
+	ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+
+	for(int32_t i = 0; i < list.size(); ++i)
+	{
+		XmlRpc::XmlRpcValue& entry = list[i];
+
+		ROS_ASSERT(entry.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+		ROS_ASSERT(entry.hasMember("name"));
+
+		std::string topic = entry["name"];
+		int flags = 0;
+
+		if(entry.hasMember("compress") && ((bool)entry["compress"]) == true)
+			flags |= TCP_FLAG_COMPRESSED;
+
+		boost::function<void(const ros::MessageEvent<topic_tools::ShapeShifter const>&)> func;
+		func = boost::bind(&TCPSender::messageCallback, this, topic, flags, _1);
+
+		ros::SubscribeOptions options;
+		options.initByFullCallbackType<const ros::MessageEvent<topic_tools::ShapeShifter const>&>(topic, 20, func);
+
+		if(entry.hasMember("type"))
+		{
+			std::string type = entry["type"];
+
+			std::string md5 = topic_info::getMd5Sum(type);
+			if(md5.empty())
+			{
+				ROS_ERROR("Could not get md5 sum of topic type '%s'", type.c_str());
+				continue;
+			}
+
+			options.datatype = type;
+			options.md5sum = md5;
+		}
+
+		m_subs.push_back(m_nh.subscribe(options));
+
+#if WITH_CONFIG_SERVER
+		bool enabled = true;
+		if (entry.hasMember("enable"))
+			enabled = entry["enable"];
+
+		std::string parameterName(topic);
+		boost::replace_first(parameterName, "/", "");
+		boost::replace_all(parameterName, "/", "_");
+
+		boost::shared_ptr<config_server::Parameter<bool>> parameter( new config_server::Parameter<bool>(parameterName, enabled));
+		m_enableTopic[topic] = parameter;
+#endif
+	}
+
+	if (m_nh.hasParam("ignored_publishers")) {
+		m_nh.getParam("ignored_publishers", m_ignoredPubs);
+		for (std::vector<std::string>::iterator ignoredPublisher = m_ignoredPubs.begin();
+			 	ignoredPublisher != m_ignoredPubs.end(); ignoredPublisher++) {
+			*ignoredPublisher = ros::names::resolve(*ignoredPublisher);
+		}
+	}
+
+	char hostnameBuf[256];
+	gethostname(hostnameBuf, sizeof(hostnameBuf));
+	hostnameBuf[sizeof(hostnameBuf)-1] = 0;
+
+	m_stats.node = ros::this_node::getName();
+	m_stats.protocol = "TCP";
+	m_stats.host = hostnameBuf;
+	m_stats.destination = addr;
+	m_stats.destination_port = port;
+	m_stats.source_port = m_sourcePort;
+	m_stats.fec = false;
+
+	m_nh.param("label", m_stats.label, std::string());
+
+	m_pub_stats = m_nh.advertise<SenderStats>("/network/sender_stats", 1);
+
+	for(auto& pair : m_topicSendBytesInStatsInteral)
+		pair.second = 0;
+
+	m_statsInterval = ros::WallDuration(2.0);
+	m_statsTimer = m_nh.createWallTimer(m_statsInterval,
+		boost::bind(&TCPSender::updateStats, this)
+	);
+	m_statsTimer.start();
+}	
+
+TCPSender::~TCPSender()
+{
+}
+
+bool TCPSender::connect()
+{
+	m_fd = socket(m_addrFamily, SOCK_STREAM, 0);
+	if(m_fd < 0)
+	{
+		ROS_ERROR("Could not create socket: %s", strerror(errno));
+		return false;
+	}
+
+	if(m_sourcePort != -1)
+	{
+		std::string source_port_str = boost::lexical_cast<std::string>(m_sourcePort);
+
+		addrinfo hints;
+		memset(&hints, 0, sizeof(hints));
+
+		hints.ai_flags = AI_PASSIVE;
+		hints.ai_family = m_addrFamily;
+		hints.ai_socktype = SOCK_STREAM;
+
+		addrinfo* localInfo = 0;
+		if(getaddrinfo(NULL, source_port_str.c_str(), &hints, &localInfo) != 0 || !localInfo)
+		{
+			ROS_FATAL("Could not get local address: %s", strerror(errno));
+			throw std::runtime_error("Could not get local address");
+		}
+
+		if(bind(m_fd, localInfo->ai_addr, localInfo->ai_addrlen) != 0)
+		{
+			ROS_FATAL("Could not bind to source port: %s", strerror(errno));
+			throw std::runtime_error(strerror(errno));
+		}
+
+		freeaddrinfo(localInfo);
+	}
+
+	if(::connect(m_fd, (sockaddr*)&m_addr, m_addrLen) != 0)
+	{
+		ROS_ERROR("Could not connect: %s", strerror(errno));
+		return false;
+	}
+
+	if(m_sourcePort == -1)
+	{
+		sockaddr_storage addr;
+		socklen_t addrlen = sizeof(addr);
+
+		char nameBuf[256];
+		char serviceBuf[256];
+
+		if(getsockname(m_fd, (sockaddr*)&addr, &addrlen) != 0)
+		{
+			ROS_ERROR("Could not get local socket addr: %s", strerror(errno));
+		}
+
+		if(getnameinfo((sockaddr*)&addr, addrlen, nameBuf, sizeof(nameBuf), serviceBuf, sizeof(serviceBuf), NI_NUMERICSERV | NI_NUMERICHOST) != 0)
+		{
+			ROS_ERROR("Could not resolve remote address to name");
+		}
+
+		m_stats.source_port = atoi(serviceBuf);
+	}
+
+#ifdef TCP_USER_TIMEOUT
+	int timeout = 8000;
+	if(setsockopt(m_fd, SOL_TCP, TCP_USER_TIMEOUT, &timeout, sizeof(timeout)) != 0)
+	{
+		ROS_ERROR("Could not set TCP_USER_TIMEOUT: %s", strerror(errno));
+		return false;
+	}
+#else
+	ROS_WARN("Not setting TCP_USER_TIMEOUT");
+#endif
+
+	return true;
+}
+
+class PtrStream
+{
+public:
+	PtrStream(uint8_t* ptr)
+	 : m_ptr(ptr)
+	{}
+
+	inline uint8_t* advance(int)
+	{ return m_ptr; }
+private:
+	uint8_t* m_ptr;
+};
+
+void TCPSender::messageCallback(const std::string& topic, int flags,
+		const ros::MessageEvent<topic_tools::ShapeShifter const>& event)
+{
+#if WITH_CONFIG_SERVER
+	if (! (*m_enableTopic[topic])() )
+		return;
+#endif
+
+	// check if the message wasn't published by an ignored publisher
+	std::string const &messagePublisher = event.getConnectionHeader()["callerid"];
+	if (std::find(m_ignoredPubs.begin(), m_ignoredPubs.end(), messagePublisher) != m_ignoredPubs.end())
+		return;
+
+	send(topic, flags, event.getMessage());
+}
+
+
+void TCPSender::send(const std::string& topic, int flags, const topic_tools::ShapeShifter::ConstPtr& shifter)
+{
+#if WITH_CONFIG_SERVER
+	if (! (*m_enableTopic[topic])() )
+		return;
+#endif
+
+	std::string type = shifter->getDataType();
+	std::string md5 = shifter->getMD5Sum();
+	uint32_t size = shifter->size();
+
+	uint32_t maxDataSize = size;
+
+	if(flags & TCP_FLAG_COMPRESSED)
+		maxDataSize = size + size / 100 + 1200; // taken from bzip2 docs
+
+	m_packet.resize(
+		sizeof(TCPHeader) + topic.length() + type.length() + maxDataSize
+	);
+
+	TCPHeader* header = (TCPHeader*)m_packet.data();
+
+	// Fill in topic & type
+	uint8_t* wptr = m_packet.data() + sizeof(TCPHeader);
+
+	memcpy(wptr, topic.c_str(), topic.length());
+	wptr += topic.length();
+
+	memcpy(wptr, type.c_str(), type.length());
+	wptr += type.length();
+
+	if(flags & TCP_FLAG_COMPRESSED)
+	{
+		unsigned int len = m_packet.size() - (wptr - m_packet.data());
+
+		m_compressionBuf.resize(shifter->size());
+		PtrStream stream(m_compressionBuf.data());
+		shifter->write(stream);
+
+		if(BZ2_bzBuffToBuffCompress((char*)wptr, &len, (char*)m_compressionBuf.data(), m_compressionBuf.size(), 7, 0, 30) == BZ_OK)
+		{
+			header->data_len = len;
+			wptr += len;
+			size = len;
+		}
+		else
+		{
+			ROS_ERROR("Could not compress with bzip2 library, sending uncompressed");
+			flags &= ~TCP_FLAG_COMPRESSED;
+			memcpy(wptr, m_compressionBuf.data(), m_compressionBuf.size());
+			header->data_len = m_compressionBuf.size();
+			wptr += m_compressionBuf.size();
+		}
+	}
+	else
+	{
+		PtrStream stream(wptr);
+		shifter->write(stream);
+		header->data_len = size;
+		wptr += size;
+	}
+
+	header->topic_len = topic.length();
+	header->type_len = type.length();
+	header->data_len = size;
+	header->flags = flags;
+	topic_info::packMD5(md5, header->topic_md5sum);
+
+	// Resize to final size
+	m_packet.resize(
+		wptr - m_packet.data()
+	);
+
+	// Try to send the packet
+	for(int tries = 0; tries < 10; ++tries)
+	{
+		if(m_fd == -1)
+		{
+			if(!connect())
+			{
+				ROS_WARN("Connection failed, trying again");
+				continue;
+			}
+		}
+
+		if(write(m_fd, m_packet.data(), m_packet.size()) != (int)m_packet.size())
+		{
+			ROS_WARN("Could not send data, trying again");
+			close(m_fd);
+			m_fd = -1;
+			continue;
+		}
+		m_sentBytesInStatsInterval += m_packet.size();
+		m_topicSendBytesInStatsInteral[topic] += m_packet.size();
+
+		// Read ACK
+		uint8_t ack;
+		if(read(m_fd, &ack, 1) != 1)
+		{
+			ROS_WARN("Could not read ACK, sending again (!)");
+			close(m_fd);
+			m_fd = -1;
+			continue;
+		}
+
+		return;
+	}
+
+	ROS_ERROR("Could not send TCP packet. Dropping message from topic %s!", topic.c_str());
+}
+
+void TCPSender::updateStats()
+{
+	m_stats.header.stamp = ros::Time::now();
+	m_stats.bandwidth = 8 * m_sentBytesInStatsInterval / m_statsInterval.toSec();
+	m_stats.topics.clear();
+	for(auto& pair : m_topicSendBytesInStatsInteral)
+	{
+		nimbro_topic_transport::TopicBandwidth tp;
+		tp.name = pair.first;
+		tp.bandwidth = pair.second / m_statsInterval.toSec();
+		pair.second = 0;
+		m_stats.topics.push_back(tp);
+	}
+
+	m_pub_stats.publish(m_stats);
+	m_sentBytesInStatsInterval = 0;
+}
+
+}
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "tcp_sender");
+
+	nimbro_topic_transport::TCPSender sender;
+
+	ros::spin();
+
+	return 0;
+}
+

software/ros_packages/nimbro_topic_transport/src/tcp/tcp_sender.h

@@ -0,0 +1,67 @@
+// TCP sender
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TCP_SENDER_H
+#define TCP_SENDER_H
+
+#include <ros/node_handle.h>
+#include <topic_tools/shape_shifter.h>
+#include <arpa/inet.h>
+
+#include "tcp_packet.h"
+
+#if WITH_CONFIG_SERVER
+#include <config_server/parameter.h>
+#endif
+
+#include <map>
+
+#include "ros/message_event.h"
+
+#include <nimbro_topic_transport/SenderStats.h>
+
+namespace nimbro_topic_transport
+{
+
+class TCPSender
+{
+public:
+	TCPSender();
+	~TCPSender();
+
+	bool connect();
+
+	void send(const std::string& topic, int flags, const topic_tools::ShapeShifter::ConstPtr& shifter);
+	void messageCallback(const std::string& topic, int flags,
+		const ros::MessageEvent<topic_tools::ShapeShifter const>& shifter);
+private:
+	void updateStats();
+
+	ros::NodeHandle m_nh;
+	int m_fd;
+
+	int m_addrFamily;
+	sockaddr_storage m_addr;
+	socklen_t m_addrLen;
+
+	int m_sourcePort;
+	std::vector<ros::Subscriber> m_subs;
+	std::vector<uint8_t> m_packet;
+	std::vector<uint8_t> m_compressionBuf;
+	std::vector<std::string> m_ignoredPubs;
+
+#if WITH_CONFIG_SERVER
+	std::map<std::string, boost::shared_ptr<config_server::Parameter<bool>>> m_enableTopic;
+#endif
+
+	nimbro_topic_transport::SenderStats m_stats;
+	ros::Publisher m_pub_stats;
+	ros::WallDuration m_statsInterval;
+	ros::WallTimer m_statsTimer;
+	uint64_t m_sentBytesInStatsInterval;
+	std::map<std::string, uint64_t> m_topicSendBytesInStatsInteral;
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/topic_info.cpp

@@ -0,0 +1,124 @@
+// Provides information about topic types
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "topic_info.h"
+
+#include <ros/names.h>
+
+#include <sys/wait.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <string>
+
+namespace nimbro_topic_transport
+{
+
+namespace topic_info
+{
+
+/**
+ * Execute a rosmsg query and return result.
+ *
+ * The executed command is "rosmsg @a cmd @a type".
+ *
+ * @param stripNL If true, strip off the final newline.
+ * @return rosmsg output
+ **/
+static std::string msgQuery(const std::string& cmd, const std::string& type, bool stripNL)
+{
+	std::vector<char> buf(1024);
+	int idx = 0;
+
+	std::string error;
+	if(!ros::names::validate(type, error))
+	{
+		ROS_WARN("Got invalid message type '%s'", type.c_str());
+		return "";
+	}
+
+	int fds[2];
+	if(pipe(fds) != 0)
+		throw std::runtime_error("Could not create pipe");
+
+	int pid = fork();
+
+	if(pid == 0)
+	{
+		close(fds[0]);
+		dup2(fds[1], STDOUT_FILENO);
+
+		if(execlp("rosmsg", "rosmsg", cmd.c_str(), type.c_str(), 0) != 0)
+		{
+			throw std::runtime_error("Could not execlp() rosmsg");
+		}
+	}
+
+	close(fds[1]);
+
+	while(1)
+	{
+		buf.resize(idx + 1024);
+		int size = read(fds[0], buf.data() + idx, 1024);
+
+		if(size < 0)
+			throw std::runtime_error("Could not read()");
+		else if(size == 0)
+			break;
+
+		idx += size;
+	}
+
+	close(fds[0]);
+
+	int status;
+	waitpid(pid, &status, 0);
+
+	if(!WIFEXITED(status) || WEXITSTATUS(status) != 0 || idx == 0)
+	{
+		ROS_WARN("Could not get rosmsg %s for type '%s'", cmd.c_str(), type.c_str());
+		return "";
+	}
+
+	std::string ret(buf.data(), idx);
+
+	if(stripNL)
+		return std::string(buf.data(), idx-1);
+	else
+		return std::string(buf.data(), idx);
+}
+
+std::string getMsgDef(const std::string& type)
+{
+	return msgQuery("show", type, false);
+}
+
+std::string getMd5Sum(const std::string& type)
+{
+	return msgQuery("md5", type, true);
+}
+
+void packMD5(const std::string& str, LEValue< 4 >* dest)
+{
+	for(int i = 0; i < 4; ++i)
+	{
+		std::string md5_part = str.substr(8*i, 8);
+		uint32_t md5_num = strtoll(md5_part.c_str(), 0, 16);
+		dest[i] = md5_num;
+	}
+}
+
+void unpackMD5(const LEValue< 4 >* src, std::string* dest)
+{
+	dest->clear();
+	for(int i = 0; i < 4; ++i)
+	{
+		char buf[10];
+		snprintf(buf, sizeof(buf), "%08x", src[i]());
+		*dest += buf;
+	}
+}
+
+}
+
+}

software/ros_packages/nimbro_topic_transport/src/topic_info.h

@@ -0,0 +1,45 @@
+// Provides information about topic types
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TOPIC_INFO_H
+#define TOPIC_INFO_H
+
+#include "le_value.h"
+
+#include <string>
+
+namespace nimbro_topic_transport
+{
+
+namespace topic_info
+{
+
+/**
+ * Get the message definition string (from the .msg file)
+ *
+ * @return msg definition, or empty on failure (unknown type, etc)
+ **/
+std::string getMsgDef(const std::string& type);
+
+/**
+ * Get the message md5 string
+ *
+ * @return md5 sum (32 chars), or empty on failure (unknown type, etc)
+ **/
+std::string getMd5Sum(const std::string& type);
+
+/**
+ * md5 string -> LEValue<4>
+ **/
+void packMD5(const std::string& str, LEValue<4>* dest);
+
+/**
+ * LEValue<4> -> md5 string
+ **/
+void unpackMD5(const LEValue<4>* src, std::string* dest);
+
+}
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/udp/topic_receiver.cpp

@@ -0,0 +1,166 @@
+// Topic receiver (part of udp_receiver)
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "topic_receiver.h"
+
+#include <bzlib.h>
+
+namespace nimbro_topic_transport
+{
+
+bool Message::decompress(Message* dest)
+{
+	unsigned int destLen = 1024;
+	dest->payload.resize(destLen);
+
+	while(1)
+	{
+		int ret = BZ2_bzBuffToBuffDecompress((char*)dest->payload.data(), &destLen, (char*)payload.data(), payload.size(), 0, 0);
+
+		if(ret == BZ_OUTBUFF_FULL)
+		{
+			destLen *= 2;
+			dest->payload.resize(destLen);
+			continue;
+		}
+
+		if(ret != BZ_OK)
+		{
+			ROS_ERROR("Could not decompress message");
+			return false;
+		}
+
+		break;
+	}
+
+	dest->payload.resize(destLen);
+	dest->header = header;
+	dest->id = id;
+	dest->size = destLen;
+	return true;
+}
+
+TopicReceiver::TopicReceiver()
+ : waiting_for_subscriber(true)
+ , m_decompressionThreadRunning(false)
+{
+}
+
+TopicReceiver::~TopicReceiver()
+{
+	{
+		boost::unique_lock<boost::mutex> lock(m_mutex);
+		m_decompressionThreadShouldExit = true;
+	}
+	m_cond.notify_one();
+}
+
+void TopicReceiver::takeForDecompression(const boost::shared_ptr<Message>& msg)
+{
+	if(!m_decompressionThreadRunning)
+	{
+		m_decompressionThreadShouldExit = false;
+		m_decompressionThread = boost::thread(boost::bind(&TopicReceiver::decompress, this));
+		m_decompressionThreadRunning = true;
+	}
+
+	boost::unique_lock<boost::mutex> lock(m_mutex);
+	m_compressedMsg = msg;
+	m_cond.notify_one();
+}
+
+void TopicReceiver::decompress()
+{
+	while(1)
+	{
+		boost::shared_ptr<Message> currentMessage;
+
+		{
+			boost::unique_lock<boost::mutex> lock(m_mutex);
+
+			while(!m_compressedMsg && !m_decompressionThreadShouldExit)
+				m_cond.wait(lock);
+
+			if(m_decompressionThreadShouldExit)
+				return;
+
+			currentMessage = m_compressedMsg;
+			m_compressedMsg.reset();
+		}
+
+		Message decompressed;
+		currentMessage->decompress(&decompressed);
+
+		boost::shared_ptr<topic_tools::ShapeShifter> shapeShifter(new topic_tools::ShapeShifter);
+		shapeShifter->morph(md5_str, decompressed.header.topic_type, msg_def, "");
+
+		shapeShifter->read(decompressed);
+
+		publish(shapeShifter);
+	}
+}
+
+void TopicReceiver::publish(const boost::shared_ptr<topic_tools::ShapeShifter>& msg)
+{
+	if(!waiting_for_subscriber)
+	{
+		publisher.publish(msg);
+	}
+	else
+	{
+		m_msgInQueue = msg;
+		m_queueTime = ros::WallTime::now();
+	}
+}
+
+void TopicReceiver::publishCompressed(const CompressedMsgConstPtr& msg)
+{
+	if(!waiting_for_subscriber)
+	{
+		publisher.publish(msg);
+	}
+	else
+	{
+		m_compressedMsgInQueue = msg;
+		m_queueTime = ros::WallTime::now();
+	}
+}
+
+void TopicReceiver::handleSubscriber()
+{
+	if(!waiting_for_subscriber)
+		return;
+
+	if(!m_compressedMsg && !m_msgInQueue)
+	{
+		// No msg arrived before the first subscriber
+		waiting_for_subscriber = false;
+		return;
+	}
+
+	ros::WallTime now = ros::WallTime::now();
+	if(now - m_queueTime > ros::WallDuration(1.0))
+	{
+		// Message to old, drop it
+		waiting_for_subscriber = false;
+		m_msgInQueue.reset();
+		m_compressedMsgInQueue.reset();
+		return;
+	}
+
+	if(m_msgInQueue)
+	{
+		publisher.publish(m_msgInQueue);
+		m_msgInQueue.reset();
+	}
+	else if(m_compressedMsgInQueue)
+	{
+		publisher.publish(m_compressedMsgInQueue);
+		m_compressedMsgInQueue.reset();
+	}
+
+	waiting_for_subscriber = false;
+}
+
+}
+

software/ros_packages/nimbro_topic_transport/src/udp/topic_receiver.h

@@ -0,0 +1,111 @@
+// Topic receiver (part of udp_receiver)
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TOPIC_RECEIVER_H
+#define TOPIC_RECEIVER_H
+
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <vector>
+
+#include <ros/publisher.h>
+
+#include <topic_tools/shape_shifter.h>
+
+#include <nimbro_topic_transport/CompressedMsg.h>
+
+#include <boost/thread.hpp>
+
+#if WITH_OPENFEC
+extern "C"
+{
+#include <of_openfec_api.h>
+}
+#endif
+
+#include "udp_packet.h"
+
+namespace nimbro_topic_transport
+{
+
+struct Message
+{
+	Message(uint16_t id)
+	 : id(id)
+	 , size(0)
+	 , complete(false)
+	{}
+
+	Message()
+	{}
+
+	~Message()
+	{
+	}
+
+	uint32_t getLength() const
+	{ return size; }
+
+	uint8_t* getData()
+	{ return payload.data(); }
+
+	bool decompress(Message* dest);
+
+	uint16_t id;
+	UDPFirstPacket::Header header;
+	std::vector<uint8_t> payload;
+	size_t size;
+	std::vector<bool> msgs;
+
+	bool complete;
+
+#if WITH_OPENFEC
+	boost::shared_ptr<of_session_t> decoder;
+	boost::shared_ptr<of_parameters_t> params;
+	unsigned int received_symbols;
+	std::vector<boost::shared_ptr<std::vector<uint8_t>>> fecPackets;
+#endif
+};
+
+struct TopicReceiver
+{
+	TopicReceiver();
+	~TopicReceiver();
+
+	ros::Publisher publisher;
+
+	uint32_t md5[4];
+	std::string md5_str;
+	std::string msg_def;
+
+	bool compressed;
+
+	int last_message_counter;
+
+	bool waiting_for_subscriber;
+
+	void takeForDecompression(const boost::shared_ptr<Message>& compressed);
+
+	void publish(const boost::shared_ptr<topic_tools::ShapeShifter>& msg);
+	void publishCompressed(const CompressedMsgConstPtr& msg);
+	void handleSubscriber();
+private:
+	boost::shared_ptr<Message> m_compressedMsg;
+	boost::condition_variable m_cond;
+	boost::mutex m_mutex;
+
+	boost::thread m_decompressionThread;
+	bool m_decompressionThreadRunning;
+	bool m_decompressionThreadShouldExit;
+
+	boost::shared_ptr<topic_tools::ShapeShifter> m_msgInQueue;
+	CompressedMsgConstPtr m_compressedMsgInQueue;
+	ros::WallTime m_queueTime;
+
+	void decompress();
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/udp/topic_sender.cpp

@@ -0,0 +1,476 @@
+// Sends a single topic
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "topic_sender.h"
+#include "udp_sender.h"
+#include "udp_packet.h"
+#include "../topic_info.h"
+
+#include <bzlib.h>
+
+#include <nimbro_topic_transport/CompressedMsg.h>
+
+#include <boost/algorithm/string/replace.hpp>
+
+#include <random>
+#include <algorithm>
+#include <chrono>
+
+#if WITH_OPENFEC
+extern "C"
+{
+#include <of_openfec_api.h>
+}
+#endif
+
+#include <fcntl.h>
+
+namespace nimbro_topic_transport
+{
+
+TopicSender::TopicSender(UDPSender* sender, ros::NodeHandle* nh, const std::string& topic, double rate, bool resend, int flags, bool enable, const std::string& type)
+ : m_sender(sender)
+ , m_flags(flags)
+ , m_updateBuf(true)
+ , m_msgCounter(0)
+ , m_inputMsgCounter(0)
+ , m_directTransmission(true)
+#if WITH_CONFIG_SERVER
+ , m_enable(escapeTopicName(topic), enable)
+#endif
+{
+	ros::SubscribeOptions ops;
+	boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> func = boost::bind(&TopicSender::handleData, this, _1);
+	ops.initByFullCallbackType(topic, 20, func);
+
+	if(!type.empty())
+	{
+		ops.datatype = type;
+		ops.md5sum = topic_info::getMd5Sum(type);
+	}
+
+	m_subscriber = nh->subscribe(ops);
+	m_topicName = topic;
+
+	if(rate == 0.0)
+		m_durationBetweenPackets = ros::Duration(0.0);
+	else
+		m_durationBetweenPackets = ros::Duration(1.0 / rate);
+
+	if(resend)
+	{
+		m_resendTimer = nh->createTimer(m_durationBetweenPackets, boost::bind(&TopicSender::resend, this));
+		m_resendTimer.start();
+	}
+}
+
+TopicSender::~TopicSender()
+{
+	ROS_DEBUG("Topic '%s': Sent %d messages", m_topicName.c_str(), m_msgCounter);
+}
+
+void TopicSender::send()
+{
+	if(m_updateBuf)
+	{
+		boost::lock_guard<boost::mutex> lock(m_dataMutex);
+
+		if(!m_lastData)
+			return;
+
+		// If the data was sent over our CompressedMsg format, do not recompress it
+		if((m_flags & UDP_FLAG_COMPRESSED) && m_lastData->getDataType() == "nimbro_topic_transport/CompressedMsg")
+		{
+			CompressedMsg::Ptr compressed = m_lastData->instantiate<CompressedMsg>();
+			if(!compressed)
+			{
+				ROS_ERROR("Could not instantiate CompressedMsg");
+				return;
+			}
+
+			m_buf.swap(compressed->data);
+			memcpy(m_md5, compressed->md5.data(), sizeof(m_md5));
+			m_topicType = compressed->type;
+		}
+		else
+		{
+			m_buf.resize(m_lastData->size());
+			m_lastData->write(*this);
+
+			if(m_flags & UDP_FLAG_COMPRESSED)
+			{
+				unsigned int len = m_buf.size() + m_buf.size() / 100 + 1200;
+				m_compressionBuf.resize(len);
+				int ret = BZ2_bzBuffToBuffCompress((char*)m_compressionBuf.data(), &len, (char*)m_buf.data(), m_buf.size(), 3, 0, 30);
+				if(ret == BZ_OK)
+				{
+					m_buf.swap(m_compressionBuf);
+					m_buf.resize(len);
+				}
+				else
+				{
+					ROS_ERROR("Could not compress data, sending uncompressed");
+				}
+			}
+
+			std::string md5 = m_lastData->getMD5Sum();
+			for(int i = 0; i < 4; ++i)
+			{
+				std::string md5_part = md5.substr(8*i, 8);
+				uint32_t md5_num = strtoll(md5_part.c_str(), 0, 16);
+				m_md5[i] = md5_num;
+			}
+
+			m_topicType = m_lastData->getDataType();
+		}
+
+		m_updateBuf = false;
+	}
+
+	// Do we want to do forward error correction?
+	if(m_sender->fec() != 0.0)
+	{
+		sendWithFEC();
+	}
+	else
+	{
+		sendWithoutFEC();
+	}
+
+	m_msgCounter++;
+}
+
+inline uint64_t div_round_up(uint64_t a, uint64_t b)
+{
+	return (a + b - 1) / b;
+}
+
+void TopicSender::sendWithFEC()
+{
+#if WITH_OPENFEC
+	uint16_t msg_id = m_sender->allocateMessageID();
+	uint64_t dataSize = sizeof(FECHeader) + m_buf.size();
+
+	// If the message fits in a single packet, use that as the buffer size
+	uint64_t symbolSize;
+	uint64_t sourceSymbols;
+
+	if(dataSize <= FECPacket::MaxDataSize)
+	{
+		sourceSymbols = 1;
+		symbolSize = dataSize;
+	}
+	else
+	{
+		// We need to pad the data to a multiple of our packet payload size.
+		sourceSymbols = div_round_up(dataSize, FECPacket::MaxDataSize);
+		symbolSize = FECPacket::MaxDataSize;
+	}
+
+	ROS_DEBUG("dataSize: %lu, symbol size: %lu, sourceSymbols: %lu", dataSize, symbolSize, sourceSymbols);
+
+	uint64_t packetSize = sizeof(FECPacket::Header) + symbolSize;
+
+	ROS_DEBUG("=> packetSize: %lu", packetSize);
+
+	uint64_t repairSymbols = std::ceil(m_sender->fec() * sourceSymbols);
+
+	uint64_t numPackets = sourceSymbols + repairSymbols;
+
+	of_session_t* ses = 0;
+	uint32_t prng_seed = rand();
+	if(sourceSymbols >= MIN_PACKETS_LDPC)
+	{
+		ROS_DEBUG("%s: Choosing LDPC-Staircase codec", m_topicName.c_str());
+
+		if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_ENCODER, 1) != OF_STATUS_OK)
+		{
+			ROS_ERROR("%s: Could not create LDPC codec instance", m_topicName.c_str());
+			return;
+		}
+
+		of_ldpc_parameters_t params;
+		params.nb_source_symbols = sourceSymbols;
+		params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
+		params.encoding_symbol_length = symbolSize;
+		params.prng_seed = prng_seed;
+		params.N1 = 7;
+
+		ROS_DEBUG("LDPC seed: 7, 0x%X", params.prng_seed);
+
+		if(of_set_fec_parameters(ses, (of_parameters_t*)&params) != OF_STATUS_OK)
+		{
+			ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
+			of_release_codec_instance(ses);
+			return;
+		}
+	}
+	else
+	{
+		ROS_DEBUG("%s: Choosing Reed-Solomon codec", m_topicName.c_str());
+
+		if(of_create_codec_instance(&ses, OF_CODEC_REED_SOLOMON_GF_2_M_STABLE, OF_ENCODER, 0) != OF_STATUS_OK)
+		{
+			ROS_ERROR("%s: Could not create REED_SOLOMON codec instance", m_topicName.c_str());
+			return;
+		}
+
+		of_rs_2_m_parameters params;
+		params.nb_source_symbols = sourceSymbols;
+		params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
+		params.encoding_symbol_length = symbolSize;
+		params.m = 8;
+
+		if(of_set_fec_parameters(ses, (of_parameters_t*)&params) != OF_STATUS_OK)
+		{
+			ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
+			of_release_codec_instance(ses);
+			return;
+		}
+	}
+
+	std::vector<uint8_t> packetBuffer(numPackets * packetSize);
+	std::vector<void*> symbols(sourceSymbols + repairSymbols);
+
+	uint64_t writtenData = 0;
+
+	// Fill the source packets
+	for(uint64_t i = 0; i < sourceSymbols; ++i)
+	{
+		uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
+
+		FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
+
+		header->msg_id = msg_id;
+		header->symbol_id = i;
+		header->symbol_length = symbolSize;
+		header->source_symbols = sourceSymbols;
+		header->repair_symbols = repairSymbols;
+		header->prng_seed = prng_seed;
+
+		uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
+		uint64_t remainingSpace = symbolSize;
+
+		symbols[i] = dataPtr;
+
+		if(i == 0)
+		{
+			// First packet includes the FECHeader
+			FECHeader* msgHeader = reinterpret_cast<FECHeader*>(dataPtr);
+
+			// Fill in header fields
+			msgHeader->flags = m_flags;
+			msgHeader->topic_msg_counter = m_inputMsgCounter;
+
+			strncpy(msgHeader->topic_name, m_topicName.c_str(), sizeof(msgHeader->topic_name));
+			if(msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] != 0)
+			{
+				ROS_ERROR("Topic '%s' is too long. Please shorten the name.", m_topicName.c_str());
+				msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] = 0;
+			}
+
+			strncpy(msgHeader->topic_type, m_topicType.c_str(), sizeof(msgHeader->topic_type));
+			if(msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] != 0)
+			{
+				ROS_ERROR("Topic type '%s' is too long. Please shorten the name.", m_topicType.c_str());
+				msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] = 0;
+			}
+
+			for(int i = 0; i < 4; ++i)
+				msgHeader->topic_md5[i] = m_md5[i];
+
+			dataPtr += sizeof(FECHeader);
+			remainingSpace -= sizeof(FECHeader);
+		}
+
+		uint64_t chunkSize = std::min(remainingSpace, m_buf.size() - writtenData);
+		memcpy(dataPtr, m_buf.data() + writtenData, chunkSize);
+		writtenData += chunkSize;
+
+		// Set any padding to zero
+		if(chunkSize < remainingSpace)
+			memset(dataPtr + chunkSize, 0, remainingSpace - chunkSize);
+	}
+
+	// Fill the repair packets
+	for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
+	{
+		uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
+
+		FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
+
+		header->msg_id = msg_id;
+		header->symbol_id = i;
+		header->symbol_length = symbolSize;
+		header->source_symbols = sourceSymbols;
+		header->repair_symbols = repairSymbols;
+		header->prng_seed = prng_seed;
+
+		uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
+		symbols[i] = dataPtr;
+	}
+	for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
+	{
+		if(of_build_repair_symbol(ses, symbols.data(), i) != OF_STATUS_OK)
+		{
+			ROS_ERROR("%s: Could not build repair symbol", m_topicName.c_str());
+			of_release_codec_instance(ses);
+			return;
+		}
+	}
+
+	// FEC work is done
+	of_release_codec_instance(ses);
+
+	std::vector<unsigned int> packetOrder(numPackets);
+	std::iota(packetOrder.begin(), packetOrder.end(), 0);
+
+	// Send the packets in random order
+	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
+	std::mt19937 mt(seed);
+	std::shuffle(packetOrder.begin(), packetOrder.end(), mt);
+
+	ROS_DEBUG("Sending %d packets", (int)packetOrder.size());
+	for(unsigned int idx : packetOrder)
+	{
+		if(!m_sender->send(packetBuffer.data() + idx * packetSize, packetSize, m_topicName))
+			return;
+	}
+#else
+	throw std::runtime_error("Forward error correction requested, but I was not compiled with FEC support...");
+#endif
+}
+
+void TopicSender::sendWithoutFEC()
+{
+	uint32_t size = m_buf.size();
+
+	uint8_t buf[PACKET_SIZE];
+	uint32_t buf_size = std::min<uint32_t>(PACKET_SIZE, sizeof(UDPFirstPacket) + size);
+	UDPFirstPacket* first = (UDPFirstPacket*)buf;
+
+	uint16_t msg_id = m_sender->allocateMessageID();
+
+	first->header.frag_id = 0;
+	first->header.msg_id = msg_id;
+	first->header.flags = m_flags;
+	first->header.topic_msg_counter = m_inputMsgCounter;
+
+	// Calculate number of packets
+	first->header.remaining_packets = std::max<uint32_t>(0,
+		(size - UDPFirstPacket::MaxDataSize + (UDPDataPacket::MaxDataSize-1)) / UDPDataPacket::MaxDataSize
+	);
+
+	strncpy(first->header.topic_name, m_topicName.c_str(), sizeof(first->header.topic_name));
+	if(first->header.topic_name[sizeof(first->header.topic_name)-1] != 0)
+	{
+		ROS_ERROR("Topic '%s' is too long. Please shorten the name.", m_topicName.c_str());
+		first->header.topic_name[sizeof(first->header.topic_name)-1] = 0;
+	}
+
+	strncpy(first->header.topic_type, m_topicType.c_str(), sizeof(first->header.topic_type));
+	if(first->header.topic_type[sizeof(first->header.topic_type)-1] != 0)
+	{
+		ROS_ERROR("Topic type '%s' is too long. Please shorten the name.", m_topicType.c_str());
+		first->header.topic_type[sizeof(first->header.topic_type)-1] = 0;
+	}
+
+	for(int i = 0; i < 4; ++i)
+		first->header.topic_md5[i] = m_md5[i];
+
+	uint8_t* rptr = m_buf.data();
+	uint32_t psize = std::min<uint32_t>(UDPFirstPacket::MaxDataSize, size);
+	memcpy(first->data, rptr, psize);
+	rptr += psize;
+	size -= psize;
+
+	if(!m_sender->send(buf, buf_size, m_topicName))
+		return;
+
+	if(m_sender->duplicateFirstPacket())
+	{
+		if(!m_sender->send(buf, buf_size, m_topicName))
+			return;
+	}
+
+	uint16_t frag_id = 1;
+	while(size > 0)
+	{
+		buf_size = std::min<uint32_t>(PACKET_SIZE, sizeof(UDPDataPacket) + size);
+		UDPDataPacket* next_packet = (UDPDataPacket*)buf;
+		next_packet->header.frag_id = frag_id++;
+		next_packet->header.msg_id = msg_id;
+
+		psize = std::min<uint32_t>(UDPDataPacket::MaxDataSize, size);
+		memcpy(next_packet->data, rptr, psize);
+		rptr += psize;
+		size -= psize;
+
+		if(!m_sender->send(buf, buf_size, m_topicName))
+			return;
+	}
+}
+
+void TopicSender::handleData(const topic_tools::ShapeShifter::ConstPtr& shapeShifter)
+{
+#if WITH_CONFIG_SERVER
+	if (!m_enable() ) 
+		return;
+#endif
+
+	{
+		boost::lock_guard<boost::mutex> lock(m_dataMutex);
+
+		m_lastData = shapeShifter;
+		m_updateBuf = true;
+
+		ros::Time now = ros::Time::now();
+		if(now - m_lastTime < m_durationBetweenPackets)
+			return;
+
+		m_lastTime = now;
+		m_inputMsgCounter++;
+	}
+
+	if(m_directTransmission)
+		send();
+}
+
+void TopicSender::resend()
+{
+	if(!m_lastData)
+		return;
+
+	ros::Time now = ros::Time::now();
+	if(now - m_lastTime < m_durationBetweenPackets)
+		return;
+
+	sendCurrentMessage();
+}
+
+void TopicSender::sendCurrentMessage()
+{
+	if(!m_lastData)
+		return;
+
+	send();
+}
+
+void TopicSender::setDirectTransmissionEnabled(bool value)
+{
+	m_directTransmission = value;
+	if(m_directTransmission && m_resendTimer.isValid())
+		m_resendTimer.start();
+	else
+		m_resendTimer.stop();
+}
+
+
+std::string TopicSender::escapeTopicName(std::string topicName)
+{
+	boost::replace_first(topicName, "/", "");
+	boost::replace_all(topicName, "/", "_");
+	return topicName;
+}
+
+}

software/ros_packages/nimbro_topic_transport/src/udp/topic_sender.h

@@ -0,0 +1,73 @@
+// Sends a single topic
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef TOPIC_SENDER_H
+#define TOPIC_SENDER_H
+
+#include <ros/subscriber.h>
+#include <topic_tools/shape_shifter.h>
+
+#include <boost/thread.hpp>
+
+#if WITH_CONFIG_SERVER
+#include <config_server/parameter.h>
+#endif
+
+namespace nimbro_topic_transport
+{
+	class UDPSender;
+
+	class TopicSender
+	{
+	public:
+		TopicSender(UDPSender* sender, ros::NodeHandle* nh, const std::string& topic, double rate, bool resend, int flags, bool enable = true, const std::string& type = "");
+		~TopicSender();
+
+		void handleData(const topic_tools::ShapeShifter::ConstPtr& shapeShifter);
+
+		inline uint8_t* advance(int)
+		{ return m_buf.data(); }
+
+		bool isDirectTransmissionEnabled() const
+		{ return m_directTransmission; }
+		void setDirectTransmissionEnabled(bool value);
+
+		void sendCurrentMessage();
+	private:
+		void send();
+		void resend();
+
+		void sendWithFEC();
+		void sendWithoutFEC();
+
+		static std::string escapeTopicName(std::string topicName);
+
+		UDPSender* m_sender;
+		ros::Subscriber m_subscriber;
+		std::string m_topicName;
+		std::string m_topicType;
+		int m_flags;
+		std::vector<uint8_t> m_buf;
+		uint32_t m_md5[4];
+
+		std::vector<uint8_t> m_compressionBuf;
+		ros::Duration m_durationBetweenPackets;
+		ros::Time m_lastTime;
+		ros::Timer m_resendTimer;
+		topic_tools::ShapeShifter::ConstPtr m_lastData;
+		bool m_updateBuf;
+		unsigned int m_msgCounter;
+		unsigned int m_inputMsgCounter;
+
+		bool m_directTransmission;
+
+		boost::mutex m_dataMutex;
+
+#if WITH_CONFIG_SERVER
+		config_server::Parameter<bool> m_enable;
+#endif
+	};
+
+};
+
+#endif

software/ros_packages/nimbro_topic_transport/src/udp/udp_packet.h

@@ -0,0 +1,104 @@
+// UDP packet definition
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef UDP_PACKET_H
+#define UDP_PACKET_H
+
+#include "../le_value.h"
+
+namespace nimbro_topic_transport
+{
+
+// Since we sometimes tunnel nimbro_network packets through QuickTun,
+// leave space for one additional UDP/IP envelope.
+const int PACKET_SIZE = 1500 - 20 - 8 - 20 - 8;
+
+// const int PACKET_SIZE = 1024 + 12; // for symbol size == 1024
+
+enum PacketType
+{
+	PACKET_INFO,
+	PACKET_DATA
+};
+
+enum UDPFlag
+{
+	UDP_FLAG_COMPRESSED = (1 << 0),
+	UDP_FLAG_RELAY_MODE = (1 << 1)
+};
+
+struct UDPGenericPacket
+{
+	LEValue<2> frag_id;
+	LEValue<2> msg_id;
+} __attribute__((packed));
+
+struct UDPFirstPacket
+{
+	struct Header
+	{
+		LEValue<2> frag_id;
+		LEValue<2> msg_id;
+		char topic_name[64];
+		char topic_type[64];
+		LEValue<4> topic_md5[4];
+		LEValue<2> remaining_packets;
+		LEValue<2> flags;
+		LEValue<2> topic_msg_counter;
+	} __attribute__((packed));
+
+	enum { MaxDataSize = PACKET_SIZE - sizeof(Header) };
+
+	Header header;
+	uint8_t data[];
+} __attribute__((packed));
+
+struct UDPDataPacket
+{
+	struct Header
+	{
+		LEValue<2> frag_id;
+		LEValue<2> msg_id;
+	} __attribute__((packed));
+
+	enum { MaxDataSize = PACKET_SIZE - sizeof(Header) };
+
+	Header header;
+	uint8_t data[];
+} __attribute__((packed));
+
+// Minimum number of packets for choosing the LDPC-Staircase algorithm
+const int MIN_PACKETS_LDPC = 255;
+
+struct FECHeader
+{
+	char topic_name[64];
+	char topic_type[64];
+	LEValue<4> topic_md5[4];
+	LEValue<2> flags;
+	LEValue<2> topic_msg_counter;
+
+	uint8_t data[];
+} __attribute__((packed));
+
+struct FECPacket
+{
+	struct Header
+	{
+		LEValue<2> msg_id;
+		LEValue<4> symbol_id;
+		LEValue<2> symbol_length;
+		LEValue<2> source_symbols;
+		LEValue<2> repair_symbols;
+		LEValue<4> prng_seed;
+	} __attribute__((packed));
+
+	enum { MaxDataSize = PACKET_SIZE - sizeof(Header) };
+
+	Header header;
+	uint8_t data[];
+} __attribute__((packed));
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/udp/udp_receiver.cpp

@@ -0,0 +1,715 @@
+// UDP receiver node
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "udp_receiver.h"
+#include "udp_packet.h"
+
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <ros/console.h>
+#include <ros/node_handle.h>
+
+#include <topic_tools/shape_shifter.h>
+
+#include <std_msgs/Time.h>
+
+#include <stdio.h>
+
+#include <ros/names.h>
+
+#include "../topic_info.h"
+
+#include <nimbro_topic_transport/CompressedMsg.h>
+
+#if WITH_PLOTTING
+#  include <plot_msgs/Plot.h>
+#endif
+
+#include <fcntl.h>
+
+namespace nimbro_topic_transport
+{
+
+UDPReceiver::UDPReceiver()
+ : m_receivedBytesInStatsInterval(0)
+ , m_expectedPacketsInStatsInterval(0)
+ , m_missingPacketsInStatsInterval(0)
+ , m_remoteAddrLen(0)
+{
+	ros::NodeHandle nh("~");
+
+	m_pub_heartbeat = nh.advertise<std_msgs::Time>("heartbeat", 1);
+
+#if WITH_PLOTTING
+	m_pub_plot = nh.advertise<plot_msgs::Plot>("/plot", 1000);
+#endif
+
+	m_fd = socket(AF_INET, SOCK_DGRAM, 0);
+	if(m_fd < 0)
+	{
+		ROS_FATAL("Could not create socket: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	int port;
+	nh.param("port", port, 5050);
+
+	sockaddr_in addr;
+	addr.sin_family = AF_INET;
+	addr.sin_addr.s_addr = INADDR_ANY;
+	addr.sin_port = htons(port);
+
+	ROS_INFO("Binding to :%d", port);
+
+	if(bind(m_fd, (sockaddr*)&addr, sizeof(addr)) != 0)
+	{
+		ROS_FATAL("Could not bind socket: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	int on = 1;
+	if(setsockopt(m_fd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) != 0)
+	{
+		ROS_FATAL("Could not set broadcast flag: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	nh.param("drop_repeated_msgs", m_dropRepeatedMessages, true);
+	nh.param("warn_drop_incomplete", m_warnDropIncomplete, true);
+	nh.param("keep_compressed", m_keepCompressed, false);
+
+	nh.param("fec", m_fec, false);
+
+#if !(WITH_OPENFEC)
+	if(m_fec)
+		throw std::runtime_error("Please compile with FEC support to enable FEC");
+#endif
+
+	char hostnameBuf[256];
+	gethostname(hostnameBuf, sizeof(hostnameBuf));
+	hostnameBuf[sizeof(hostnameBuf)-1] = 0;
+
+	m_stats.node = ros::this_node::getName();
+	m_stats.protocol = "UDP";
+	m_stats.host = hostnameBuf;
+	m_stats.local_port = port;
+	m_stats.fec = m_fec;
+
+	nh.param("label", m_stats.label, std::string());
+
+	m_pub_stats = nh.advertise<ReceiverStats>("/network/receiver_stats", 1);
+	m_statsInterval = ros::WallDuration(2.0);
+	m_statsTimer = nh.createWallTimer(m_statsInterval,
+		boost::bind(&UDPReceiver::updateStats, this)
+	);
+
+	nh.param("topic_prefix", m_topicPrefix, std::string());
+}
+
+UDPReceiver::~UDPReceiver()
+{
+}
+
+template<class HeaderType>
+void UDPReceiver::handleFinishedMessage(Message* msg, HeaderType* header)
+{
+	if(msg->complete)
+		return;
+
+	// Packet complete
+	msg->complete = true;
+
+	// Enforce termination
+	header->topic_type[sizeof(header->topic_type)-1] = 0;
+	header->topic_name[sizeof(header->topic_name)-1] = 0;
+
+	ROS_DEBUG("Got a packet of type %s, topic %s, (msg id %d), size %d", header->topic_type, header->topic_name, msg->id, (int)msg->payload.size());
+
+	// Find topic
+	TopicMap::iterator topic_it = m_topics.find(header->topic_name);
+
+	boost::shared_ptr<TopicReceiver> topic;
+	if(topic_it == m_topics.end())
+	{
+		m_topics.insert(std::pair<std::string, boost::shared_ptr<TopicReceiver>>(
+			header->topic_name,
+			boost::make_shared<TopicReceiver>()
+		));
+		topic = m_topics[header->topic_name];
+
+		topic->last_message_counter = -1;
+	}
+	else
+		topic = topic_it->second;
+
+	// Send heartbeat message
+	ros::Time now = ros::Time::now();
+	if(now - m_lastHeartbeatTime > ros::Duration(0.2))
+	{
+		std_msgs::Time time;
+		time.data = now;
+		m_pub_heartbeat.publish(time);
+
+		m_lastHeartbeatTime = now;
+	}
+
+#if WITH_PLOTTING
+	// Plot packet size
+	plot_msgs::Plot plot;
+	plot.header.stamp = ros::Time::now();
+
+	plot_msgs::PlotPoint point;
+
+	std::string safe_topic = header->topic_name;
+	std::replace(safe_topic.begin(), safe_topic.end(), '/', '_');
+
+	point.name = "/udp_receiver/mbyte/" + safe_topic;
+	point.value = ((double)msg->getLength()) / 1024 / 1024;
+
+	plot.points.push_back(point);
+	m_pub_plot.publish(plot);
+#endif
+
+	if(m_dropRepeatedMessages && header->topic_msg_counter() == topic->last_message_counter)
+	{
+		// This is the same message, probably sent in relay mode
+		return;
+	}
+
+	bool compressed = header->flags & UDP_FLAG_COMPRESSED;
+
+	// Compare md5
+	if(topic->last_message_counter == -1 || memcmp(topic->md5, header->topic_md5, sizeof(topic->md5)) != 0 || (m_keepCompressed && topic->compressed != compressed))
+	{
+		topic->msg_def = topic_info::getMsgDef(header->topic_type);
+		topic->md5_str = topic_info::getMd5Sum(header->topic_type);
+
+		if(topic->msg_def.empty() || topic->md5_str.size() != 8*4)
+		{
+			ROS_ERROR("Could not find msg type '%s', please make sure msg definitions are up to date", header->topic_type);
+			return;
+		}
+
+		ROS_INFO("Received first message on topic '%s'", header->topic_name);
+		for(int i = 0; i < 4; ++i)
+		{
+			std::string md5_part = topic->md5_str.substr(8*i, 8);
+			uint32_t md5_num = strtoll(md5_part.c_str(), 0, 16);
+			topic->md5[i] = md5_num;
+		}
+
+		if(memcmp(topic->md5, header->topic_md5, sizeof(topic->md5)) != 0)
+		{
+			ROS_ERROR("Invalid md5 sum for topic type '%s', please make sure msg definitions are up to date", header->topic_type);
+			return;
+		}
+
+		if(m_keepCompressed && compressed)
+		{
+			// If we are requested to keep the messages compressed, we advertise our compressed msg type
+			topic->publisher = m_nh.advertise<CompressedMsg>(
+				m_topicPrefix + header->topic_name,
+				1,
+				boost::bind(&TopicReceiver::handleSubscriber, topic)
+			);
+		}
+		else
+		{
+			// ... otherwise, we advertise the native type
+			ros::AdvertiseOptions options(
+				m_topicPrefix + header->topic_name,
+				1,
+				topic->md5_str,
+				header->topic_type,
+				topic->msg_def,
+				boost::bind(&TopicReceiver::handleSubscriber, topic)
+			);
+
+			// Latching is often unexpected. Better to lose the first msg.
+			//options.latch = 1;
+			topic->publisher = m_nh.advertise(options);
+		}
+
+		topic->compressed = compressed;
+	}
+
+	if(compressed && m_keepCompressed)
+	{
+		CompressedMsgPtr compressed(new CompressedMsg);
+		compressed->type = header->topic_type;
+		memcpy(compressed->md5.data(), topic->md5, sizeof(topic->md5));
+
+		compressed->data.swap(msg->payload);
+
+		topic->publishCompressed(compressed);
+	}
+	else if(header->flags & UDP_FLAG_COMPRESSED)
+		topic->takeForDecompression(boost::make_shared<Message>(*msg));
+	else
+	{
+		boost::shared_ptr<topic_tools::ShapeShifter> shapeShifter(new topic_tools::ShapeShifter);
+		shapeShifter->morph(topic->md5_str, header->topic_type, topic->msg_def, "");
+
+		shapeShifter->read(*msg);
+
+		topic->publish(shapeShifter);
+	}
+
+	topic->last_message_counter = header->topic_msg_counter();
+}
+
+void UDPReceiver::run()
+{
+	std::vector<uint8_t> buf;
+
+	ROS_INFO("UDP receiver ready");
+	while(1)
+	{
+		ros::spinOnce();
+
+		// handleMessagePacket() below might swap() our buffer out, so make
+		// sure that we have enough room.
+		buf.resize(PACKET_SIZE);
+
+		fd_set fds;
+		FD_ZERO(&fds);
+		FD_SET(m_fd, &fds);
+
+		timeval timeout;
+		timeout.tv_usec = 50 * 1000;
+		timeout.tv_sec = 0;
+
+		int ret = select(m_fd+1, &fds, 0, 0, &timeout);
+		if(ret < 0)
+		{
+			if(errno == EINTR || errno == EAGAIN)
+				continue;
+
+			ROS_FATAL("Could not select(): %s", strerror(errno));
+			throw std::runtime_error(strerror(errno));
+		}
+		if(ret == 0)
+			continue;
+
+		sockaddr_storage addr;
+		socklen_t addrlen = sizeof(addr);
+
+		ssize_t size = recvfrom(m_fd, buf.data(), buf.size(), 0, (sockaddr*)&addr, &addrlen);
+
+		if(size < 0)
+		{
+			ROS_FATAL("Could not recv(): %s", strerror(errno));
+			throw std::runtime_error(strerror(errno));
+		}
+
+		if(addrlen != m_remoteAddrLen || memcmp(&addr, &m_remoteAddr, addrlen) != 0)
+		{
+			// Perform reverse lookup
+			char nameBuf[256];
+			char serviceBuf[256];
+
+			ros::WallTime startLookup = ros::WallTime::now();
+			if(getnameinfo((sockaddr*)&addr, addrlen, nameBuf, sizeof(nameBuf), serviceBuf, sizeof(serviceBuf), NI_NUMERICSERV) == 0)
+			{
+				ROS_INFO("New remote: %s:%s", nameBuf, serviceBuf);
+				m_stats.remote = nameBuf;
+				m_stats.remote_port = atoi(serviceBuf);
+			}
+			else
+			{
+				ROS_ERROR("Could not resolve remote address to name");
+				m_stats.remote = "unknown";
+				m_stats.remote_port = -1;
+			}
+			ros::WallTime endLookup = ros::WallTime::now();
+
+			// Warn if lookup takes up time (otherwise the user does not know
+			// what is going on)
+			if(endLookup - startLookup > ros::WallDuration(1.0))
+			{
+				ROS_WARN("Reverse address lookup took more than a second. "
+					"Consider adding '%s' to /etc/hosts",
+					m_stats.remote.c_str()
+				);
+			}
+
+			m_remoteAddr = addr;
+			m_remoteAddrLen = addrlen;
+		}
+
+		ROS_DEBUG("packet of size %lu", size);
+		m_receivedBytesInStatsInterval += size;
+
+		uint16_t msg_id;
+
+		// Obtain the message ID from the packet
+		if(m_fec)
+		{
+			FECPacket::Header* header = (FECPacket::Header*)buf.data();
+			msg_id = header->msg_id();
+		}
+		else
+		{
+			UDPGenericPacket* generic = (UDPGenericPacket*)buf.data();
+			msg_id = generic->msg_id();
+		}
+
+		// Look up the message ID in our list of incomplete messages
+		MessageBuffer::iterator it = std::find_if(m_incompleteMessages.begin(), m_incompleteMessages.end(),
+			[=](const Message& msg) { return msg.id == msg_id; }
+		);
+
+		if(it == m_incompleteMessages.end())
+		{
+			// Insert a new message
+			m_incompleteMessages.push_front(Message(msg_id));
+			it = m_incompleteMessages.begin();
+
+			pruneMessages();
+		}
+
+		handleMessagePacket(it, &buf, size);
+	}
+}
+
+void UDPReceiver::updateStats()
+{
+	m_stats.header.stamp = ros::Time::now();
+	m_stats.bandwidth = m_receivedBytesInStatsInterval / m_statsInterval.toSec();
+
+	m_stats.drop_rate = ((double)m_missingPacketsInStatsInterval) / m_expectedPacketsInStatsInterval;
+
+	m_pub_stats.publish(m_stats);
+
+	// Reset all stats counters
+	m_receivedBytesInStatsInterval = 0;
+	m_missingPacketsInStatsInterval = 0;
+	m_expectedPacketsInStatsInterval = 0;
+}
+
+void UDPReceiver::pruneMessages()
+{
+	// Erase messages that are too old (after index 31)
+	MessageBuffer::iterator itr = m_incompleteMessages.begin();
+	MessageBuffer::iterator it_end = m_incompleteMessages.end();
+	for(int i = 0; i < 31; ++i)
+	{
+		itr++;
+		if(itr == it_end)
+			break;
+	}
+
+	// Collect statistics on packets which will be deleted
+	for(MessageBuffer::iterator itd = itr; itd != it_end; ++itd)
+	{
+		const Message& msg = *itd;
+
+#if WITH_OPENFEC
+		if(m_fec)
+		{
+			if(msg.params)
+			{
+				int total = msg.params->nb_source_symbols + msg.params->nb_repair_symbols;
+				m_expectedPacketsInStatsInterval += total;
+				m_missingPacketsInStatsInterval += total - msg.received_symbols;
+			}
+		}
+		else
+#endif
+		{
+			int num_fragments = msg.msgs.size();
+			int received = 0;
+			for(unsigned int i = 0; i < msg.msgs.size(); ++i)
+			{
+				if(msg.msgs[i])
+					received++;
+			}
+
+			m_expectedPacketsInStatsInterval += num_fragments;
+			m_missingPacketsInStatsInterval += num_fragments - received;
+		}
+	}
+
+	// If enabled, warn each time we drop an incomplete message
+	if(m_warnDropIncomplete)
+	{
+		for(MessageBuffer::iterator itd = itr; itd != it_end; ++itd)
+		{
+			const Message& msg = *itd;
+
+			if(msg.complete)
+				continue;
+
+			int num_fragments = msg.msgs.size();
+			int received = 0;
+			for(unsigned int i = 0; i < msg.msgs.size(); ++i)
+			{
+				if(msg.msgs[i])
+					received++;
+			}
+
+#if WITH_OPENFEC
+			if(msg.decoder)
+			{
+				ROS_WARN("Dropping FEC message %d (%u/%u symbols)", msg.id, msg.received_symbols, msg.params->nb_source_symbols);
+			}
+			else
+#endif
+			{
+				ROS_WARN("Dropping message %d, %.2f%% of fragments received (%d/%d)",
+					msg.id, 100.0 * received / num_fragments, received, num_fragments
+				);
+			}
+		}
+	}
+
+	// Finally, delete all messages after index 31
+	m_incompleteMessages.erase(itr, it_end);
+}
+
+void UDPReceiver::handleMessagePacket(MessageBuffer::iterator it, std::vector<uint8_t>* buf, std::size_t size)
+{
+	Message* msg = &*it;
+
+	// If the message is already completed (happens especially with FEC),
+	// drop this packet.
+	if(msg->complete)
+	{
+#if WITH_OPENFEC
+		// ... but still include it in the statistics!
+		msg->received_symbols++;
+#endif
+		return;
+	}
+
+	if(m_fec)
+	{
+#if WITH_OPENFEC
+		// Save the received packet (OpenFEC expects all symbols to stay
+		// available until end of decoding)
+		boost::shared_ptr<std::vector<uint8_t>> fecBuffer(new std::vector<uint8_t>);
+		fecBuffer->swap(*buf);
+
+		msg->fecPackets.push_back(fecBuffer);
+
+		FECPacket* packet = (FECPacket*)fecBuffer->data();
+
+		if(!msg->decoder)
+		{
+			of_session_t* ses = 0;
+			of_parameters_t* params = 0;
+
+			if(packet->header.source_symbols() >= MIN_PACKETS_LDPC)
+			{
+				if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_DECODER, 1) != OF_STATUS_OK)
+				{
+					ROS_ERROR("Could not create LDPC decoder");
+					return;
+				}
+
+				of_ldpc_parameters_t* ldpc_params = (of_ldpc_parameters_t*)malloc(sizeof(of_ldpc_parameters_t));
+				ldpc_params->nb_source_symbols = packet->header.source_symbols();
+				ldpc_params->nb_repair_symbols = packet->header.repair_symbols();
+				ldpc_params->encoding_symbol_length = packet->header.symbol_length();
+				ldpc_params->prng_seed = packet->header.prng_seed();
+				ldpc_params->N1 = 7;
+
+				ROS_DEBUG("LDPC parameters: %d, %d, %d, 0x%X, %d", ldpc_params->nb_source_symbols, ldpc_params->nb_repair_symbols, ldpc_params->encoding_symbol_length, ldpc_params->prng_seed, ldpc_params->N1);
+
+				params = (of_parameters*)ldpc_params;
+			}
+			else
+			{
+				if(of_create_codec_instance(&ses, OF_CODEC_REED_SOLOMON_GF_2_M_STABLE, OF_DECODER, 1) != OF_STATUS_OK)
+				{
+					ROS_ERROR("Could not create REED_SOLOMON decoder");
+					return;
+				}
+
+				of_rs_2_m_parameters* rs_params = (of_rs_2_m_parameters_t*)malloc(sizeof(of_rs_2_m_parameters_t));
+				rs_params->nb_source_symbols = packet->header.source_symbols();
+				rs_params->nb_repair_symbols = packet->header.repair_symbols();
+				rs_params->encoding_symbol_length = packet->header.symbol_length();
+				rs_params->m = 8;
+
+				params = (of_parameters_t*)rs_params;
+			}
+
+			if(of_set_fec_parameters(ses, params) != OF_STATUS_OK)
+			{
+				ROS_ERROR("Could not set FEC parameters");
+				of_release_codec_instance(ses);
+				return;
+			}
+
+			msg->decoder.reset(ses, of_release_codec_instance);
+			msg->params.reset(params, free);
+
+			msg->received_symbols = 0;
+		}
+
+		msg->received_symbols++;
+
+		ROS_DEBUG("msg: %10d, symbol: %10d/%10d", msg->id, packet->header.symbol_id(), packet->header.source_symbols());
+
+		uint8_t* symbol_begin = packet->data;
+
+		if(size - sizeof(FECPacket::Header) != msg->params->encoding_symbol_length)
+		{
+			ROS_ERROR("Symbol size mismatch: got %d, expected %d",
+				(int)(size - sizeof(FECPacket::Header)),
+				(int)(msg->params->encoding_symbol_length)
+			);
+			return;
+		}
+
+		// FEC iterative decoding
+		if(of_decode_with_new_symbol(msg->decoder.get(), symbol_begin, packet->header.symbol_id()) != OF_STATUS_OK)
+		{
+			ROS_ERROR("Could not decode symbol");
+			return;
+		}
+
+		bool done = false;
+
+		if(msg->received_symbols >= msg->params->nb_source_symbols)
+		{
+			// Are we finished using the iterative decoding already?
+			done = of_is_decoding_complete(msg->decoder.get());
+
+			// As it is implemented in OpenFEC right now, we can only
+			// try the ML decoding (gaussian elimination) *once*.
+			// After that the internal state is screwed up and the message
+			// has to be discarded.
+			// So we have to be sure that it's worth it ;-)
+			if(!done && msg->received_symbols >= msg->params->nb_source_symbols + msg->params->nb_repair_symbols / 2)
+			{
+				of_status_t ret = of_finish_decoding(msg->decoder.get());
+				if(ret == OF_STATUS_OK)
+					done = true;
+				else
+				{
+					ROS_ERROR("ML decoding failed, dropping message...");
+					msg->complete = true;
+					return;
+				}
+			}
+		}
+
+		if(done)
+		{
+			ROS_DEBUG("FEC: Decoding done!");
+
+			std::vector<void*> symbols(msg->params->nb_source_symbols, 0);
+
+			if(of_get_source_symbols_tab(msg->decoder.get(), symbols.data()) != OF_STATUS_OK)
+			{
+				ROS_ERROR("Could not get decoded symbols");
+				return;
+			}
+
+			uint64_t payloadLength = msg->params->nb_source_symbols * msg->params->encoding_symbol_length;
+			if(msg->params->encoding_symbol_length < sizeof(FECHeader) || payloadLength < sizeof(FECHeader))
+			{
+				ROS_ERROR("Invalid short payload");
+				m_incompleteMessages.erase(it);
+				return;
+			}
+
+			FECHeader msgHeader;
+			memcpy(&msgHeader, symbols[0], sizeof(FECHeader));
+			payloadLength -= sizeof(FECHeader);
+
+			msg->payload.resize(payloadLength);
+
+			uint8_t* writePtr = msg->payload.data();
+			memcpy(
+				msg->payload.data(),
+				((uint8_t*)symbols[0]) + sizeof(FECHeader),
+				msg->params->encoding_symbol_length - sizeof(FECHeader)
+			);
+			writePtr += msg->params->encoding_symbol_length - sizeof(FECHeader);
+
+			for(unsigned int symbol = 1; symbol < msg->params->nb_source_symbols; ++symbol)
+			{
+				memcpy(writePtr, symbols[symbol], msg->params->encoding_symbol_length);
+				writePtr += msg->params->encoding_symbol_length;
+			}
+
+			msg->size = payloadLength;
+
+			handleFinishedMessage(msg, &msgHeader);
+
+			// keep completed messages in the buffer so that we know that
+			// we have to ignore any additional symbols of that message.
+		}
+#endif
+	}
+	else
+	{
+		UDPGenericPacket* generic = (UDPGenericPacket*)buf->data();
+		if(generic->frag_id == 0)
+		{
+			UDPFirstPacket* first = (UDPFirstPacket*)buf->data();
+
+			msg->header = first->header;
+
+			// We can calculate an approximate size now
+			uint32_t required_size = (msg->header.remaining_packets()+1) * PACKET_SIZE;
+			uint32_t my_size = size - sizeof(UDPFirstPacket);
+			if(msg->payload.size() < required_size)
+				msg->payload.resize(required_size);
+			memcpy(msg->payload.data(), first->data, my_size);
+
+			if(msg->size < my_size)
+				msg->size = my_size;
+
+			if(((uint16_t)msg->msgs.size()) < msg->header.remaining_packets()+1)
+				msg->msgs.resize(msg->header.remaining_packets()+1, false);
+		}
+		else
+		{
+			UDPDataPacket* data = (UDPDataPacket*)buf->data();
+
+			uint32_t offset = UDPFirstPacket::MaxDataSize + (data->header.frag_id-1) * UDPDataPacket::MaxDataSize;
+			uint32_t required_size = offset + size - sizeof(UDPDataPacket);
+			if(msg->payload.size() < required_size)
+				msg->payload.resize(required_size);
+			memcpy(msg->payload.data() + offset, data->data, size - sizeof(UDPDataPacket));
+
+			if(msg->size < required_size)
+				msg->size = required_size;
+		}
+
+		if(generic->frag_id >= msg->msgs.size())
+			msg->msgs.resize(generic->frag_id+1, false);
+
+		msg->msgs[generic->frag_id] = true;
+
+		if(std::all_of(msg->msgs.begin(), msg->msgs.end(), [](bool x){return x;}))
+		{
+			handleFinishedMessage(msg, &msg->header);
+
+			// as we delete the message from the buffer here, immediately
+			// add it to the statistics
+			m_expectedPacketsInStatsInterval += msg->msgs.size();
+
+			m_incompleteMessages.erase(it);
+		}
+	}
+}
+
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "udp_receiver", ros::init_options::NoSigintHandler);
+	ros::NodeHandle nh;
+
+	nimbro_topic_transport::UDPReceiver recv;
+	recv.run();
+
+	return 0;
+}

software/ros_packages/nimbro_topic_transport/src/udp/udp_receiver.h

@@ -0,0 +1,84 @@
+// UDP receiver node
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef UDP_RECEIVER_H
+#define UDP_RECEIVER_H
+
+#include <map>
+#include <vector>
+#include <string>
+#include <list>
+
+#include <sys/socket.h>
+
+#include <ros/publisher.h>
+#include <ros/wall_timer.h>
+
+#include <boost/thread.hpp>
+
+#include <nimbro_topic_transport/ReceiverStats.h>
+#include <nimbro_topic_transport/CompressedMsg.h>
+
+#include "udp_packet.h"
+#include "topic_receiver.h"
+
+namespace nimbro_topic_transport
+{
+
+
+
+class UDPReceiver
+{
+public:
+	UDPReceiver();
+	~UDPReceiver();
+
+	void run();
+private:
+	typedef std::map<std::string, boost::shared_ptr<TopicReceiver>> TopicMap;
+	typedef std::list<Message> MessageBuffer;
+
+	void handleMessagePacket(MessageBuffer::iterator it, std::vector<uint8_t>* buf, std::size_t size);
+
+	template<class HeaderType>
+	void handleFinishedMessage(Message* msg, HeaderType* header);
+
+	void pruneMessages();
+
+	void updateStats();
+
+	int m_fd;
+	MessageBuffer m_incompleteMessages;
+	TopicMap m_topics;
+
+	bool m_dropRepeatedMessages;
+	bool m_warnDropIncomplete;
+	bool m_keepCompressed;
+
+	ros::NodeHandle m_nh;
+	ros::Publisher m_pub_heartbeat;
+	ros::Time m_lastHeartbeatTime;
+
+	ros::Publisher m_pub_plot;
+
+	Message m_decompressedMessage;
+
+	bool m_fec;
+
+	nimbro_topic_transport::ReceiverStats m_stats;
+	uint64_t m_receivedBytesInStatsInterval;
+	uint64_t m_expectedPacketsInStatsInterval;
+	uint64_t m_missingPacketsInStatsInterval;
+	ros::Publisher m_pub_stats;
+	ros::WallDuration m_statsInterval;
+	ros::WallTimer m_statsTimer;
+
+	sockaddr_storage m_remoteAddr;
+	socklen_t m_remoteAddrLen;
+
+	std::string m_topicPrefix;
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/src/udp/udp_sender.cpp

@@ -0,0 +1,351 @@
+// UDP sender node
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include "udp_sender.h"
+#include "topic_sender.h"
+#include "udp_packet.h"
+
+#include <ros/init.h>
+#include <ros/node_handle.h>
+
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <sys/timerfd.h>
+#include <netdb.h>
+
+#include <ros/console.h>
+#include <stdio.h>
+#include <errno.h>
+
+#include <XmlRpcValue.h>
+
+#include <signal.h>
+
+#include <nimbro_topic_transport/SenderStats.h>
+
+namespace nimbro_topic_transport
+{
+
+UDPSender::UDPSender()
+ : m_msgID(0)
+ , m_sentBytesInStatsInterval(0)
+{
+	ros::NodeHandle nh("~");
+
+	nh.param("relay_mode", m_relayMode, false);
+
+	std::string dest_host;
+	nh.param("destination_addr", dest_host, std::string("localhost"));
+
+	int dest_port;
+	nh.param("destination_port", dest_port, 5050);
+
+	std::string dest_port_str = boost::lexical_cast<std::string>(dest_port);
+
+	addrinfo *info = 0;
+
+	if(getaddrinfo(dest_host.c_str(), dest_port_str.c_str(), 0, &info) != 0 || !info)
+	{
+		ROS_FATAL("Could not lookup destination address\n '%s': %s",
+			dest_host.c_str(), strerror(errno)
+		);
+		throw std::runtime_error(strerror(errno));
+	}
+
+	m_fd = socket(info->ai_family, SOCK_DGRAM, 0);
+	if(m_fd < 0)
+	{
+		ROS_FATAL("Could not create socket: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	int on = 1;
+	if(setsockopt(m_fd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) != 0)
+	{
+		ROS_FATAL("Could not enable SO_BROADCAST flag: %s", strerror(errno));
+		throw std::runtime_error(strerror(errno));
+	}
+
+	memcpy(&m_addr, info->ai_addr, info->ai_addrlen);
+	m_addrLen = info->ai_addrlen;
+
+	int source_port = dest_port;
+	if(nh.hasParam("source_port"))
+	{
+		if(!nh.getParam("source_port", source_port))
+		{
+			ROS_FATAL("Invalid source_port");
+			throw std::runtime_error("Invalid source port");
+		}
+
+		std::string source_port_str = boost::lexical_cast<std::string>(source_port);
+
+		addrinfo hints;
+		memset(&hints, 0, sizeof(hints));
+
+		hints.ai_flags = AI_PASSIVE;
+		hints.ai_family = info->ai_family;
+		hints.ai_socktype = SOCK_DGRAM;
+
+		addrinfo* localInfo = 0;
+		if(getaddrinfo(NULL, source_port_str.c_str(), &hints, &localInfo) != 0 || !localInfo)
+		{
+			ROS_FATAL("Could not get local address: %s", strerror(errno));
+			throw std::runtime_error("Could not get local address");
+		}
+
+		if(bind(m_fd, localInfo->ai_addr, localInfo->ai_addrlen) != 0)
+		{
+			ROS_FATAL("Could not bind to source port: %s", strerror(errno));
+			throw std::runtime_error(strerror(errno));
+		}
+
+		freeaddrinfo(localInfo);
+	}
+
+	freeaddrinfo(info);
+
+	nh.param("fec", m_fec, 0.0);
+
+	XmlRpc::XmlRpcValue list;
+	nh.getParam("topics", list);
+
+	ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+
+	for(int32_t i = 0; i < list.size(); ++i)
+	{
+		ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+		ROS_ASSERT(list[i].hasMember("name"));
+
+		int flags = 0;
+
+		bool resend = false;
+
+		double rate = 0.0; // unlimited
+		if(list[i].hasMember("rate"))
+			rate = list[i]["rate"];
+
+		if(list[i].hasMember("compress") && ((bool)list[i]["compress"]))
+			flags |= UDP_FLAG_COMPRESSED;
+
+		if(list[i].hasMember("resend") && ((bool)list[i]["resend"]))
+			resend = true;
+
+		bool enabled = true;
+		if(list[i].hasMember("enable") && (!(bool)list[i]["enable"]))
+			enabled = false;
+
+		std::string type;
+		if(list[i].hasMember("type"))
+			type = (std::string)(list[i]["type"]);
+
+		TopicSender* sender = new TopicSender(this, &nh, list[i]["name"], rate, resend, flags, enabled, type);
+
+		if(m_relayMode)
+			sender->setDirectTransmissionEnabled(false);
+
+		m_senders.push_back(sender);
+	}
+
+	nh.param("duplicate_first_packet", m_duplicateFirstPacket, false);
+
+	if(m_relayMode)
+	{
+		double target_bitrate;
+		if(!nh.getParam("relay_target_bitrate", target_bitrate))
+		{
+			throw std::runtime_error("relay mode needs relay_target_bitrate param");
+		}
+
+		double relay_control_rate;
+		nh.param("relay_control_rate", relay_control_rate, 100.0);
+
+		m_relayTokens = 0;
+		m_relayIndex = 0;
+		m_relayTokensPerStep = target_bitrate / 8.0 / relay_control_rate;
+
+		m_relayThreadShouldExit = false;
+		m_relayRate = relay_control_rate;
+		m_relayThread = boost::thread(boost::bind(&UDPSender::relay, this));
+
+		ROS_INFO("udp_sender: relay mode configured with control rate %f, target bitrate %f bit/s and token increment %d",
+			relay_control_rate, target_bitrate, m_relayTokensPerStep
+		);
+	}
+
+	char hostnameBuf[256];
+	gethostname(hostnameBuf, sizeof(hostnameBuf));
+	hostnameBuf[sizeof(hostnameBuf)-1] = 0;
+
+	m_stats.node = ros::this_node::getName();
+	m_stats.protocol = "UDP";
+	m_stats.host = hostnameBuf;
+	m_stats.destination = dest_host;
+	m_stats.destination_port = dest_port;
+	m_stats.source_port = source_port;
+	m_stats.fec = m_fec != 0.0;
+
+	nh.param("label", m_stats.label, std::string());
+
+	m_pub_stats = nh.advertise<nimbro_topic_transport::SenderStats>("/network/sender_stats", 1);
+
+	m_statsInterval = ros::WallDuration(2.0);
+	m_statsTimer = nh.createWallTimer(m_statsInterval,
+		boost::bind(&UDPSender::updateStats, this)
+	);
+	m_statsTimer.start();
+}
+
+UDPSender::~UDPSender()
+{
+	if(m_relayMode)
+	{
+		m_relayThreadShouldExit = true;
+		m_relayThread.join();
+	}
+
+	for(unsigned int i = 0; i < m_senders.size(); ++i)
+		delete m_senders[i];
+}
+
+uint16_t UDPSender::allocateMessageID()
+{
+	return m_msgID++;
+}
+
+bool UDPSender::send(const void* data, uint32_t size, const std::string& topic)
+{
+	if(m_relayMode)
+	{
+		std::vector<uint8_t> packet(size);
+		memcpy(packet.data(), data, size);
+
+		m_relayBuffer.emplace_back(std::move(packet));
+		m_relayNameBuffer.push_back(topic);
+		return true;
+	}
+	else
+	{
+		return internalSend(data, size, topic);
+	}
+}
+
+bool UDPSender::internalSend(const void* data, uint32_t size, const std::string& topic)
+{
+	if(sendto(m_fd, data, size, 0, (sockaddr*)&m_addr, m_addrLen) != size)
+	{
+		ROS_ERROR("Could not send data of size %d: %s", size, strerror(errno));
+		return false;
+	}
+
+	m_sentBytesInStatsInterval += size;
+	if (!topic.empty())
+		m_sentTopicBytesInStatsInterval[topic] += size;
+
+	return true;
+}
+
+void UDPSender::relay()
+{
+	ros::WallRate rate(m_relayRate);
+
+	ROS_INFO("Relay thread starting...");
+
+	while(!m_relayThreadShouldExit)
+	{
+		// New tokens! Bound to 100*m_relayTokensPerStep to prevent token buildup.
+		m_relayTokens = std::min<uint64_t>(
+			100*m_relayTokensPerStep,
+			m_relayTokens + m_relayTokensPerStep
+		);
+
+		if(m_senders.empty())
+			throw std::runtime_error("No senders configured");
+
+		// While we have enough token, send something!
+		while(1)
+		{
+			unsigned int tries = 0;
+			bool noData = false;
+
+			// Find a topic sender that has some data for us. Note that we
+			// do not consume the message!
+			while(m_relayBuffer.empty())
+			{
+				if(tries++ == m_senders.size())
+				{
+					noData = true;
+					break; // No data yet
+				}
+
+				m_senders[m_relayIndex]->sendCurrentMessage();
+				m_relayIndex = (m_relayIndex + 1) % m_senders.size();
+			}
+
+			if(noData)
+				break;
+
+			const std::vector<uint8_t>& packet = m_relayBuffer.front();
+
+			// Get the topic name the packet is from
+			const std::string& topic = m_relayNameBuffer.front();
+			std::size_t sizeOnWire = packet.size() + 20 + 8;
+
+			// out of tokens? Wait for next iteration.
+			if(sizeOnWire > m_relayTokens)
+				break;
+
+			if(!internalSend(packet.data(), packet.size(), topic))
+			{
+				ROS_ERROR("Could not send packet");
+				break;
+			}
+
+			// Consume tokens
+			m_relayTokens -= sizeOnWire;
+			m_relayBuffer.pop_front();
+			m_relayNameBuffer.pop_front();
+		}
+
+		rate.sleep();
+	}
+
+	ROS_INFO("Relay thread exiting...");
+}
+
+void UDPSender::updateStats()
+{
+	m_stats.header.stamp = ros::Time::now();
+	m_stats.bandwidth = 8 * m_sentBytesInStatsInterval / m_statsInterval.toSec();
+
+	// Get Bytes per topic in the message
+	m_stats.topics.clear();
+	for(auto& pair : m_sentTopicBytesInStatsInterval)
+	{
+		nimbro_topic_transport::TopicBandwidth tp;
+		tp.name = pair.first;
+		tp.bandwidth = 8 * pair.second / m_statsInterval.toSec();
+		pair.second = 0;
+        m_stats.topics.emplace_back(tp);
+	}
+
+	m_pub_stats.publish(m_stats);
+	m_sentBytesInStatsInterval = 0;
+}
+
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "udp_sender");
+
+	ros::NodeHandle nh("~");
+
+	nimbro_topic_transport::UDPSender sender;
+
+	ros::spin();
+
+	return 0;
+}
+

software/ros_packages/nimbro_topic_transport/src/udp/udp_sender.h

@@ -0,0 +1,80 @@
+// UDP sender node
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#ifndef UDP_SENDER_H
+#define UDP_SENDER_H
+
+#include <arpa/inet.h>
+#include <ros/time.h>
+#include <ros/timer.h>
+#include <ros/rate.h>
+#include <ros/wall_timer.h>
+#include <ros/publisher.h>
+
+#include <deque>
+#include <map>
+
+#include <boost/thread.hpp>
+
+#include <nimbro_topic_transport/SenderStats.h>
+
+namespace nimbro_topic_transport
+{
+
+class TopicSender;
+
+class UDPSender
+{
+public:
+	UDPSender();
+	~UDPSender();
+
+	uint16_t allocateMessageID();
+	bool send(const void* data, uint32_t size, const std::string& topic);
+
+	inline bool duplicateFirstPacket() const
+	{ return m_duplicateFirstPacket; }
+
+	inline double fec() const
+	{ return m_fec; }
+private:
+	void relay();
+	bool internalSend(const void* data, uint32_t size, const std::string& topic);
+
+	void updateStats();
+
+	uint16_t m_msgID;
+	int m_fd;
+	sockaddr_storage m_addr;
+	socklen_t m_addrLen;
+	ros::Time m_lastTime;
+	bool m_duplicateFirstPacket;
+	std::vector<TopicSender*> m_senders;
+
+	bool m_relayMode;
+	double m_relayRate;
+	int m_relayTokensPerStep;
+	uint64_t m_relayTokens;
+
+	std::deque<std::vector<uint8_t>> m_relayBuffer;
+	std::deque<std::string> m_relayNameBuffer;
+	unsigned int m_relayIndex;
+
+	boost::thread m_relayThread;
+	bool m_relayThreadShouldExit;
+
+	double m_fec;
+
+	nimbro_topic_transport::SenderStats m_stats;
+	ros::Publisher m_pub_stats;
+	ros::WallDuration m_statsInterval;
+	ros::WallTimer m_statsTimer;
+	uint64_t m_sentBytesInStatsInterval;
+
+	// Count sent bytes for each topic seperately for logging
+	std::map<std::string, uint64_t> m_sentTopicBytesInStatsInterval;
+};
+
+}
+
+#endif

software/ros_packages/nimbro_topic_transport/test/bidirectional.test

@@ -0,0 +1,11 @@
+<launch>
+	<arg name="allow_bidirectional" default="true" />
+
+	<include file="$(find nimbro_topic_transport)/launch/bidirectional_topics.launch">
+		<arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
+	</include>
+
+	<test test-name="test_bidirectional_$(arg allow_bidirectional)" pkg="nimbro_topic_transport" type="test_bidirectional">
+		<param name="allow_bidirectional" value="$(arg allow_bidirectional)" />
+	</test>
+</launch>

software/ros_packages/nimbro_topic_transport/test/test_bidirectional.cpp

@@ -0,0 +1,96 @@
+// Unit tests for nimbro_topic_transport
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include <catch_ros/catch.hpp>
+
+#include <ros/package.h>
+#include <ros/param.h>
+#include <ros/node_handle.h>
+
+#include <std_msgs/Int64.h>
+
+int g_counter = 0;
+
+void handleMessageLocal(const std_msgs::Int64 &msg)
+{
+	//REQUIRE(g_counter == msg.data);
+	g_counter++;
+}
+
+void
+testOneDirection(ros::NodeHandle &nh, bool allow_bidirectional, std::string source_topic, std::string sink_topic) {
+
+	ros::Publisher pub = nh.advertise<std_msgs::Int64>(source_topic, 2);
+	ros::Subscriber sub = nh.subscribe(sink_topic, 2, &handleMessageLocal);
+
+	int timeout = 50;
+	while(pub.getNumSubscribers() == 0)
+	{
+		ros::spinOnce();
+		if(--timeout == 0)
+		{
+			CAPTURE(pub.getNumSubscribers());
+			FAIL("sender did not subscribe on our topic");
+			return;
+		}
+
+		usleep(20 * 1000);
+	}
+
+	g_counter = 0;
+
+	std_msgs::Int64 msg;
+	msg.data = 0;
+	pub.publish(msg);
+
+	ROS_INFO("Sent first message, waiting for 5 seconds to receive it.");
+
+	for(int i = 0; i < 5000; ++i)
+	{
+		ros::spinOnce();
+		usleep(1000);
+	}
+
+	msg.data = 1;
+	pub.publish(msg);
+
+	ROS_INFO("Sent second message, waiting for 5 seconds to receive it.");
+
+	for(int i = 0; i < 5000; ++i)
+	{
+		ros::spinOnce();
+		usleep(1000);
+	}
+
+	if (allow_bidirectional) {
+		if (g_counter == 2) {
+			ROS_INFO("With bidirectional support, each message was received "
+				"exactly once.");
+			return;
+		}
+	} else {
+		if (g_counter > 10) {
+			ROS_INFO_STREAM("Without bidirectional support, the relay entered "
+				"an infinite loop; the messsages were received "
+				<< g_counter << " times.");
+			CAPTURE(g_counter);
+			return;
+		}
+	}
+	CAPTURE(allow_bidirectional);
+	CAPTURE(g_counter);
+	FAIL();
+}
+
+TEST_CASE("bidirectional_communication", "[topic]")
+{
+	ros::NodeHandle nh("~");
+	bool allow_bidirectional;
+	nh.getParam("allow_bidirectional", allow_bidirectional);
+
+	ROS_INFO("Testing machine1 to machine2 direction.");
+	testOneDirection(nh, allow_bidirectional, "/my_first_topic", "/recv/my_first_topic");
+
+	ROS_INFO("Testing machine2 to machine1 direction.");
+	testOneDirection(nh, allow_bidirectional, "/recv/my_first_topic", "/my_first_topic");
+}

software/ros_packages/nimbro_topic_transport/test/test_comm.cpp

@@ -0,0 +1,156 @@
+// Unit tests for nimbro_topic_transport
+// Author: Max Schwarz <max.schwarz@uni-bonn.de>
+
+#include <catch_ros/catch.hpp>
+
+#include <ros/package.h>
+#include <ros/node_handle.h>
+#include <ros/init.h>
+
+#include <std_msgs/Int64.h>
+#include <std_msgs/UInt64MultiArray.h>
+
+int g_counter = 0;
+
+void handleMessageLocal(const std_msgs::Int64 &msg)
+{
+	REQUIRE(g_counter == msg.data);
+	g_counter++;
+}
+
+int g_arrayCounter = 0;
+
+void handle_array(const std_msgs::UInt64MultiArray& msg)
+{
+	REQUIRE(msg.data.size() == 512);
+	for(int i = 0; i < 512; ++i)
+	{
+		REQUIRE(msg.data[i] == i);
+	}
+	g_arrayCounter++;
+}
+
+const int HUGE_SIZE = 3 * 1024;
+
+void handle_huge(const std_msgs::UInt64MultiArray& msg)
+{
+	REQUIRE(msg.data.size() == HUGE_SIZE);
+	for(int i = 0; i < HUGE_SIZE; ++i)
+	{
+		REQUIRE(msg.data[i] == i);
+	}
+	g_arrayCounter++;
+}
+
+TEST_CASE("simple", "[topic]")
+{
+	std_msgs::Int64 msg;
+
+	g_counter = 0;
+
+	ros::NodeHandle nh("~");
+
+	ros::Publisher pub = nh.advertise<std_msgs::Int64>("/test_topic", 2);
+	ros::Subscriber sub = nh.subscribe("/receive/test_topic", 2, &handleMessageLocal);
+
+	int timeout = 50;
+	while(pub.getNumSubscribers() == 0)
+	{
+		ros::spinOnce();
+		if(--timeout == 0)
+		{
+			CAPTURE(pub.getNumSubscribers());
+			FAIL("sender did not subscribe on our topic");
+			return;
+		}
+
+		usleep(20 * 1000);
+	}
+
+	g_counter = 0;
+
+	msg.data = 0;
+	pub.publish(msg);
+
+	for(int i = 0; i < 1000; ++i)
+	{
+		ros::spinOnce();
+		usleep(1000);
+	}
+
+	msg.data = 1;
+	pub.publish(msg);
+
+	for(int i = 0; i < 1000; ++i)
+	{
+		ros::spinOnce();
+		usleep(1000);
+
+		if(g_counter == 2)
+			return;
+	}
+
+	FAIL();
+}
+
+TEST_CASE("array", "[topic]")
+{
+	std_msgs::UInt64MultiArray msg;
+
+	g_arrayCounter = 0;
+
+	ros::NodeHandle nh("~");
+
+	ros::Publisher pub = nh.advertise<std_msgs::UInt64MultiArray>("/array_topic", 2);
+	ros::Subscriber sub;
+
+	SECTION("small")
+	{
+		sub = nh.subscribe("/receive/array_topic", 2, &handle_array);
+		for(int i = 0; i < 512; ++i)
+			msg.data.push_back(i);
+	}
+	SECTION("huge")
+	{
+		sub = nh.subscribe("/receive/array_topic", 2, &handle_huge);
+		for(int i = 0; i < HUGE_SIZE; ++i)
+			msg.data.push_back(i);
+	}
+
+	int timeout = 50;
+	while(pub.getNumSubscribers() == 0)
+	{
+		ros::spinOnce();
+		if(--timeout == 0)
+		{
+			CAPTURE(pub.getNumSubscribers());
+			FAIL("sender did not subscribe on our topic");
+			return;
+		}
+
+		usleep(20 * 1000);
+	}
+
+	g_arrayCounter = 0;
+	pub.publish(msg);
+
+	for(int i = 0; i < 1000; ++i)
+	{
+		ros::spinOnce();
+		usleep(1000);
+	}
+
+	pub.publish(msg);
+
+	for(int i = 0; i < 1000; ++i)
+	{
+		ros::spinOnce();
+		usleep(1000);
+
+		if(g_arrayCounter == 2)
+			return;
+	}
+
+	CAPTURE(g_arrayCounter);
+	FAIL();
+}

software/ros_packages/nimbro_topic_transport/test/topic_transport.test

@@ -0,0 +1,30 @@
+<launch>
+	<arg name="protocol" />
+	<arg name="fec" default="false" />
+
+	<!-- Skip long wait on config server, not needed here -->
+	<param name="config_server/enabled" value="false" />
+
+	<node name="sender" pkg="nimbro_topic_transport" type="$(arg protocol)_sender">
+		<!-- Destination: localhost -->
+		<param name="destination_addr" value="127.0.0.1" />
+		<param name="destination_port" value="5777" />
+		<param name="source_port" value="5776" />
+
+		<param if="$(arg fec)" name="fec" value="0.5" />
+
+		<!-- Load topics from yaml file -->
+		<rosparam command="load" file="$(find nimbro_topic_transport)/test/topics.yaml" />
+	</node>
+
+	<node name="receiver" pkg="nimbro_topic_transport" type="$(arg protocol)_receiver">
+		<param name="port" value="5777" />
+		<remap from="/test_topic" to="/receive/test_topic" />
+		<remap from="/array_topic" to="/receive/array_topic" />
+
+		<param name="fec" value="$(arg fec)" />
+	</node>
+
+	<test test-name="test_$(arg protocol)_fec_$(arg fec)" pkg="nimbro_topic_transport" type="test_comm">
+	</test>
+</launch>

software/ros_packages/nimbro_topic_transport/test/topics.yaml

@@ -0,0 +1,5 @@
+topics:
+ - name: "/test_topic"
+ - name: "/throttled_topic"
+   rate: 1.0
+ - name: "/array_topic"

software/ros_packages/nimbro_topic_transport/utils/nimbro_network.lua

@@ -0,0 +1,33 @@
+do
+        local nimbro_proto = Proto("nimbro_topic", "nimbro_topic_transport");
+
+        nimbro_proto.prefs.port   = Pref.uint( "nimbro_port", 5000, "UDP port" )
+
+        nimbro_proto.fields.msg   = ProtoField.uint16("nimbro.msg", "Message ID", base.DEC)
+        nimbro_proto.fields.frag   = ProtoField.uint16("nimbro.frag", "Fragment ID", base.DEC)
+
+        local prev_proto
+        local f_udp    = Field.new("udp")
+
+        function nimbro_proto.dissector(tvb, pinfo, tree)
+            pcall(function()prev_proto:call(tvb, pinfo, tree)end)
+
+            if not f_udp() then return end
+
+
+            -- this is just to add text to "nimbro.data" field, 
+            -- which you should display as column.
+            -- as an alternate, you may remove set_hidden() and view selected data in the treeview
+            tree:add_le(nimbro_proto.fields.msg, tvb(2,2));
+            tree:add_le(nimbro_proto.fields.frag, tvb(0,2));
+        end
+
+        -- if we hook upon UDP port, then offset will mean the beginning of the UDP data
+        udp_table = DissectorTable.get("udp.port")
+        prev_proto = udp_table:get_dissector(nimbro_proto.prefs.port)
+        udp_table:add(nimbro_proto.prefs.port, nimbro_proto)
+
+        -- if we hook as post dissector, the offset will be from start of the frame. 
+        -- don't forget to remove the prev_proto call if you'll use that kind of hook
+--        register_postdissector(nimbro_proto)
+end

software/ros_packages/nimbro_topic_transport/utils/nimbro_network_fec.lua

@@ -0,0 +1,33 @@
+do
+        local nimbro_proto = Proto("nimbro_topic", "nimbro_topic_transport");
+
+        nimbro_proto.prefs.port   = Pref.uint( "nimbro_port", 5777, "UDP port" )
+
+        nimbro_proto.fields.msg   = ProtoField.uint16("nimbro.msg", "Message ID", base.DEC)
+        nimbro_proto.fields.frag   = ProtoField.uint32("nimbro.frag", "Symbol ID", base.DEC)
+
+        local prev_proto
+        local f_udp    = Field.new("udp")
+
+        function nimbro_proto.dissector(tvb, pinfo, tree)
+            pcall(function()prev_proto:call(tvb, pinfo, tree)end)
+
+            if not f_udp() then return end
+
+
+            -- this is just to add text to "nimbro.data" field, 
+            -- which you should display as column.
+            -- as an alternate, you may remove set_hidden() and view selected data in the treeview
+            tree:add_le(nimbro_proto.fields.msg, tvb(0,2));
+            tree:add_le(nimbro_proto.fields.frag, tvb(2,4));
+        end
+
+        -- if we hook upon UDP port, then offset will mean the beginning of the UDP data
+        udp_table = DissectorTable.get("udp.port")
+        prev_proto = udp_table:get_dissector(nimbro_proto.prefs.port)
+        udp_table:add(nimbro_proto.prefs.port, nimbro_proto)
+
+        -- if we hook as post dissector, the offset will be from start of the frame. 
+        -- don't forget to remove the prev_proto call if you'll use that kind of hook
+--        register_postdissector(nimbro_proto)
+end

software/ros_packages/rover_camera/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.8)
+project(rover_camera)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport message_generation sensor_msgs)
+
+# add the resized image message
+
+catkin_package(CATKIN_DEPENDS cv_bridge image_transport message_runtime sensor_msgs)
+
+find_package(OpenCV)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+
+# add the publisher example
+add_executable(rover_camera src/rover_camera.cpp)
+add_dependencies(rover_camera ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(rover_camera ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+
+# Mark executables and/or libraries for installation
+install(TARGETS rover_camera
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+#install(FILES resized_plugins.xml
+#        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+#)

software/ros_packages/rover_camera/launch/example.launch

@@ -0,0 +1,18 @@
+<launch>
+  <group ns="cameras">
+    <node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" output="screen">
+      <param name="device_path" value="/dev/rover/camera_undercarriage"/>
+    </node>
+
+    <node name="navigation" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 2" output="screen" >
+      <param name="device_path" value="/dev/rover/camera_main_navigation" />
+    </node>
+
+
+    <node name="chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" output="screen" >
+      <param name="device_path" value="/dev/rover/camera_chassis" />
+    </node>
+
+
+  </group>
+</launch>

software/ros_packages/rover_camera/package.xml

@@ -0,0 +1,23 @@
+<package>
+  <name>rover_camera</name>
+  <version>0.0.0</version>
+  <description>This package opens a camera on linux using OpenCV and publishes multiple streams at different resolutions</description>
+  <author>Corwin Perren</author>
+  <maintainer email="caperren@gmail.com">Corwin Perren</maintainer>
+  <license>Apache 2.0</license>
+
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>opencv2</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>opencv2</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+</package>

software/ros_packages/rover_camera/src/rover_camera.cpp

@@ -0,0 +1,88 @@
+#include <string>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+int main(int argc, char** argv)
+{
+    std::string capture_device_path;
+    int fps;
+
+    int large_image_width;
+    int large_image_height;
+    int medium_image_width;
+    int medium_image_height;
+    int small_image_width;
+    int small_image_height;
+
+    ros::init(argc, argv, "camera");
+    ros::NodeHandle node_handle("~");
+
+    node_handle.param("device_path", capture_device_path, std::string("/dev/video0"));
+    node_handle.param("fps", fps, 30);
+
+    node_handle.param("large_image_width", large_image_width, 1280);
+    node_handle.param("large_image_height", large_image_height, 720);
+    node_handle.param("medium_image_width", medium_image_width, 640);
+    node_handle.param("medium_image_height", medium_image_height, 360);
+    node_handle.param("small_image_width", small_image_width, 256);
+    node_handle.param("small_image_height", small_image_height, 144);
+
+    cv::VideoCapture cap(capture_device_path);
+
+    cap.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
+    cap.set(CV_CAP_PROP_FRAME_WIDTH, large_image_width);
+    cap.set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height);
+    cap.set(CV_CAP_PROP_FPS, fps);
+
+    if(cap.isOpened() == false){
+        return -1;
+    }
+
+    std::string large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height);
+    std::string medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height);
+    std::string small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height);
+
+    image_transport::ImageTransport large_image_transport(node_handle);
+    image_transport::ImageTransport medium_image_transport(node_handle);
+    image_transport::ImageTransport small_image_transport(node_handle);
+
+    image_transport::Publisher large_image_publisher = large_image_transport.advertise(large_image_node_name, 1);
+    image_transport::Publisher medium_image_publisher = medium_image_transport.advertise(medium_image_node_name, 1);
+    image_transport::Publisher small_image_publisher = small_image_transport.advertise(small_image_node_name, 1);
+
+    cv::Mat image_large;
+    cv::Mat image_medium;
+    cv::Mat image_small;
+
+    ros::Rate loop_rate(fps + 2);
+
+    while (ros::ok()) {
+
+        cap.read(image_large);
+
+        if(!image_large.empty()){
+            cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
+            cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height));
+
+            sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
+            sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
+            sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
+
+            large_image_publisher.publish(large_image_message);
+            medium_image_publisher.publish(medium_image_message);
+            small_image_publisher.publish(small_image_message);
+        }
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    cap.release();
+}

software/ros_packages/rover_control/CMakeLists.txt

@@ -0,0 +1,201 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rover_control)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+  geometry_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+   FILES
+   DriveCommandMessage.msg
+   DriveControlMessage.msg
+ )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   std_msgs  # Or other packages containing msgs
+   geometry_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES rover_control
+#  CATKIN_DEPENDS ros_msgs rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/rover_control.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/rover_control_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_control.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

software/ros_packages/rover_control/launch/rover_control.launch

@@ -0,0 +1,28 @@
+ <launch>
+    <group ns="rover_control">
+        <node name="iris_controller" pkg="rover_control" type="iris_controller.py" output="screen">
+            <param name="port" value="/dev/rover/ttyIRIS"/>
+            <param name="hertz" value="16"/>
+        </node>
+
+        <node name="control_coordinator" pkg="rover_control" type="control_coordinator.py" output="screen"/>
+
+        <node name="rear_bogie" pkg="rover_control" type="drive_control.py" output="screen">
+            <param name="port" value="/dev/rover/ttyBogieRear"/>
+            <param name="drive_control_topic" value="drive_control/rear"/>
+        </node>
+
+        <node name="left_bogie" pkg="rover_control" type="drive_control.py" output="screen">
+            <param name="port" value="/dev/rover/ttyBogieLeft"/>
+            <param name="drive_control_topic" value="drive_control/left"/>
+            <param name="invert_first_motor" value="True"/>
+            <param name="invert_second_motor" value="True"/>
+        </node>
+
+        <node name="right_bogie" pkg="rover_control" type="drive_control.py" output="screen">
+            <param name="port" value="/dev/rover/ttyBogieRight"/>
+            <param name="drive_control_topic" value="drive_control/right"/>
+        </node>
+
+    </group>
+</launch>

software/ros_packages/rover_control/msg/DriveCommandMessage.msg

@@ -0,0 +1,2 @@
+bool ignore_drive_control
+geometry_msgs/Twist drive_twist

software/ros_packages/rover_control/msg/DriveControlMessage.msg

@@ -0,0 +1,7 @@
+bool first_motor_direction
+bool first_motor_sleep
+uint16 first_motor_speed
+
+bool second_motor_direction
+bool second_motor_sleep
+uint16 second_motor_speed

software/ros_packages/rover_control/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rover_control</name>
+  <version>0.0.0</version>
+  <description>The rover_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="caperren@todo.todo">caperren</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rover_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>ros_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>ros_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>ros_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py

@@ -0,0 +1,131 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+from PyQt5 import QtCore
+from time import time, sleep
+
+# Custom Imports
+from rover_control.msg import DriveCommandMessage, DriveControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "drive_coordinator"
+
+DEFAULT_IRIS_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
+DEFAULT_GROUND_STATION_DRIVE_COMMAND_TOPIC = "command_control/ground_station_drive"
+DEFAULT_REAR_BOGIE_TOPIC = "drive_control/rear"
+DEFAULT_LEFT_BOGIE_TOPIC = "drive_control/left"
+DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right"
+
+UINT16_MAX = 65535
+
+DEFAULT_HERTZ = 15
+
+
+#####################################
+# ControlCoordinator Class Definition
+#####################################
+class DriveCoordinator(object):
+    def __init__(self):
+
+        rospy.init_node(NODE_NAME)
+
+        self.iris_drive_command_topic = rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
+        self.ground_station_drive_command_topic = \
+            rospy.get_param("~ground_station_drive_command_topic", DEFAULT_GROUND_STATION_DRIVE_COMMAND_TOPIC)
+        self.rear_bogie_topic = rospy.get_param("~rear_bogie_control_topic", DEFAULT_REAR_BOGIE_TOPIC)
+        self.left_bogie_topic = rospy.get_param("~left_bogie_control_topic", DEFAULT_LEFT_BOGIE_TOPIC)
+        self.right_bogie_topic = rospy.get_param("~right_bogie_control_topic", DEFAULT_RIGHT_BOGIE_TOPIC)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        # ########## Class Variables ##########
+        self.iris_drive_command_subscriber = rospy.Subscriber(self.iris_drive_command_topic,
+                                                              DriveCommandMessage,
+                                                              self.iris_drive_command_callback)
+
+        self.ground_station_command_subscriber = rospy.Subscriber(self.ground_station_drive_command_topic,
+                                                                  DriveCommandMessage,
+                                                                  self.ground_station_drive_command_callback)
+
+        self.rear_bogie_publisher = rospy.Publisher(self.rear_bogie_topic, DriveControlMessage, queue_size=1)
+        self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)
+        self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
+
+        # Other Vars TODO: fix this later
+        self.drive_commands = {
+            "iris": DriveCommandMessage(),
+            "ground_station": DriveCommandMessage()
+        }
+
+        # ########## Run the Class ##########
+        self.run()
+
+    def run(self):
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            try:
+                self.process_drive_commands()
+            except Exception, error:
+                print "Error occurred:", error
+
+            time_diff = time() - start_time
+
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def process_drive_commands(self):
+        if not self.drive_commands["iris"].ignore_drive_control:
+            self.send_drive_control_command(self.drive_commands["iris"])
+        else:
+            self.send_drive_control_command(self.drive_commands["ground_station"])
+
+    def send_drive_control_command(self, drive_command):
+        rear_drive = DriveControlMessage()
+        left_drive = DriveControlMessage()
+        right_drive = DriveControlMessage()
+
+        left = drive_command.drive_twist.linear.x - drive_command.drive_twist.angular.z
+        right = drive_command.drive_twist.linear.x + drive_command.drive_twist.angular.z
+
+        left_direction = 1 if left >= 0 else 0
+        rear_drive.first_motor_direction = left_direction
+        left_drive.first_motor_direction = left_direction
+        left_drive.second_motor_direction = left_direction
+
+        right_direction = 1 if right >= 0 else 0
+        rear_drive.second_motor_direction = right_direction
+        right_drive.first_motor_direction = right_direction
+        right_drive.second_motor_direction = right_direction
+
+        left_speed = min(abs(left * UINT16_MAX), UINT16_MAX)
+        right_speed = min(abs(right * UINT16_MAX), UINT16_MAX)
+
+        rear_drive.first_motor_speed = left_speed
+        left_drive.first_motor_speed = left_speed
+        left_drive.second_motor_speed = left_speed
+
+        rear_drive.second_motor_speed = right_speed
+        right_drive.first_motor_speed = right_speed
+        right_drive.second_motor_speed = right_speed
+
+        self.rear_bogie_publisher.publish(rear_drive)
+        self.left_bogie_publisher.publish(left_drive)
+        self.right_bogie_publisher.publish(right_drive)
+
+
+    def iris_drive_command_callback(self, drive_command):
+        self.drive_commands["iris"] = drive_command
+        return
+
+    def ground_station_drive_command_callback(self, drive_command):
+        self.drive_commands["ground_station"] = drive_command
+        return
+
+
+if __name__ == '__main__':
+    DriveCoordinator()

software/ros_packages/rover_control/src/drive_control/drive_control.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+import serial.rs485
+import minimalmodbus
+
+# Custom Imports
+from rover_control.msg import DriveControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "drive_control"
+
+DEFAULT_PORT = "/dev/rover/ttyBogie"
+DEFAULT_BAUD = 115200
+
+DEFAULT_INVERT = False
+
+DEFAULT_DRIVE_CONTROL_TOPIC = "drive_control/rear"
+
+FIRST_MOTOR_ID = 1
+SECOND_MOTOR_ID = 2
+
+COMMUNICATIONS_TIMEOUT = 0.15  # Seconds
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+MODBUS_REGISTERS = {
+    "DIRECTION": 0,
+    "SPEED": 1,
+    "SLEEP": 2,
+
+    "CURRENT": 3,
+    "FAULT": 4,
+
+    "TEMPERATURE": 5
+}
+
+MOTOR_DRIVER_DEFAULT_MESSAGE = [
+    1,  # Forwards
+    0,  # 0 Speed
+    1  # Not in sleep mode
+]
+
+UINT16_MAX = 65535
+
+#####################################
+# DriveControl Class Definition
+#####################################
+class DriveControl(object):
+    def __init__(self):
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.first_motor_inverted = rospy.get_param("~invert_first_motor", DEFAULT_INVERT)
+        self.second_motor_inverted = rospy.get_param("~invert_second_motor", DEFAULT_INVERT)
+
+        self.drive_control_subscriber_topic = rospy.get_param("~drive_control_topic", DEFAULT_DRIVE_CONTROL_TOPIC)
+
+        self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
+        self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
+        self.__setup_minimalmodbus_for_485()
+
+        self.drive_control_subscriber = \
+            rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback)
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+        self.first_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.first_motor.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                        delay_before_rx=RX_DELAY,
+                                                                        delay_before_tx=TX_DELAY)
+
+        self.second_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.second_motor.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                         delay_before_rx=RX_DELAY,
+                                                                         delay_before_tx=TX_DELAY)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            sleep(0.25)
+
+    def drive_control_callback(self, drive_control):
+        try:
+            first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
+            first_direction = \
+                not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction
+            first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction
+            first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.first_motor_speed, UINT16_MAX)
+
+            second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
+            second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction
+            second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction
+            second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.second_motor_speed, UINT16_MAX)
+
+            self.first_motor.write_registers(0, first_motor_register_data)
+            self.second_motor.write_registers(0, second_motor_register_data)
+
+        except Exception, error:
+            print "Error occurred:", error
+
+
+if __name__ == "__main__":
+    DriveControl()