add apm

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(cititruck_driver)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib_msgs
+  diagnostic_msgs
+  dynamic_reconfigure
+  geometry_msgs
+  move_base_msgs
+  nav_msgs
+  rosgraph_msgs
+  roslaunch
+  rospy
+  rospy_message_converter
+  sensor_msgs
+  std_msgs
+  tf
+  visualization_msgs
+)
+
+catkin_python_setup()
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS
+    actionlib_msgs
+    diagnostic_msgs
+    dynamic_reconfigure
+    geometry_msgs
+    move_base_msgs
+    nav_msgs
+    rosgraph_msgs
+    rospy_message_converter
+    sensor_msgs
+    std_msgs
+    tf
+    visualization_msgs
+)
+
+#############
+## Install ##
+#############
+
+catkin_install_python(PROGRAMS
+  nodes/fake_cititruck_joint_publisher.py
+  nodes/cititruck_bridge.py
+  nodes/rep117_filter.py
+  nodes/tf_remove_child_frames.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+# Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/launch/mir.launch

@@ -0,0 +1,85 @@
+<launch>
+  <arg name="mir_type" default="mir_250" doc="The MiR variant. Can be 'mir_100' or 'mir_250' for now." />
+
+  <arg name="tf_prefix" default="" doc="TF prefix to use for all of MiR's TF frames"/>
+
+  <arg name="mir_hostname" default="192.168.12.20" />
+
+  <arg name="disable_map" default="false" doc="Disable the map topic and map -> odom TF transform from the MiR" />
+
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)"/>
+
+  <!-- URDF -->
+  <include file="$(find cititruck_description)/launch/upload_cititruck_urdf.launch">
+    <arg name="mir_type" value="$(arg mir_type)" />
+  </include>
+
+  <!-- Robot state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+    <remap from="/tf"        to="tf_rss" />
+    <remap from="/tf_static" to="tf_static_rss" />
+  </node>
+
+  <!-- remove those TFs that are also published by the MiR to avoid conflicts -->
+  <node name="tf_remove_state_publisher_frames" pkg="cititruck_driver" type="tf_remove_child_frames.py" output="screen">
+    <remap from="tf_in"         to="tf_rss" />
+    <remap from="tf_out"        to="/tf" />
+    <remap from="tf_static_in"  to="tf_static_rss" />
+    <remap from="tf_static_out" to="/tf_static" />
+    <rosparam param="remove_frames">
+      - base_link
+      - front_laser_link
+      - back_laser_link
+      - camera_top_link
+      - camera_top_depth_optical_frame
+      - camera_floor_link
+      - camera_floor_depth_optical_frame
+      - imu_link
+    </rosparam>
+  </node>
+
+  <!-- MiR base -->
+  <group if="$(arg disable_map)">
+    <node name="cititruck_bridge" pkg="cititruck_driver" type="cititruck_bridge.py" output="screen">
+      <param name="hostname" value="$(arg mir_hostname)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" />
+      <remap from="map" to="map_mir" />
+      <remap from="map_metadata" to="map_metadata_mir" />
+      <remap from="rosout" to="/rosout" />
+      <remap from="rosout_agg" to="/rosout_agg" />
+      <remap from="tf" to="tf_mir" />
+    </node>
+
+    <!-- remove the map -> odom TF transform -->
+    <node name="tf_remove_mir_map_frame" pkg="cititruck_driver" type="tf_remove_child_frames.py" output="screen">
+      <remap from="tf_in"         to="tf_mir" />
+      <remap from="tf_out"        to="/tf" />
+      <rosparam param="remove_frames">
+        - odom
+      </rosparam>
+    </node>
+  </group>
+  <group unless="$(arg disable_map)">
+    <node name="cititruck_bridge" pkg="cititruck_driver" type="cititruck_bridge.py" output="screen">
+      <param name="hostname" value="$(arg mir_hostname)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" />
+      <remap from="map" to="/map" />
+      <remap from="map_metadata" to="/map_metadata" />
+      <remap from="rosout" to="/rosout" />
+      <remap from="rosout_agg" to="/rosout_agg" />
+      <remap from="tf" to="/tf" />
+    </node>
+  </group>
+
+  <node name="b_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="b_scan" />
+    <remap from="scan_filtered" to="b_scan_rep117" />
+  </node>
+
+  <node name="f_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="f_scan" />
+    <remap from="scan_filtered" to="f_scan_rep117" />
+  </node>
+
+  <node name="fake_mir_joint_publisher" pkg="cititruck_driver" type="fake_mir_joint_publisher.py" output="screen" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/cititruck_bridge.py

@@ -0,0 +1,529 @@
+#!/usr/bin/env python3
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the the copyright holder nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Martin Günther
+
+import rospy
+
+import copy
+import sys
+from collections.abc import Iterable
+
+from cititruck_driver import rosbridge
+from rospy_message_converter import message_converter
+
+from actionlib import SimpleActionClient
+import actionlib_msgs.msg
+import diagnostic_msgs.msg
+import dynamic_reconfigure.msg
+import geometry_msgs.msg
+import cititruck_actions.msg
+import cititruck_msgs.msg
+import move_base_msgs.msg
+import nav_msgs.msg
+import rosgraph_msgs.msg
+import sdc21x0.msg
+import sensor_msgs.msg
+import std_msgs.msg
+import tf2_msgs.msg
+import visualization_msgs.msg
+
+from collections import OrderedDict
+
+tf_prefix = ''
+static_transforms = OrderedDict()
+
+
+class TopicConfig(object):
+    def __init__(self, topic, topic_type, latch=False, dict_filter=None):
+        self.topic = topic
+        self.topic_type = topic_type
+        self.latch = latch
+        self.dict_filter = dict_filter
+
+
+# remap cititruck_actions/cititruckMoveBaseAction => move_base_msgs/MoveBaseAction
+def _move_base_goal_dict_filter(msg_dict):
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    filtered_msg_dict['goal']['move_task'] = cititruck_actions.msg.cititruckMoveBaseGoal.GLOBAL_MOVE
+    filtered_msg_dict['goal']['goal_dist_threshold'] = 0.25
+    filtered_msg_dict['goal']['clear_costmaps'] = True
+    return filtered_msg_dict
+
+
+def _move_base_feedback_dict_filter(msg_dict):
+    # filter out slots from the dict that are not in our message definition
+    # e.g., cititruckMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    filtered_msg_dict['feedback'] = {
+        key: msg_dict['feedback'][key] for key in move_base_msgs.msg.MoveBaseFeedback.__slots__
+    }
+    return filtered_msg_dict
+
+
+def _move_base_result_dict_filter(msg_dict):
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in move_base_msgs.msg.MoveBaseResult.__slots__}
+    return filtered_msg_dict
+
+
+def _cmd_vel_dict_filter(msg_dict):
+    """
+    Convert Twist to TwistStamped.
+
+    Convert a geometry_msgs/Twist message dict (as sent from the ROS side) to
+    a geometry_msgs/TwistStamped message dict (as expected by the cititruck on
+    software version >=2.7).
+    """
+    header = std_msgs.msg.Header(frame_id='', stamp=rospy.Time.now())
+    filtered_msg_dict = {
+        'header': message_converter.convert_ros_message_to_dictionary(header),
+        'twist': copy.deepcopy(msg_dict),
+    }
+    return filtered_msg_dict
+
+
+def _tf_dict_filter(msg_dict):
+    filtered_msg_dict = copy.deepcopy(msg_dict)
+    for transform in filtered_msg_dict['transforms']:
+        transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/')
+    return filtered_msg_dict
+
+
+def _tf_static_dict_filter(msg_dict):
+    """
+    Cache tf_static messages (simulate latching).
+
+    The tf_static topic needs special handling. Publishers on tf_static are *latched*, which means that the ROS master
+    caches the last message that was sent by each publisher on that topic, and will forward it to new subscribers.
+    However, since the cititruck_driver node appears to the ROS master as a single node with a single publisher on tf_static,
+    and there are multiple actual publishers hiding behind it on the cititruck side, only one of those messages will be
+    cached. Therefore, we need to implement the caching ourselves and make sure that we always publish the full set of
+    transforms as a single message.
+    """
+    global static_transforms
+
+    # prepend tf_prefix
+    filtered_msg_dict = _tf_dict_filter(msg_dict)
+
+    # The following code was copied + modified from https://github.com/tradr-project/static_transform_mux .
+
+    # Process the incoming transforms, merge them with our cache.
+    for transform in filtered_msg_dict['transforms']:
+        key = transform['child_frame_id']
+        rospy.loginfo(
+            "[%s] tf_static: updated transform %s->%s.",
+            rospy.get_name(),
+            transform['header']['frame_id'],
+            transform['child_frame_id'],
+        )
+        static_transforms[key] = transform
+
+    # Return the cached messages.
+    filtered_msg_dict['transforms'] = static_transforms.values()
+    rospy.loginfo(
+        "[%s] tf_static: sent %i transforms: %s",
+        rospy.get_name(),
+        len(static_transforms),
+        str(static_transforms.keys()),
+    )
+    return filtered_msg_dict
+
+
+def _prepend_tf_prefix_dict_filter(msg_dict):
+    # filtered_msg_dict = copy.deepcopy(msg_dict)
+    if not isinstance(msg_dict, dict):  # can happen during recursion
+        return
+    for (key, value) in msg_dict.items():
+        if key == 'header':
+            try:
+                # prepend frame_id
+                frame_id = value['frame_id'].strip('/')
+                if frame_id != 'map':
+                    # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty)
+                    value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/')
+                else:
+                    value['frame_id'] = frame_id
+
+            except TypeError:
+                pass  # value is not a dict
+            except KeyError:
+                pass  # value doesn't have key 'frame_id'
+        elif isinstance(value, dict):
+            _prepend_tf_prefix_dict_filter(value)
+        elif isinstance(value, Iterable):  # an Iterable other than dict (e.g., a list)
+            for item in value:
+                _prepend_tf_prefix_dict_filter(item)
+    return msg_dict
+
+
+def _remove_tf_prefix_dict_filter(msg_dict):
+    # filtered_msg_dict = copy.deepcopy(msg_dict)
+    if not isinstance(msg_dict, dict):  # can happen during recursion
+        return
+    for (key, value) in msg_dict.items():
+        if key == 'header':
+            try:
+                # remove frame_id
+                s = value['frame_id'].strip('/')
+                if s.find(tf_prefix) == 0:
+                    value['frame_id'] = (s[len(tf_prefix) :]).strip('/')  # strip off tf_prefix, then strip leading '/'
+            except TypeError:
+                pass  # value is not a dict
+            except KeyError:
+                pass  # value doesn't have key 'frame_id'
+        elif isinstance(value, dict):
+            _remove_tf_prefix_dict_filter(value)
+        elif isinstance(value, Iterable):  # an Iterable other than dict (e.g., a list)
+            for item in value:
+                _remove_tf_prefix_dict_filter(item)
+    return msg_dict
+
+
+# topics we want to publish to ROS (and subscribe to from the cititruck)
+PUB_TOPICS = [
+    # TopicConfig('LightCtrl/bms_data', cititruck_msgs.msg.BMSData),
+    # TopicConfig('LightCtrl/charging_state', cititruck_msgs.msg.ChargingState),
+    TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range),
+    # TopicConfig('MC/battery_currents', cititruck_msgs.msg.BatteryCurrents),
+    # TopicConfig('MC/battery_voltage', std_msgs.msg.Float64),
+    TopicConfig('MC/currents', sdc21x0.msg.MotorCurrents),
+    # TopicConfig('MC/encoders', sdc21x0.msg.StampedEncoders),
+    TopicConfig('MissionController/CheckArea/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('MissionController/goal_position_guid', std_msgs.msg.String),
+    # TopicConfig('MissionController/prompt_user', cititruck_msgs.msg.UserPrompt),
+    TopicConfig('SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('SickPLC/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('active_mapping_guid', std_msgs.msg.String),
+    TopicConfig('amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped),
+    TopicConfig('b_raw_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('b_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('camera_floor/background', sensor_msgs.msg.PointCloud2),
+    TopicConfig('camera_floor/depth/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('camera_floor/depth/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('camera_floor/depth/points', sensor_msgs.msg.PointCloud2),
+    TopicConfig('camera_floor/filter/visualization_marker', visualization_msgs.msg.Marker),
+    TopicConfig('camera_floor/floor', sensor_msgs.msg.PointCloud2),
+    TopicConfig('camera_floor/obstacles', sensor_msgs.msg.PointCloud2),
+    TopicConfig('check_area/polygon', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('check_pose_area/polygon', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('data_events/area_events', cititruck_data_msgs.msg.AreaEventEvent),
+    # TopicConfig('data_events/maps', cititruck_data_msgs.msg.MapEvent),
+    # TopicConfig('data_events/positions', cititruck_data_msgs.msg.PositionEvent),
+    # TopicConfig('data_events/registers', cititruck_data_msgs.msg.PLCRegisterEvent),
+    # TopicConfig('data_events/sounds', cititruck_data_msgs.msg.SoundEvent),
+    TopicConfig('diagnostics', diagnostic_msgs.msg.DiagnosticArray),
+    TopicConfig('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray),
+    TopicConfig('diagnostics_toplevel_state', diagnostic_msgs.msg.DiagnosticStatus),
+    TopicConfig('f_raw_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('f_scan', sensor_msgs.msg.LaserScan),
+    TopicConfig('imu_data', sensor_msgs.msg.Imu),
+    TopicConfig('laser_back/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('laser_back/driver/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('laser_front/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('laser_front/driver/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('localization_score', std_msgs.msg.Float64),
+    TopicConfig('/map', nav_msgs.msg.OccupancyGrid, latch=True),
+    TopicConfig('/map_metadata', nav_msgs.msg.MapMetaData),
+    # TopicConfig('marker_tracking_node/feedback', cititruck_marker_tracking.msg.MarkerTrackingActionFeedback),
+    # TopicConfig(
+    #     'marker_tracking_node/laser_line_extract/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription
+    # ),
+    # TopicConfig('marker_tracking_node/laser_line_extract/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('marker_tracking_node/laser_line_extract/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('marker_tracking_node/result', cititruck_marker_tracking.msg.MarkerTrackingActionResult),
+    # TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray),
+    # TopicConfig('cititruckEventTrigger/events', cititruck_msgs.msg.Events),
+    TopicConfig('cititruck_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('cititruck_amcl/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('cititruck_amcl/selected_points', sensor_msgs.msg.PointCloud2),
+    TopicConfig('cititruck_log', rosgraph_msgs.msg.Log),
+    # TopicConfig('cititruck_sound/sound_event', cititruck_msgs.msg.SoundEvent),
+    TopicConfig('cititruck_status_msg', std_msgs.msg.String),
+    # TopicConfig('cititruckspawn/node_events', cititruckSpawn.msg.LaunchItem),
+    TopicConfig('cititruckwebapp/grid_map_metadata', cititruck_msgs.msg.LocalMapStat),
+    TopicConfig('cititruckwebapp/laser_map_metadata', cititruck_msgs.msg.LocalMapStat),
+    # TopicConfig('cititruckwebapp/web_path', cititruck_msgs.msg.WebPath),
+    # really cititruck_actions/cititruckMoveBaseActionFeedback:
+    TopicConfig(
+        'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter
+    ),
+    # really cititruck_actions/cititruckMoveBaseActionResult:
+    TopicConfig('move_base/result', move_base_msgs.msg.MoveBaseActionResult, dict_filter=_move_base_result_dict_filter),
+    TopicConfig('move_base/status', actionlib_msgs.msg.GoalStatusArray),
+    # TopicConfig('move_base_node/cititruckPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2),
+    # TopicConfig('move_base_node/cititruckPlannerROS/global_plan', nav_msgs.msg.Path),
+    # TopicConfig('move_base_node/cititruckPlannerROS/len_to_goal', std_msgs.msg.Float64),
+    TopicConfig('move_base_node/cititruckPlannerROS/local_plan', nav_msgs.msg.Path),
+    # TopicConfig('move_base_node/cititruckPlannerROS/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/cititruckPlannerROS/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/cititruckPlannerROS/updated_global_plan', cititruck_msgs.msg.PlanSegments),
+    # TopicConfig('move_base_node/cititruckPlannerROS/visualization_marker', visualization_msgs.msg.MarkerArray),
+    TopicConfig('move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path),
+    # TopicConfig(
+    #   'move_base_node/SBPLLatticePlanner/sbpl_lattice_planner_stats', sbpl_lattice_planner.msg.SBPLLatticePlannerStats
+    # ),
+    # TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', visualization_msgs.msg.MarkerArray),
+    TopicConfig('move_base_node/current_goal', geometry_msgs.msg.PoseStamped),
+    # TopicConfig('move_base_node/global_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/global_costmap/obstacles', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/global_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/global_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
+    # TopicConfig('move_base_node/global_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('move_base_node/global_costmap/unknown_space', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/global_plan', nav_msgs.msg.Path),
+    TopicConfig('move_base_node/local_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/local_costmap/obstacles', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/local_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/local_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/local_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('move_base_node/local_costmap/safety_zone', geometry_msgs.msg.PolygonStamped),
+    # TopicConfig('move_base_node/local_costmap/unknown_space', nav_msgs.msg.GridCells),
+    # TopicConfig('move_base_node/cititruck_escape_recovery/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('move_base_node/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('move_base_node/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/time_to_coll', std_msgs.msg.Float64),
+    TopicConfig('move_base_node/traffic_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/traffic_costmap/obstacles', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/traffic_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    TopicConfig('move_base_node/traffic_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('move_base_node/traffic_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
+    TopicConfig('move_base_node/traffic_costmap/unknown_space', nav_msgs.msg.GridCells),
+    TopicConfig('move_base_node/visualization_marker', visualization_msgs.msg.Marker),
+    TopicConfig('move_base_simple/visualization_marker', visualization_msgs.msg.Marker),
+    TopicConfig('odom', nav_msgs.msg.Odometry),
+    TopicConfig('odom_enc', nav_msgs.msg.Odometry),
+    # TopicConfig('one_way_map', nav_msgs.msg.OccupancyGrid),
+    # TopicConfig('param_update', std_msgs.msg.String),
+    # TopicConfig('particlevizmarker', visualization_msgs.msg.MarkerArray),
+    # TopicConfig('resource_tracker/needed_resources', cititruck_msgs.msg.ResourcesState),
+    TopicConfig('robot_mode', cititruck_msgs.msg.RobotMode),
+    TopicConfig('robot_pose', geometry_msgs.msg.Pose),
+    TopicConfig('robot_state', cititruck_msgs.msg.RobotState),
+    # TopicConfig('robot_status', cititruck_msgs.msg.RobotStatus),
+    TopicConfig('/rosout', rosgraph_msgs.msg.Log),
+    TopicConfig('/rosout_agg', rosgraph_msgs.msg.Log),
+    TopicConfig('scan', sensor_msgs.msg.LaserScan),
+    # TopicConfig('scan_filter/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
+    # TopicConfig('scan_filter/parameter_updates', dynamic_reconfigure.msg.Config),
+    TopicConfig('scan_filter/visualization_marker', visualization_msgs.msg.Marker),
+    # TopicConfig('session_importer_node/info', cititruckSessionImporter.msg.SessionImportInfo),
+    # TopicConfig('set_mc_PID', std_msgs.msg.Float64MultiArray),
+    TopicConfig('/tf', tf2_msgs.msg.TFMessage, dict_filter=_tf_dict_filter),
+    TopicConfig('/tf_static', tf2_msgs.msg.TFMessage, dict_filter=_tf_static_dict_filter, latch=True),
+    # TopicConfig('traffic_map', nav_msgs.msg.OccupancyGrid),
+    # TopicConfig('wifi_diagnostics', diagnostic_msgs.msg.DiagnosticArray),
+    # TopicConfig('wifi_diagnostics/cur_ap', cititruck_wifi_msgs.msg.APInfo),
+    # TopicConfig('wifi_diagnostics/roam_events', cititruck_wifi_msgs.msg.WifiRoamEvent),
+    # TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', cititruck_wifi_msgs.msg.WifiInterfaceStats),
+    # TopicConfig('wifi_diagnostics/wifi_ap_rssi', cititruck_wifi_msgs.msg.APRssiStats),
+    # TopicConfig('wifi_diagnostics/wifi_ap_time_stats', cititruck_wifi_msgs.msg.APTimeStats),
+    # TopicConfig('wifi_watchdog/ping', cititruck_wifi_msgs.msg.APPingStats),
+]
+
+# topics we want to subscribe to from ROS (and publish to the cititruck)
+SUB_TOPICS = [
+    # really geometry_msgs.msg.TwistStamped:
+    TopicConfig('cmd_vel', geometry_msgs.msg.Twist, dict_filter=_cmd_vel_dict_filter),
+    TopicConfig('initialpose', geometry_msgs.msg.PoseWithCovarianceStamped),
+    TopicConfig('light_cmd', std_msgs.msg.String),
+    TopicConfig('cititruck_cmd', std_msgs.msg.String),
+    TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID),
+    # really cititruck_actions/cititruckMoveBaseActionGoal:
+    TopicConfig('move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter),
+]
+
+
+class PublisherWrapper(rospy.SubscribeListener):
+    def __init__(self, topic_config, robot):
+        self.topic_config = topic_config
+        self.robot = robot
+        self.connected = False
+        # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
+        self.pub = rospy.Publisher(
+            name=topic_config.topic,
+            data_class=topic_config.topic_type,
+            subscriber_listener=self,
+            latch=topic_config.latch,
+            queue_size=10,
+        )
+        rospy.loginfo(
+            "[%s] publishing topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
+        )
+        # latched topics must be subscribed immediately
+        if topic_config.latch:
+            self.peer_subscribe("", None, None)
+
+    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
+        if not self.connected:
+            self.connected = True
+            rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
+            absolute_topic = '/' + self.topic_config.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+            self.robot.subscribe(topic=absolute_topic, callback=self.callback)
+
+    def peer_unsubscribe(self, topic_name, num_peers):
+        pass
+        # doesn't work: once ubsubscribed, robot doesn't let us subscribe again
+        # if self.connected and self.pub.get_num_connections() == 0 and not self.topic_config.latch:
+        #     self.connected = False
+        #     rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
+        #     absolute_topic = '/' + self.topic_config.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+        #     self.robot.unsubscribe(topic=absolute_topic)
+
+    def callback(self, msg_dict):
+        msg_dict = _prepend_tf_prefix_dict_filter(msg_dict)
+        if self.topic_config.dict_filter is not None:
+            msg_dict = self.topic_config.dict_filter(msg_dict)
+        msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict)
+        self.pub.publish(msg)
+
+
+class SubscriberWrapper(object):
+    def __init__(self, topic_config, robot):
+        self.topic_config = topic_config
+        self.robot = robot
+        # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
+        self.sub = rospy.Subscriber(
+            name=topic_config.topic, data_class=topic_config.topic_type, callback=self.callback, queue_size=10
+        )
+        rospy.loginfo(
+            "[%s] subscribing to topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
+        )
+
+    def callback(self, msg):
+        msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
+        msg_dict = _remove_tf_prefix_dict_filter(msg_dict)
+        if self.topic_config.dict_filter is not None:
+            msg_dict = self.topic_config.dict_filter(msg_dict)
+        absolute_topic = '/' + self.topic_config.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+        self.robot.publish(absolute_topic, msg_dict)
+
+
+class cititruckBridge(object):
+    def __init__(self):
+        try:
+            hostname = rospy.get_param('~hostname')
+        except KeyError:
+            rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
+            sys.exit(-1)
+        port = rospy.get_param('~port', 9090)
+
+        global tf_prefix
+        tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
+
+        rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
+        self.robot = rosbridge.RosbridgeSetup(hostname, port)
+
+        r = rospy.Rate(10)
+        i = 1
+        while not self.robot.is_connected():
+            if rospy.is_shutdown():
+                sys.exit(0)
+            if self.robot.is_errored():
+                rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
+                sys.exit(-1)
+            if i % 10 == 0:
+                rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
+            i += 1
+            r.sleep()
+
+        rospy.loginfo('[%s] ... connected.', rospy.get_name())
+
+        topics = self.get_topics()
+        published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
+        subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
+
+        for pub_topic in PUB_TOPICS:
+            PublisherWrapper(pub_topic, self.robot)
+            absolute_topic = '/' + pub_topic.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+            if absolute_topic not in published_topics:
+                rospy.logwarn("[%s] topic '%s' is not published by the cititruck!", rospy.get_name(), pub_topic.topic)
+
+        for sub_topic in SUB_TOPICS:
+            SubscriberWrapper(sub_topic, self.robot)
+            absolute_topic = '/' + sub_topic.topic.lstrip('/')  # ensure exactly 1 leading slash for cititruck comm
+            if absolute_topic not in subscribed_topics:
+                rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the cititruck!", rospy.get_name(), sub_topic.topic)
+
+        # At least with software version 2.8 there were issues when forwarding a simple goal to the robot
+        # This workaround converts it into an action. Check https://github.com/DFKI-NI/cititruck_robot/issues/60 for details.
+        self._move_base_client = SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction)
+        rospy.Subscriber("move_base_simple/goal", geometry_msgs.msg.PoseStamped, self._move_base_simple_goal_callback)
+
+    def get_topics(self):
+        srv_response = self.robot.callService('/rosapi/topics', msg={})
+        topic_names = sorted(srv_response['topics'])
+        topics = []
+
+        for topic_name in topic_names:
+            srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name})
+            topic_type = srv_response['type']
+
+            srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name})
+            has_publishers = True if len(srv_response['publishers']) > 0 else False
+
+            srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name})
+            has_subscribers = True if len(srv_response['subscribers']) > 0 else False
+
+            topics.append([topic_name, topic_type, has_publishers, has_subscribers])
+
+        print('Publishers:')
+        for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
+            if has_publishers:
+                print((' * %s [%s]' % (topic_name, topic_type)))
+
+        print('\nSubscribers:')
+        for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
+            if has_subscribers:
+                print((' * %s [%s]' % (topic_name, topic_type)))
+
+        return topics
+
+    def _move_base_simple_goal_callback(self, msg):
+        if not self._move_base_client.wait_for_server(rospy.Duration(2)):
+            rospy.logwarn("Could not connect to 'move_base' server after two seconds. Dropping goal.")
+            rospy.logwarn("Did you activate 'planner' in the cititruck web interface?")
+            return
+        goal = move_base_msgs.msg.MoveBaseGoal()
+        goal.target_pose.header = copy.deepcopy(msg.header)
+        goal.target_pose.pose = copy.deepcopy(msg.pose)
+        self._move_base_client.send_goal(goal)
+
+
+def main():
+    rospy.init_node('cititruck_bridge')
+    cititruckBridge()
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/fake_cititruck_joint_publisher.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the the copyright holder nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Martin Günther
+
+import rospy
+from sensor_msgs.msg import JointState
+
+
+def fake_cititruck_joint_publisher():
+    rospy.init_node('fake_cititruck_joint_publisher')
+    prefix = rospy.get_param('~prefix', '')
+    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+    r = rospy.Rate(50.0)
+    while not rospy.is_shutdown():
+        js = JointState()
+        js.header.stamp = rospy.Time.now()
+        js.name = [
+            prefix + 'base2steer_joint',
+            prefix + 'steer2sd_wheel_joint',
+            prefix + 'base2fixed_left_wheel_joint',
+            prefix + 'base2fixed_right_wheel_joint',
+        ]
+        js.position = [0.0 for _ in js.name]
+        js.velocity = [0.0 for _ in js.name]
+        js.effort = [0.0 for _ in js.name]
+        pub.publish(js)
+        r.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        fake_cititruck_joint_publisher()
+    except rospy.ROSInterruptException:
+        pass

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/rep117_filter.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the the copyright holder nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Martin Günther
+
+import rospy
+from sensor_msgs.msg import LaserScan
+
+pub = None
+
+
+def callback(msg):
+    """
+    Convert laser scans to REP 117 standard.
+
+    See http://www.ros.org/reps/rep-0117.html
+    """
+    ranges_out = []
+    for dist in msg.ranges:
+        if dist < msg.range_min:
+            # assume "reading too close to measure",
+            # although it could also be "reading invalid" (nan)
+            ranges_out.append(float("-inf"))
+
+        elif dist > msg.range_max:
+            # assume "reading of no return (outside sensor range)",
+            # although it could also be "reading invalid" (nan)
+            ranges_out.append(float("inf"))
+        else:
+            ranges_out.append(dist)
+
+    msg.ranges = ranges_out
+    pub.publish(msg)
+
+
+def main():
+    global pub
+    rospy.init_node('rep117_filter')
+
+    pub = rospy.Publisher('scan_filtered', LaserScan, queue_size=10)
+    rospy.Subscriber('scan', LaserScan, callback)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/nodes/tf_remove_child_frames.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2016 The Cartographer Authors
+# Copyright 2018 DFKI GmbH
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#    http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rospy
+from tf.msg import tfMessage
+
+
+def main():
+    rospy.init_node('tf_remove_child_frames')
+    remove_frames = rospy.get_param('~remove_frames', [])
+
+    # filter tf_in topic
+    tf_pub = rospy.Publisher('tf_out', tfMessage, queue_size=1)
+
+    def tf_cb(msg):
+        msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames]
+        if len(msg.transforms) > 0:
+            tf_pub.publish(msg)
+
+    rospy.Subscriber('tf_in', tfMessage, tf_cb)
+
+    # filter tf_static_in topic
+    tf_static_pub = rospy.Publisher('tf_static_out', tfMessage, queue_size=1, latch=True)
+
+    def tf_static_cb(msg):
+        msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames]
+        if len(msg.transforms) > 0:
+            tf_static_pub.publish(msg)
+
+    rospy.Subscriber('tf_static_in', tfMessage, tf_static_cb)
+
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cititruck_driver</name>
+  <version>1.1.7</version>
+  <description>A reverse ROS bridge for the cititruck robot</description>
+
+  <maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
+  <author email="martin.guenther@dfki.de">Martin Günther</author>
+
+  <license>BSD</license>
+  <license>Apache 2.0</license>
+
+  <url type="website">https://github.com/DFKI-NI/cititruck_robot</url>
+  <url type="repository">https://github.com/DFKI-NI/cititruck_robot</url>
+  <url type="bugtracker">https://github.com/DFKI-NI/cititruck_robot/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roslaunch</build_depend>
+
+  <depend>actionlib_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>geometry_msgs</depend>
+  <depend>move_base_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>python3-websocket</depend>
+  <depend>rosgraph_msgs</depend>
+  <depend>rospy</depend>
+  <depend>rospy_message_converter</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>visualization_msgs</depend>
+
+  <exec_depend>cititruck_description</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+</package>

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/setup.py

@@ -0,0 +1,9 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(packages=['cititruck_driver'], package_dir={'': 'src'})
+
+setup(**setup_args)

Controllers/Packages/robot_gazebo_plugins/cititruck_driver/src/cititruck_driver/rosbridge.py

@@ -0,0 +1,214 @@
+# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the the copyright holder nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import websocket
+import threading
+
+import json
+import traceback
+import time
+
+import string
+import random
+
+
+class RosbridgeSetup:
+    def __init__(self, host, port):
+        self.callbacks = {}
+        self.service_callbacks = {}
+        self.resp = None
+        self.connection = RosbridgeWSConnection(host, port)
+        self.connection.registerCallback(self.onMessageReceived)
+
+    def publish(self, topic, obj):
+        pub = {"op": "publish", "topic": topic, "msg": obj}
+        self.send(pub)
+
+    def subscribe(self, topic, callback, throttle_rate=-1):
+        if self.addCallback(topic, callback):
+            sub = {"op": "subscribe", "topic": topic}
+            if throttle_rate > 0:
+                sub['throttle_rate'] = throttle_rate
+
+            self.send(sub)
+
+    def unhook(self, callback):
+        keys_for_deletion = []
+        for key, values in self.callbacks.items():
+            for value in values:
+                if callback == value:
+                    print("Found!")
+                    values.remove(value)
+                    if len(values) == 0:
+                        keys_for_deletion.append(key)
+
+        for key in keys_for_deletion:
+            self.unsubscribe(key)
+            self.callbacks.pop(key)
+
+    def unsubscribe(self, topic):
+        unsub = {"op": "unsubscribe", "topic": topic}
+        self.send(unsub)
+
+    def callService(self, serviceName, callback=None, msg=None):
+        id = self.generate_id()
+        call = {"op": "call_service", "id": id, "service": serviceName}
+        if msg is not None:
+            call['args'] = msg
+
+        if callback is None:
+            self.resp = None
+
+            def internalCB(msg):
+                self.resp = msg
+                return None
+
+            self.addServiceCallback(id, internalCB)
+            self.send(call)
+
+            while self.resp is None:
+                time.sleep(0.01)
+
+            return self.resp
+
+        self.addServiceCallback(id, callback)
+        self.send(call)
+        return None
+
+    def send(self, obj):
+        try:
+            self.connection.sendString(json.dumps(obj))
+        except Exception:
+            traceback.print_exc()
+            raise
+
+    def generate_id(self, chars=16):
+        return ''.join(random.SystemRandom().choice(string.ascii_letters + string.digits) for _ in range(chars))
+
+    def addServiceCallback(self, id, callback):
+        self.service_callbacks[id] = callback
+
+    def addCallback(self, topic, callback):
+        if topic in self.callbacks:
+            self.callbacks[topic].append(callback)
+            return False
+
+        self.callbacks[topic] = [callback]
+        return True
+
+    def is_connected(self):
+        return self.connection.connected
+
+    def is_errored(self):
+        return self.connection.errored
+
+    def onMessageReceived(self, message):
+        try:
+            # Load the string into a JSON object
+            obj = json.loads(message)
+            # print "Received: ", obj
+
+            if 'op' in obj:
+                option = obj['op']
+                if option == "publish":  # A message from a topic we have subscribed to..
+                    topic = obj["topic"]
+                    msg = obj["msg"]
+                    if topic in self.callbacks:
+                        for callback in self.callbacks[topic]:
+                            try:
+                                callback(msg)
+                            except Exception:
+                                print("exception on callback", callback, "from", topic)
+                                traceback.print_exc()
+                                raise
+                elif option == "service_response":
+                    if "id" in obj:
+                        id = obj["id"]
+                        values = obj["values"]
+                        if id in self.service_callbacks:
+                            try:
+                                # print 'id:', id, 'func:', self.service_callbacks[id]
+                                self.service_callbacks[id](values)
+                            except Exception:
+                                print("exception on callback ID:", id)
+                                traceback.print_exc()
+                                raise
+                    else:
+                        print("Missing ID!")
+                else:
+                    print("Recieved unknown option - it was: ", option)
+            else:
+                print("No OP key!")
+        except Exception:
+            print("exception in onMessageReceived")
+            print("message", message)
+            traceback.print_exc()
+            raise
+
+
+class RosbridgeWSConnection:
+    def __init__(self, host, port):
+        self.ws = websocket.WebSocketApp(
+            ("ws://%s:%d/" % (host, port)), on_message=self.on_message, on_error=self.on_error, on_close=self.on_close
+        )
+        self.ws.on_open = self.on_open
+        self.run_thread = threading.Thread(target=self.run)
+        self.run_thread.start()
+        self.connected = False
+        self.errored = False
+        self.callbacks = []
+
+    def on_open(self):
+        print("### ROS bridge connected ###")
+        self.connected = True
+
+    def sendString(self, message):
+        if not self.connected:
+            print("Error: not connected, could not send message")
+            # TODO: throw exception
+        else:
+            self.ws.send(message)
+
+    def on_error(self, error):
+        self.errored = True
+        print("Error: %s" % error)
+
+    def on_close(self):
+        self.connected = False
+        print("### ROS bridge closed ###")
+
+    def run(self, *args):
+        self.ws.run_forever()
+
+    def on_message(self, message):
+        # Call the handlers
+        for callback in self.callbacks:
+            callback(message)
+
+    def registerCallback(self, callback):
+        self.callbacks.append(callback)