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)

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/CHANGELOG.rst

@@ -0,0 +1,104 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mir_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.7 (2023-01-20)
+------------------
+* Don't set cmake_policy CMP0048
+* Contributors: Martin Günther
+
+1.1.6 (2022-06-02)
+------------------
+* Add arg mir_type to launch files and urdfs
+* Rename mir_100 -> mir
+  This is in preparation of mir_250 support.
+* Contributors: Martin Günther
+
+1.1.5 (2022-02-11)
+------------------
+
+1.1.4 (2021-12-10)
+------------------
+* Remove outdated comment
+* Contributors: Martin Günther
+
+1.1.3 (2021-06-11)
+------------------
+* Merge branch 'melodic-2.8' into noetic
+* Rename tf frame and topic 'odom_comb' -> 'odom'
+  This is how they are called on the real MiR since MiR software 2.0.
+* Contributors: Martin Günther
+
+1.1.2 (2021-05-12)
+------------------
+* Fix laser scan frame_id with gazebo_plugins 2.9.2
+* Contributors: Martin Günther
+
+1.1.1 (2021-02-11)
+------------------
+* mir_gazebo: Add model_name arg
+* Move joint_state_publisher to mir_gazebo_common.launch
+* Add optional namespace to launch files
+* Add prepend_prefix_to_laser_frame to URDF and launch files
+  Fixes `#65 <https://github.com/DFKI-NI/mir_robot/issues/65>`_.
+* Add tf_prefix to URDF and launch files
+* Contributors: Martin Günther
+
+1.1.0 (2020-06-30)
+------------------
+* Initial release into noetic
+* Contributors: Martin Günther
+
+1.0.6 (2020-06-30)
+------------------
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.0.5 (2020-05-01)
+------------------
+
+1.0.4 (2019-05-06)
+------------------
+* Fix gazebo launch file
+  Before this commit, the mobile base plugin couldn't initialize, because
+  subst_value didn't work.
+* Contributors: Martin Günther
+
+1.0.3 (2019-03-04)
+------------------
+* Add hector_mapping
+* fake_localization.launch: Add frame id args
+* Merge pull request `#16 <https://github.com/DFKI-NI/mir_robot/issues/16>`_ from niniemann/add-prefix-argument-to-configs
+  Add prefix argument to configs
+* adds $(arg prefix) to a lot of configs
+  This is an important step to be able to re-parameterize move base,
+  the diffdrive controller, ekf, amcl and the costmaps for adding a
+  tf prefix to the robots links
+* Fix translation error in odom_comb (`#12 <https://github.com/DFKI-NI/mir_robot/issues/12>`_)
+  Previously, the ekf localization only computed a correct orientation, but the translation still followed the pure odometry data. This led to strange errors where the robot would move sideways (despite only having a diff drive).
+  This PR changes the ekf configuration to not use any position information from the odometry, but to integrate the velocities, which fixes this problem.
+* Split scan_rep117 topic into two separate topics
+  This fixes the problem that the back laser scanner was ignored in the
+  navigation costmap in Gazebo (probably because in Gazebo, both laser
+  scanners have the exact same timestamp).
+* Contributors: Martin Günther, Nils Niemann
+
+1.0.2 (2018-07-30)
+------------------
+* mir_gazebo: Install config directory
+* Contributors: Martin Günther
+
+1.0.1 (2018-07-17)
+------------------
+* gazebo: Replace robot_pose_ekf with robot_localization
+  robot_pose_ekf is deprecated, and has been removed from the navigation
+  stack starting in melodic.
+* gazebo: Adjust ekf.yaml
+* gazebo: Copy robot_localization/ekf_template.yaml
+  ... for modification.
+* Contributors: Martin Günther
+
+1.0.0 (2018-07-12)
+------------------
+* Initial release
+* Contributors: Martin Günther

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(cititruck_gazebo)
+
+find_package(catkin REQUIRED COMPONENTS
+  roslaunch
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package()
+
+#############
+## Install ##
+#############
+
+# Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY
+  config
+  launch
+  maps
+  sdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/config/ekf.yaml

@@ -0,0 +1,217 @@
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+frequency: 40
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+sensor_timeout: 0.1
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+transform_time_offset: 0.0
+
+# Use this parameter to specify how long the tf listener should wait for a transform to become available.
+# Defaults to 0.0 if unspecified.
+transform_timeout: 0.0
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+print_diagnostics: true
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+map_frame: map                                   # Defaults to "map" if unspecified
+odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
+base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+odom0: odom
+
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+odom0_config: [false, false, false,   # x y z
+               false, false, false,   # roll pitch yaw
+               true,  true,  false,   # vx vy vz
+               false, false, true,    # vroll vpitch vyaw
+               false, false, false]   # ax ay az
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+odom0_queue_size: 10
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+odom0_differential: false
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+#odom0_pose_rejection_threshold: 5
+#odom0_twist_rejection_threshold: 1
+
+# Further input parameter examples
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+imu0: imu_data
+imu0_config: [false, false, false,   # x y z
+              false, false, true,    # roll pitch yaw
+              false, false, false,   # vx vy vz
+              false, false, true,    # vroll vpitch vyaw
+              true,  false, false]    # ax ay az
+imu0_nodelay: false
+imu0_differential: false
+imu0_relative: true
+imu0_queue_size: 10
+#imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
+#imu0_twist_rejection_threshold: 0.8                #
+#imu0_linear_acceleration_rejection_threshold: 0.8  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+imu0_remove_gravitational_acceleration: false
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]
+
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+initial_estimate_covariance: [100.0,0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    100.0,0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    1.0,  0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     10.0, 0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    10.0, 0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_empty_world.launch

@@ -0,0 +1,64 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable, but can also be an absolute path -->
+
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <arg name="robot_type" default="cititruck-01" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
+
+  <group if="$(eval namespace != '')">
+    <group>
+      <remap from="$(arg namespace)/joint_states"                   to="$(arg namespace)/cititruck/joint_states" />
+      <remap from="$(arg namespace)/mobile_base_controller/cmd_vel" to="$(arg namespace)/cmd_vel" />
+      <remap from="$(arg namespace)/mobile_base_controller/odom"    to="$(arg namespace)/odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="true" />
+        <arg name="gui" value="$(arg gui)" />
+      </include>
+    </group>
+
+    <group ns="$(arg namespace)">
+      <!-- spawn robot and bring up controllers etc. -->
+      <include file="$(find cititruck_gazebo)/launch/cititruck_gazebo_common.launch">
+        <arg name="robot_x"   value="$(arg robot_x)" />
+        <arg name="robot_y"   value="$(arg robot_y)" />
+        <arg name="robot_yaw" value="$(arg robot_yaw)" />
+        <arg name="robot_type" value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+      </include>
+    </group>
+  </group>
+
+  <!-- Duplicate of the above in case namespace is empty. This is necessary to
+       avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
+  <group unless="$(eval namespace != '')">
+    <group>
+      <remap from="joint_states"                   to="cititruck/joint_states" />
+      <remap from="mobile_base_controller/cmd_vel" to="cmd_vel" />
+      <remap from="mobile_base_controller/odom"    to="odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="true" />
+        <arg name="gui" value="$(arg gui)" />
+      </include>
+    </group>
+
+    <!-- spawn robot and bring up controllers etc. -->
+    <include file="$(find cititruck_gazebo)/launch/cititruck_gazebo_common.launch">
+      <arg name="robot_x"   value="$(arg robot_x)" />
+      <arg name="robot_y"   value="$(arg robot_y)" />
+      <arg name="robot_yaw" value="$(arg robot_yaw)" />
+      <arg name="robot_type" value="$(arg robot_type)" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+  </group>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_gazebo_common.launch

@@ -0,0 +1,71 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <arg name="robot_type" default="cititruck-01" />
+  
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="prefix" value="$(arg tf_prefix)/" if="$(eval tf_prefix != '')" /> <!-- $(arg prefix) is used in all the config files! TODO: For multiple robots, create groups when loading the parameters to overwrite the arg? -->
+  <arg name="prefix" value=""                  unless="$(eval tf_prefix != '')" />
+
+  <arg name="model_name" default="cititruck" doc="Name of the Gazebo robot model (needs to be different for each robot)" />
+
+
+  <!-- Load URDF -->
+  <include file="$(find cititruck_description)/launch/upload_cititruck_urdf.launch">
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+
+  <!-- Spawn the robot into Gazebo -->
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg model_name)
+    -x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw) " respawn="false"/>
+
+<!-- Load ros_control controller configurations -->
+  <rosparam file="$(find cititruck_description)/config/joint_state_controller.yaml" command="load" />
+  <rosparam file="$(find cititruck_description)/config/steerdrive_controller.yaml" command="load" subst_value="true" />
+
+  <!-- Start the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+    args="joint_state_controller mobile_base_controller"/>
+
+  <!-- EKF -->
+  <include file="$(find cititruck_gazebo)/launch/includes/ekf.launch.xml">
+      <arg name="tf_prefix" value="$(arg prefix)" />
+  </include>
+
+  <!-- Add passive + mimic joints to joint_states topic -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen" >
+    <rosparam param="source_list">[cititruck/joint_states]</rosparam>
+    <param name="rate" value="200.0" />
+  </node>
+
+  <!-- Robot state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
+
+  <!-- Load teleop -->
+  <node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering">
+    <param name="default_topic" value="cmd_vel"/>
+    <param name="default_vx_max" value="1.0" />
+    <param name="default_vx_min" value="-1.0" />
+    <param name="default_vw_max" value="1.57079" />
+    <param name="default_vw_min" value="-1.57079" />
+  </node>
+
+  <!-- create combined scan topic (like on real cititruck) -->
+  <node pkg="topic_tools" type="relay" name="l_scan_relay" args="l_scan scan"/>
+  <node pkg="topic_tools" type="relay" name="r_scan_relay" args="r_scan scan"/>
+
+  <node name="l_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="l_scan" />
+    <remap from="scan_filtered" to="l_scan_rep117" />
+  </node>
+
+  <node name="r_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="r_scan" />
+    <remap from="scan_filtered" to="r_scan_rep117" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_maze_world.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="robot_type" default="cititruck-01" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <include file="$(find cititruck_gazebo)/launch/cititruck_empty_world.launch">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="robot_x"   value="$(arg robot_x)" />
+    <arg name="robot_y"   value="$(arg robot_y)" />
+    <arg name="robot_yaw" value="$(arg robot_yaw)" />
+  </include>
+
+  <include file="$(find cititruck_gazebo)/launch/includes/spawn_maze.launch.xml" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/fake_localization.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="delta_x" default="0.0" />
+  <arg name="delta_y" default="0.0" />
+  <arg name="delta_yaw" default="0.0" />
+  <arg name="odom_frame_id" default="odom"/>
+  <arg name="base_frame_id" default="base_footprint"/>
+
+  <node name="fake_localization" pkg="fake_localization" type="fake_localization" output="screen">
+    <param name="odom_frame_id" value="$(arg odom_frame_id)"/>
+    <param name="base_frame_id" value="$(arg base_frame_id)"/>
+    <param name="delta_x"       value="$(arg delta_x)" />
+    <param name="delta_y"       value="$(arg delta_y)" />
+    <param name="delta_yaw"     value="$(arg delta_yaw)" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/includes/ekf.launch.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_prefix" default="" />
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find cititruck_gazebo)/config/ekf.yaml" subst_value="true" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/includes/spawn_maze.launch.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" args="-sdf
+      -file $(find cititruck_gazebo)/sdf/maze/model.sdf -model walls" output="screen" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maps_new/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maps_new/maze_virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: maze_virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maze_virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: maze_virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/critical_zones.yaml

@@ -0,0 +1,6 @@
+image: critical_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/direction_zones.yaml

@@ -0,0 +1,6 @@
+image: direction_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/map_empty.yaml

@@ -0,0 +1,6 @@
+image: map_empty.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [-13.0, -2.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/preferred_zones.yaml

@@ -0,0 +1,6 @@
+image: preferred_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/unpreferred_zones.yaml

@@ -0,0 +1,6 @@
+image: unpreferred_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cititruck_gazebo</name>
+  <version>1.1.7</version>
+  <description>Simulation specific launch and configuration files 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>
+
+  <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>
+
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>fake_localization</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>cititruck_description</exec_depend>
+  <exec_depend>cititruck_driver</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rostopic</exec_depend>
+  <exec_depend>rqt_robot_steering</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+</package>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/sdf/maze/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>maze</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name>Martin Günther</name>
+        <email>martin.guenther@dfki.de</email>
+    </author>
+    <description></description>
+</model>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/sdf/maze/model.sdf

@@ -0,0 +1,345 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='maze'>
+    <pose frame=''>-0.078283 0.098984 0 0 -0 0</pose>
+    <link name='Wall_0'>
+      <collision name='Wall_0_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_0_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 9.925 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_1'>
+      <collision name='Wall_1_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_1_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>9.95554 0 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_2'>
+      <collision name='Wall_2_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_2_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 -9.925 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_24'>
+      <collision name='Wall_24_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_24_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>5.35089 3.21906 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_25'>
+      <collision name='Wall_25_Collision'>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_25_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>4.67589 5.76906 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_27'>
+      <collision name='Wall_27_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_27_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>7.10914 4.73454 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_28'>
+      <collision name='Wall_28_Collision'>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_28_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>8.53414 2.05954 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_3'>
+      <collision name='Wall_3_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_3_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-9.89446 0 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_30'>
+      <collision name='Wall_30_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_30_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-4.35914 -2.82889 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_31'>
+      <collision name='Wall_31_Collision'>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_31_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-7.15914 -5.50389 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_5'>
+      <collision name='Wall_5_Collision'>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_5_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-1.89911 1.86906 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_6'>
+      <collision name='Wall_6_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_6_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 2.54406 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_8'>
+      <collision name='Wall_8_Collision'>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_8_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 3.21906 0 0 -0 0</pose>
+    </link>
+    <static>1</static>
+  </model>
+</sdf>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/CHANGELOG.rst

@@ -0,0 +1,79 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package diff_drive_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.9.3 (2016-02-12)
+------------------
+* Reduced pedantry, redundancy.
+* Added tests for the odom_frame_id parameter.
+* Parameterized diff_drive_controller's odom_frame_id
+* add check for multiple publishers on cmd_vel
+* Address -Wunused-parameter warnings
+* Limit jerk
+* Add param velocity_rolling_window_size
+* Minor fixes
+  1. Coding style
+  2. Tolerance to fall-back to Runge-Kutta 2 integration
+  3. Remove unused variables
+* Fix forward test
+  Fix the following bugs in the testForward test:
+  1. Check traveled distance in XY plane
+  2. Use expected speed variable on test check
+* Add test for NaN
+* Add test for bad URDF
+  This unit test exercises a controller load failure caused by
+  a wrong wheel geometry. The controller requires that wheels be
+  modeled by cylinders, while the bad URDF uses spheres.
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Bence Magyar, Enrique Fernandez, Eric Tappan, Karsten Knese, Paul Mathieu, tappan-at-git
+
+0.9.2 (2015-05-04)
+------------------
+* Allow the wheel separation and radius to be set from different sources
+  i.e. one can be set from the URDF, the other from the parameter server.
+  If wheel separation and wheel diameter is specified in the parameter server, don't look them up from urdf
+* Contributors: Bence Magyar, Nils Berg
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* Add support for multiple wheels per side
+* Odometry computation:
+  - New option to compute in open loop fashion
+  - New option to skip publishing odom frame to tf
+* Remove dependency on angles package
+* Buildsystem fixes
+* Contributors: Bence Magyar, Lukas Bulwahn, efernandez
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Add base_frame_id param (defaults to base_link)
+  The nav_msgs/Odometry message specifies the child_frame_id field,
+  which was previously not set.
+  This commit creates a parameter to replace the previously hard-coded
+  value of the child_frame_id of the published tf frame, and uses it
+  in the odom message as well.
+* Contributors: enriquefernandez
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+* Changed test-depend to build-depend for release jobs.
+* Contributors: Bence Magyar
+
+0.7.0 (2014-03-28)
+------------------
+* diff_drive_controller: New controller for differential drive wheel systems.
+* Control is in the form of a velocity command, that is split then sent on the two wheels of a differential drive
+wheel base.
+* Odometry is published to tf and to a dedicated nav__msgs/Odometry topic.
+* Realtime-safe implementation.
+* Implements task-space velocity and acceleration limits.
+* Automatic stop after command time-out.
+* Contributors: Bence Magyar, Paul Mathieu, Enrique Fernandez.

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/CMakeLists.txt

@@ -0,0 +1,103 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(steer_drive_controller)
+
+find_package(catkin REQUIRED COMPONENTS
+  controller_interface
+  urdf
+  realtime_tools
+  tf
+  nav_msgs
+  roscpp
+  angles
+  control_toolbox
+  gazebo_ros_control
+  hardware_interface
+  joint_limits_interface
+)
+
+find_package(gazebo REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS
+  roscpp
+  angles
+  control_toolbox
+  gazebo_ros_control
+  hardware_interface
+  joint_limits_interface
+)
+
+include_directories(
+  include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}
+)
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+list(APPEND CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+add_library(${PROJECT_NAME} src/steer_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES steer_drive_controller_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS
+    rostest
+    std_srvs
+    controller_manager
+    tf
+  )
+  include_directories(include ${catkin_INCLUDE_DIRS})
+
+  add_executable(${PROJECT_NAME}_steerbot test/common/src/steerbot.cpp)
+  target_link_libraries(${PROJECT_NAME}_steerbot ${catkin_LIBRARIES})
+
+  add_dependencies(tests ${PROJECT_NAME}_steerbot)
+
+  add_rostest_gtest(${PROJECT_NAME}_test
+    test/steer_drive_controller_test/steer_drive_controller.test
+    test/steer_drive_controller_test/steer_drive_controller_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES})
+  add_rostest_gtest(${PROJECT_NAME}_nan_test
+    test/steer_drive_controller_nan_test/steer_drive_controller_nan.test
+    test/steer_drive_controller_nan_test/steer_drive_controller_nan_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_nan_test ${catkin_LIBRARIES})
+  add_rostest_gtest(${PROJECT_NAME}_limits_test
+    test/steer_drive_controller_limits_test/steer_drive_controller_limits.test
+    test/steer_drive_controller_limits_test/steer_drive_controller_limits_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_limits_test ${catkin_LIBRARIES})
+  add_rostest_gtest(${PROJECT_NAME}_timeout_test
+    test/steer_drive_controller_timeout_test/steer_drive_controller_timeout.test
+    test/steer_drive_controller_timeout_test/steer_drive_controller_timeout_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_timeout_test ${catkin_LIBRARIES})
+  add_rostest_gtest(steer_drive_controller_fail_test
+    test/steer_drive_controller_fail_test/steer_drive_controller_wrong.test
+    test/steer_drive_controller_fail_test/steer_drive_controller_fail_test.cpp)
+  target_link_libraries(steer_drive_controller_fail_test ${catkin_LIBRARIES})
+  add_rostest_gtest(${PROJECT_NAME}_odom_tf_test
+    test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf.test
+    test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_odom_tf_test ${catkin_LIBRARIES})
+  add_rostest_gtest(${PROJECT_NAME}_default_odom_frame_test
+    test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame.test
+    test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_default_odom_frame_test ${catkin_LIBRARIES})
+  add_rostest_gtest(steer_drive_controller_odom_frame_test
+    test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame.test
+    test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_odom_frame_test ${catkin_LIBRARIES})
+  add_rostest(test/steer_drive_controller_open_loop_test/steer_drive_controller_open_loop.test)
+  add_rostest(test/steer_drive_controller_no_wheel_test/steer_drive_controller_no_wheel.test)
+  add_rostest(test/steer_drive_controller_no_steer_test/steer_drive_controller_no_steer.test)
+  add_rostest(test/steer_drive_controller_radius_param_test/steer_drive_controller_radius_param.test)
+  add_rostest(test/steer_drive_controller_radius_param_fail_test/steer_drive_controller_radius_param_fail.test)
+  add_rostest(test/steer_drive_controller_separation_param_test/steer_drive_controller_separation_param.test)
+endif()

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/README.md

@@ -0,0 +1,80 @@
+## Steer Drive Controller ##
+
+Controller for a steer drive mobile base.
+
+## 仕様
+- Subscribe
+  - `steer_drive_controller/cmd_vel` にTwist型のメッセージを投げて下さい．
+- Publish
+  - `steer_drive_controller/odom` を出すべきなのですが，未完成です．
+
+- 例：SteerDriveController + SteerBot(RobotHWSim) で起動して，以下のメッセージで動く
+```bash
+rostopic pub -1 steer_drive_controller/cmd_vel geometry_msgs/Twist -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.5]'
+```
+
+## third_robotのlinkメモ
+### ベース
+- base_link
+
+### ステアリング
+- steer
+
+### ホイール
+- left_front_wheel
+- left_rear_wheel
+- right_front_wheel
+- right_rear_wheel
+
+### センサ
+- camera_link
+- front_bottom_lrf
+- front_top_lrf
+- rear_bottom_lrf
+
+## third_robotのjointメモ
+### ホイール
+- base_to_left_front_wheel
+- base_to_left_rear_wheel
+- base_to_right_front_wheel
+- base_to_right_rear_wheel
+
+### ステアリング
+- base_to_steer
+- base_to_steer_right: gazebo用のvirtual.
+- base_to_steer_left: gazebo用のvirtual.
+
+## デバッグ開発の手順
+### 準備
+- パスを通す
+
+```bash
+source path.bash
+```
+
+- gazeboを実行する
+```bash
+roslaunch third_robot_gazebo third_robot_world.launch 
+```
+
+- コントローラ動作用の最低限の設定を行うlaunchファイルを実行する
+```bash
+roslaunch steer_drive_controller steer_drive_test_setting.launch 
+```
+
+### デバッガ
+- パスを通したコンソールでQtCreatorを起動する
+```bash
+qtcreator
+```
+
+- [QtCreatorでROSのパッケージをビルド&デバッグ実行する](http://qiita.com/MoriKen/items/ea41e485929e0724d15e)にしたがってビルドする．実行オプションで`steer_drive_test`を選択する．
+
+- `third_robot_control/steer_drive_controller/include/steer_drive_controller.h`内で`#define GUI_DEBUG`をアンコメントアウトする．
+ - 53行目：`#define GUI_DEBUG // uncommentout when you use qtcreator for debugging` ってとこ．
+ - デバッグ用のモジュールに名前空間がついてないので，無理やり付与する設定．
+
+- `third_robot_control/steer_drive_controller/test/steer_drive_test.cpp` のmain関数内でブレイクポイントを置いてデバッグ実行．
+  - SteerRobotはフェイクで作ってある．
+  - `bool rt_code = rb_controller.init(steer_drive_bot, nh_main, nh_control);` でコントローラの初期化
+  

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/config/joint_state_controller.yaml

@@ -0,0 +1,4 @@
+# Publish all joint states -----------------------------------
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/config/steerdrive_controller.yaml

@@ -0,0 +1,53 @@
+# -----------------------------------
+mobile_base_controller:
+  type        : "steer_drive_controller/SteerDriveController"
+  rear_wheel: '$(arg prefix)steer2sd_wheel_joint'
+  front_steer: '$(arg prefix)base2steer_joint'
+  publish_rate: 41.2               # this is what the real MiR platform publishes (default: 50)
+  
+  pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  enable_odom_tf: false
+
+  # Wheel separation and diameter. These are both optional.
+  # diff_drive_controller will attempt to read either one or both from the
+  # URDF if not specified as a parameter.
+  # We don't set the value here because it's different for each MiR type (100, 250, ...), and
+  # the plugin figures out the correct values.
+  wheel_separation : 1.3268
+  wheel_radius : 0.125
+
+  # Wheel separation and radius multipliers
+  wheel_separation_multiplier: 1.0 # default: 1.0
+  wheel_radius_multiplier    : 1.0 # default: 1.0
+
+  # Steer position angle multipliers for fine tuning.
+  steer_pos_multiplier       : 1.0
+
+  # Velocity commands timeout [s], default 0.5
+  cmd_vel_timeout: 0.25
+
+  # frame_ids (same as real MiR platform)
+  base_frame_id: $(arg prefix)base_footprint # default: base_link
+  odom_frame_id: $(arg prefix)odom           # default: odom
+
+  # Velocity and acceleration limits
+  # Whenever a min_* is unspecified, default to -max_*
+  linear:
+    x:
+      has_velocity_limits    : true
+      max_velocity           : 1.0  # m/s
+      min_velocity           : -0.5 # m/s
+      has_acceleration_limits: true
+      max_acceleration       : 1.38  # m/s^2
+      min_acceleration       : -1.38 # m/s^2
+      has_jerk_limits        : true
+      max_jerk               : 5.0 # m/s^3
+  angular:
+    z:
+      has_velocity_limits    : true
+      max_velocity           : 1.5  # rad/s
+      has_acceleration_limits: true
+      max_acceleration       : 0.857142857  # rad/s^2
+      has_jerk_limits        : true
+      max_jerk               : 2.5 # rad/s^3

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/include/steer_drive_controller/odometry.h

@@ -0,0 +1,209 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  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 PAL Robotics 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 OWNER 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: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+
+#ifndef ODOMETRY_H_
+#define ODOMETRY_H_
+
+#include <ros/time.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/rolling_mean.hpp>
+#include <boost/function.hpp>
+
+namespace steer_drive_controller
+{
+  namespace bacc = boost::accumulators;
+
+  /**
+   * \brief The Odometry class handles odometry readings
+   * (2D pose and velocity with related timestamp)
+   */
+  class Odometry
+  {
+  public:
+
+    /// Integration function, used to integrate the odometry:
+    typedef boost::function<void(double, double)> IntegrationFunction;
+
+    /**
+     * \brief Constructor
+     * Timestamp will get the current time value
+     * Value will be set to zero
+     * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+     */
+    Odometry(size_t velocity_rolling_window_size = 10);
+
+    /**
+     * \brief Initialize the odometry
+     * \param time Current time
+     */
+    void init(const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest wheels position
+     * \param rear_wheel_pos  Rear wheel position [rad]
+     * \param front_steer_pos Front Steer position [rad]
+     * \param time      Current time
+     * \return true if the odometry is actually updated
+     */
+    bool update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest velocity command
+     * \param linear  Linear velocity [m/s]
+     * \param angular Angular velocity [rad/s]
+     * \param time    Current time
+     */
+    void updateOpenLoop(double linear, double angular, const ros::Time &time);
+
+    /**
+     * \brief heading getter
+     * \return heading [rad]
+     */
+    double getHeading() const
+    {
+      return heading_;
+    }
+
+    /**
+     * \brief x position getter
+     * \return x position [m]
+     */
+    double getX() const
+    {
+      return x_;
+    }
+
+    /**
+     * \brief y position getter
+     * \return y position [m]
+     */
+    double getY() const
+    {
+      return y_;
+    }
+
+    /**
+     * \brief linear velocity getter
+     * \return linear velocity [m/s]
+     */
+    double getLinear() const
+    {
+      return linear_;
+    }
+
+    /**
+     * \brief angular velocity getter
+     * \return angular velocity [rad/s]
+     */
+    double getAngular() const
+    {
+      return angular_;
+    }
+
+    /**
+     * \brief Sets the wheel parameters: radius and separation
+     * \param wheel_separation Seperation between left and right wheels [m]
+     * \param wheel_radius     Wheel radius [m]
+     */
+    void setWheelParams(double wheel_reparation_h, double wheel_radius);
+
+    /**
+     * \brief Velocity rolling window size setter
+     * \param velocity_rolling_window_size Velocity rolling window size
+     */
+    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+  private:
+
+    /// Rolling mean accumulator and window:
+    typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
+    typedef bacc::tag::rolling_window RollingWindow;
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateRungeKutta2(double linear, double angular);
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using exact method
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateExact(double linear, double angular);
+
+    /**
+     *  \brief Reset linear and angular accumulators
+     */
+    void resetAccumulators();
+
+    /// Current timestamp:
+    ros::Time timestamp_;
+
+    /// Current pose:
+    double x_;        //   [m]
+    double y_;        //   [m]
+    double heading_;  // [rad]
+
+    /// Current velocity:
+    double linear_;  //   [m/s]
+    double angular_; // [rad/s]
+
+    /// Wheel kinematic parameters [m]:
+    double wheel_separation_h_;
+    double wheel_radius_;
+
+    /// Previous wheel position/state [rad]:
+    double rear_wheel_old_pos_;
+
+    /// Rolling mean accumulators for the linar and angular velocities:
+    size_t velocity_rolling_window_size_;
+    RollingMeanAcc linear_acc_;
+    RollingMeanAcc angular_acc_;
+
+    /// Integration funcion, used to integrate the odometry:
+    IntegrationFunction integrate_fun_;
+  };
+}
+
+#endif /* ODOMETRY_H_ */

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/include/steer_drive_controller/speed_limiter.h

@@ -0,0 +1,131 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  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 PAL Robotics 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 OWNER 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: Enrique Fernández
+ */
+
+#ifndef SPEED_LIMITER_H
+#define SPEED_LIMITER_H
+
+namespace steer_drive_controller
+{
+
+  class SpeedLimiter
+  {
+  public:
+
+    /**
+     * \brief Constructor
+     * \param [in] has_velocity_limits     if true, applies velocity limits
+     * \param [in] has_acceleration_limits if true, applies acceleration limits
+     * \param [in] has_jerk_limits         if true, applies jerk limits
+     * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
+     * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
+     * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
+     * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+     * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
+     * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+     */
+    SpeedLimiter(
+      bool has_velocity_limits = false,
+      bool has_acceleration_limits = false,
+      bool has_jerk_limits = false,
+      double min_velocity = 0.0,
+      double max_velocity = 0.0,
+      double min_acceleration = 0.0,
+      double max_acceleration = 0.0,
+      double min_jerk = 0.0,
+      double max_jerk = 0.0
+    );
+
+    /**
+     * \brief Limit the velocity and acceleration
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity to v  [m/s]
+     * \param [in]      v1 Previous velocity to v0 [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit(double& v, double v0, double v1, double dt);
+
+    /**
+     * \brief Limit the velocity
+     * \param [in, out] v Velocity [m/s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit_velocity(double& v);
+
+    /**
+     * \brief Limit the acceleration
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit_acceleration(double& v, double v0, double dt);
+
+    /**
+     * \brief Limit the jerk
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity to v  [m/s]
+     * \param [in]      v1 Previous velocity to v0 [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
+     */
+    double limit_jerk(double& v, double v0, double v1, double dt);
+
+  public:
+    // Enable/Disable velocity/acceleration/jerk limits:
+    bool has_velocity_limits;
+    bool has_acceleration_limits;
+    bool has_jerk_limits;
+
+    // Velocity limits:
+    double min_velocity;
+    double max_velocity;
+
+    // Acceleration limits:
+    double min_acceleration;
+    double max_acceleration;
+
+    // Jerk limits:
+    double min_jerk;
+    double max_jerk;
+  };
+
+} // namespace diff_drive_controller
+
+#endif // SPEED_LIMITER_H

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/include/steer_drive_controller/steer_drive_controller.h

@@ -0,0 +1,225 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  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 PAL Robotics 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 OWNER 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: Enrique Fernández
+ */
+
+#include <controller_interface/controller.h>
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <nav_msgs/Odometry.h>
+#include <tf/tfMessage.h>
+
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <steer_drive_controller/odometry.h>
+#include <steer_drive_controller/speed_limiter.h>
+
+//#define GUI_DEBUG // uncommentout when you use qtcreator for debugging
+
+namespace steer_drive_controller{
+
+  /**
+   * This class makes some assumptions on the model of the robot:
+   *  - the rotation axes of wheels are collinear
+   *  - the wheels are identical in radius
+   * Additional assumptions to not duplicate information readily available in the URDF:
+   *  - the wheels have the same parent frame
+   *  - a wheel collision geometry is a cylinder in the urdf
+   *  - a wheel joint frame center's vertical projection on the floor must lie within the contact patch
+   */
+  class SteerDriveController
+      : public controller_interface::MultiInterfaceController<
+      hardware_interface::PositionJointInterface,
+      hardware_interface::VelocityJointInterface>
+  {
+    // constants
+  private:
+    enum INDX_WHEEL {
+      INDX_WHEEL_FRNT = 0,
+      INDX_WHEEL_MID = 1,
+      INDX_WHEEL_BACK = 2,
+    };
+    enum INDX_STEER {
+      INDX_STEER_FRNT = 0,
+      INDX_STEER_BACK = 1,
+    };
+    // constant
+    enum INDEX_WHEEL {
+      INDEX_RIGHT = 0,
+      INDEX_LEFT = 1
+    };
+  public:
+    SteerDriveController();
+
+    /**
+     * \brief Initialize controller
+     * \param hw            Velocity joint interface for the wheels
+     * \param root_nh       Node handle at root namespace
+     * \param controller_nh Node handle inside the controller namespace
+     */
+    bool init(//hardware_interface::VelocityJointInterface *hw,
+              hardware_interface::RobotHW* robot_hw,
+              ros::NodeHandle& root_nh,
+              ros::NodeHandle &controller_nh);
+
+    /**
+     * \brief Updates controller, i.e. computes the odometry and sets the new velocity commands
+     * \param time   Current time
+     * \param period Time since the last called to update
+     */
+    void update(const ros::Time& time, const ros::Duration& period);
+
+    /**
+     * \brief Starts controller
+     * \param time Current time
+     */
+    void starting(const ros::Time& time);
+
+    /**
+     * \brief Stops controller
+     * \param time Current time
+     */
+    void stopping(const ros::Time& /*time*/);
+
+  private:
+    std::string name_;
+
+    /// Odometry related:
+    ros::Duration publish_period_;
+    ros::Time last_state_publish_time_;
+    bool open_loop_;
+
+    /// Hardware handles:
+    hardware_interface::JointHandle rear_wheel_joint_;
+    hardware_interface::JointHandle front_steer_joint_;
+
+    /// Velocity command related:
+    struct Commands
+    {
+      double lin;
+      double ang;
+      ros::Time stamp;
+
+      Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
+    };
+    realtime_tools::RealtimeBuffer<Commands> command_;
+    Commands command_struct_;
+    ros::Subscriber sub_command_;
+
+    /// Odometry related:
+    boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
+    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
+    Odometry odometry_;
+
+    /// Wheel separation, wrt the midpoint of the wheel width:
+    double wheel_separation_h_;
+
+    /// Wheel radius (assuming it's the same for the left and right wheels):
+    double wheel_radius_;
+
+    /// Wheel separation and radius calibration multipliers:
+    double wheel_separation_h_multiplier_;
+    double wheel_radius_multiplier_;
+    double steer_pos_multiplier_;
+
+    /// Timeout to consider cmd_vel commands old:
+    double cmd_vel_timeout_;
+
+    /// Whether to allow multiple publishers on cmd_vel topic or not:
+    bool allow_multiple_cmd_vel_publishers_;
+
+    /// Frame to use for the robot base:
+    std::string base_frame_id_;
+
+    /// Frame to use for odometry and odom tf:
+    std::string odom_frame_id_;
+
+    /// Whether to publish odometry to tf or not:
+    bool enable_odom_tf_;
+
+    /// Number of wheel joints:
+    size_t wheel_joints_size_;
+
+    /// Number of steer joints:
+    size_t steer_joints_size_;
+
+    /// Speed limiters:
+    Commands last1_cmd_;
+    Commands last0_cmd_;
+    SpeedLimiter limiter_lin_;
+    SpeedLimiter limiter_ang_;
+
+    // FOR_DEBUG
+    std::string ns_;
+
+  private:
+    /**
+     * \brief Brakes the wheels, i.e. sets the velocity to 0
+     */
+    void brake();
+
+    /**
+     * \brief Velocity command callback
+     * \param command Velocity command message (twist)
+     */
+    void cmdVelCallback(const geometry_msgs::Twist& command);
+
+    /**
+     * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation
+     * \param root_nh Root node handle
+     * \param left_wheel_name Name of the left wheel joint
+     * \param right_wheel_name Name of the right wheel joint
+     */
+    bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
+                               const std::string rear_wheel_name,
+                               const std::string front_steer_name,
+                               bool lookup_wheel_separation_h,
+                               bool lookup_wheel_radius);
+
+    /**
+     * \brief Sets the odometry publishing fields
+     * \param root_nh Root node handle
+     * \param controller_nh Node handle inside the controller namespace
+     */
+    void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+
+  };
+
+  PLUGINLIB_EXPORT_CLASS(steer_drive_controller::SteerDriveController, controller_interface::ControllerBase)
+} // namespace diff_drive_controller

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/package.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>steer_drive_controller</name>
+  <version>0.1.0</version>
+  <description>Controller for a steer drive mobile base.</description>
+  <maintainer email="p595201m@mail.kyutech.jp">Masaru Morita</maintainer>
+
+  <license>BSD</license>
+
+  <url type="bugtracker">https://github.com/CIR-KIT/steer_drive_ros/issues</url>
+  <url type="repository">https://github.com/CIR-KIT/steer_drive_ros.git</url>
+  <author email="cirkit.infomation@gmail.com">CIR-KIT</author>
+  <author email="p595201m@mail.kyutech.jp">Masaru Morita</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>angles</depend>
+  <depend>controller_interface</depend>
+  <depend>control_toolbox</depend>
+  <depend>gazebo_ros_control</depend>
+  <depend>hardware_interface</depend>
+  <depend>joint_limits_interface</depend>
+  <depend>nav_msgs</depend>
+  <depend>realtime_tools</depend>
+  <depend>roscpp</depend>
+  <depend>tf</depend>
+  <depend>urdf</depend>
+
+  <build_depend>gazebo</build_depend>
+
+  <!--Tests-->
+  <build_depend>rostest</build_depend>
+
+  <test_depend>controller_manager</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+  <test_depend>std_srvs</test_depend>
+  <test_depend>xacro</test_depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/steer_drive_controller_plugins.xml"/>
+  </export>
+</package>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/path.bash

@@ -0,0 +1,9 @@
+#!/bin/bash
+
+# include path を作成する
+include_path=$(pwd)/include
+gazebo_path=/usr/include/gazebo-2.2/
+sdf_path=/usr/include/sdformat-1.4/
+
+# CPATHに追加
+export CPATH=${CPATH}:${include_path}:${gazebo_path}:${sdf_path}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/src/odometry.cpp

@@ -0,0 +1,174 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  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 PAL Robotics 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 OWNER 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: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+
+#include <steer_drive_controller/odometry.h>
+
+#include <boost/bind.hpp>
+
+namespace steer_drive_controller
+{
+  namespace bacc = boost::accumulators;
+
+  Odometry::Odometry(size_t velocity_rolling_window_size)
+  : timestamp_(0.0)
+  , x_(0.0)
+  , y_(0.0)
+  , heading_(0.0)
+  , linear_(0.0)
+  , angular_(0.0)
+  , wheel_separation_h_(0.0)
+  , wheel_radius_(0.0)
+  , rear_wheel_old_pos_(0.0)
+  , velocity_rolling_window_size_(velocity_rolling_window_size)
+  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
+  {
+  }
+
+  void Odometry::init(const ros::Time& time)
+  {
+    // Reset accumulators and timestamp:
+    resetAccumulators();
+    timestamp_ = time;
+  }
+
+  bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time)
+  {
+    /// Get current wheel joint positions:
+    const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_;
+
+    /// Estimate velocity of wheels using old and current position:
+    //const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
+    //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
+
+    const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_;
+
+    /// Update old position with current:
+    rear_wheel_old_pos_ = rear_wheel_cur_pos;
+
+    /// Compute linear and angular diff:
+    const double linear  = rear_wheel_est_vel * cos(front_steer_pos);//(right_wheel_est_vel + left_wheel_est_vel) * 0.5;
+    //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_;
+    const double angular = tan(front_steer_pos) * linear / wheel_separation_h_;
+
+    /// Integrate odometry:
+    integrate_fun_(linear, angular);
+
+    /// We cannot estimate the speed with very small time intervals:
+    const double dt = (time - timestamp_).toSec();
+    if (dt < 0.0001)
+      return false; // Interval too small to integrate with
+
+    timestamp_ = time;
+
+    /// Estimate speeds using a rolling mean to filter them out:
+    linear_acc_(linear/dt);
+    angular_acc_(angular/dt);
+
+    linear_ = bacc::rolling_mean(linear_acc_);
+    angular_ = bacc::rolling_mean(angular_acc_);
+
+    return true;
+  }
+
+  void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
+  {
+    /// Save last linear and angular velocity:
+    linear_ = linear;
+    angular_ = angular;
+
+    /// Integrate odometry:
+    const double dt = (time - timestamp_).toSec();
+    timestamp_ = time;
+    integrate_fun_(linear * dt, angular * dt);
+  }
+
+  void Odometry::setWheelParams(double wheel_separation_h, double wheel_radius)
+  {
+    wheel_separation_h_ = wheel_separation_h;
+    wheel_radius_     = wheel_radius;
+  }
+
+  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
+  {
+    velocity_rolling_window_size_ = velocity_rolling_window_size;
+
+    resetAccumulators();
+  }
+
+  void Odometry::integrateRungeKutta2(double linear, double angular)
+  {
+    const double direction = heading_ + angular * 0.5;
+
+    /// Runge-Kutta 2nd order integration:
+    x_       += linear * cos(direction);
+    y_       += linear * sin(direction);
+    heading_ += angular;
+  }
+
+  /**
+   * \brief Other possible integration method provided by the class
+   * \param linear
+   * \param angular
+   */
+  void Odometry::integrateExact(double linear, double angular)
+  {
+    if (fabs(angular) < 1e-6)
+      integrateRungeKutta2(linear, angular);
+    else
+    {
+      /// Exact integration (should solve problems when angular is zero):
+      const double heading_old = heading_;
+      const double r = linear/angular;
+      heading_ += angular;
+      x_       +=  r * (sin(heading_) - sin(heading_old));
+      y_       += -r * (cos(heading_) - cos(heading_old));
+    }
+  }
+
+  void Odometry::resetAccumulators()
+  {
+    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+  }
+
+} // namespace diff_drive_controller

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/src/speed_limiter.cpp

@@ -0,0 +1,137 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  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 PAL Robotics 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 OWNER 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: Enrique Fernández
+ */
+
+#include <algorithm>
+
+#include <steer_drive_controller/speed_limiter.h>
+
+template<typename T>
+T clamp(T x, T min, T max)
+{
+  return std::min(std::max(min, x), max);
+}
+
+namespace steer_drive_controller
+{
+
+  SpeedLimiter::SpeedLimiter(
+    bool has_velocity_limits,
+    bool has_acceleration_limits,
+    bool has_jerk_limits,
+    double min_velocity,
+    double max_velocity,
+    double min_acceleration,
+    double max_acceleration,
+    double min_jerk,
+    double max_jerk
+  )
+  : has_velocity_limits(has_velocity_limits)
+  , has_acceleration_limits(has_acceleration_limits)
+  , has_jerk_limits(has_jerk_limits)
+  , min_velocity(min_velocity)
+  , max_velocity(max_velocity)
+  , min_acceleration(min_acceleration)
+  , max_acceleration(max_acceleration)
+  , min_jerk(min_jerk)
+  , max_jerk(max_jerk)
+  {
+  }
+
+  double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
+  {
+    const double tmp = v;
+
+    limit_jerk(v, v0, v1, dt);
+    limit_acceleration(v, v0, dt);
+    limit_velocity(v);
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_velocity(double& v)
+  {
+    const double tmp = v;
+
+    if (has_velocity_limits)
+    {
+      v = clamp(v, min_velocity, max_velocity);
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
+  {
+    const double tmp = v;
+
+    if (has_acceleration_limits)
+    {
+      const double dv_min = min_acceleration * dt;
+      const double dv_max = max_acceleration * dt;
+
+      const double dv = clamp(v - v0, dv_min, dv_max);
+
+      v = v0 + dv;
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
+  {
+    const double tmp = v;
+
+    if (has_jerk_limits)
+    {
+      const double dv  = v  - v0;
+      const double dv0 = v0 - v1;
+
+      const double dt2 = 2. * dt * dt;
+
+      const double da_min = min_jerk * dt2;
+      const double da_max = max_jerk * dt2;
+
+      const double da = clamp(dv - dv0, da_min, da_max);
+
+      v = v0 + dv0 + da;
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+} // namespace diff_drive_controller

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/src/steer_drive_controller.cpp

@@ -0,0 +1,493 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  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 PAL Robotics 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 OWNER 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: Bence Magyar
+ */
+
+#include <cmath>
+
+#include <tf/transform_datatypes.h>
+
+#include <urdf_parser/urdf_parser.h>
+
+#include <boost/assign.hpp>
+
+#include <steer_drive_controller/steer_drive_controller.h>
+
+static double euclideanOfVectors(const urdf::Vector3 &vec1, const urdf::Vector3 &vec2)
+{
+  return std::sqrt(std::pow(vec1.x - vec2.x, 2) +
+                   std::pow(vec1.y - vec2.y, 2) +
+                   std::pow(vec1.z - vec2.z, 2));
+}
+
+/*
+ * \brief Check if the link is modeled as a cylinder
+ * \param link Link
+ * \return true if the link is modeled as a Cylinder; false otherwise
+ */
+static bool isCylinder(const urdf::LinkConstSharedPtr &link)
+{
+  if (!link)
+  {
+    ROS_ERROR("Link == NULL.");
+    return false;
+  }
+
+  if (!link->collision)
+  {
+    ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
+    return false;
+  }
+
+  if (!link->collision->geometry)
+  {
+    ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
+    return false;
+  }
+
+  if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
+  {
+    ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
+    return false;
+  }
+
+  return true;
+}
+
+/*
+ * \brief Get the wheel radius
+ * \param [in]  wheel_link   Wheel link
+ * \param [out] wheel_radius Wheel radius [m]
+ * \return true if the wheel radius was found; false other
+wise
+ */
+static bool getWheelRadius(const urdf::LinkConstSharedPtr &wheel_link, double &wheel_radius)
+{
+  if (!isCylinder(wheel_link))
+  {
+    ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
+    return false;
+  }
+
+  wheel_radius = (static_cast<urdf::Cylinder *>(wheel_link->collision->geometry.get()))->radius;
+  return true;
+}
+
+namespace steer_drive_controller
+{
+
+  SteerDriveController::SteerDriveController()
+      : open_loop_(false), command_struct_(), wheel_separation_h_(0.0), wheel_radius_(0.0), wheel_separation_h_multiplier_(1.0), wheel_radius_multiplier_(1.0), steer_pos_multiplier_(1.0), cmd_vel_timeout_(0.5), allow_multiple_cmd_vel_publishers_(true), base_frame_id_("base_link"), odom_frame_id_("odom"), enable_odom_tf_(true), wheel_joints_size_(0), ns_("")
+  {
+  }
+
+  bool SteerDriveController::init(hardware_interface::RobotHW *robot_hw,
+                                  ros::NodeHandle &root_nh,
+                                  ros::NodeHandle &controller_nh)
+  {
+    typedef hardware_interface::VelocityJointInterface VelIface;
+    typedef hardware_interface::PositionJointInterface PosIface;
+    typedef hardware_interface::JointStateInterface StateIface;
+
+    // get multiple types of hardware_interface
+    VelIface *vel_joint_if = robot_hw->get<VelIface>(); // vel for wheels
+    PosIface *pos_joint_if = robot_hw->get<PosIface>(); // pos for steers
+
+    const std::string complete_ns = controller_nh.getNamespace();
+
+    std::size_t id = complete_ns.find_last_of("/");
+    name_ = complete_ns.substr(id + 1);
+
+    //-- single rear wheel joint
+    std::string rear_wheel_name = "rear_wheel_joint";
+    controller_nh.param(ns_ + "rear_wheel", rear_wheel_name, rear_wheel_name);
+
+    //-- single front steer joint
+    std::string front_steer_name = "front_steer_joint";
+    controller_nh.param(ns_ + "front_steer", front_steer_name, front_steer_name);
+
+    // Odometry related:
+    double publish_rate;
+    controller_nh.param("publish_rate", publish_rate, 50.0);
+    ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
+                                     << publish_rate << "Hz.");
+    publish_period_ = ros::Duration(1.0 / publish_rate);
+
+    controller_nh.param(ns_ + "open_loop", open_loop_, open_loop_);
+
+    controller_nh.param(ns_ + "wheel_separation_h_multiplier", wheel_separation_h_multiplier_, wheel_separation_h_multiplier_);
+    ROS_INFO_STREAM_NAMED(name_, "Wheel separation height will be multiplied by "
+                                     << wheel_separation_h_multiplier_ << ".");
+
+    controller_nh.param(ns_ + "wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_);
+    ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
+                                     << wheel_radius_multiplier_ << ".");
+
+    controller_nh.param(ns_ + "steer_pos_multiplier", steer_pos_multiplier_, steer_pos_multiplier_);
+    ROS_INFO_STREAM_NAMED(name_, "Steer pos will be multiplied by "
+                                     << steer_pos_multiplier_ << ".");
+
+    int velocity_rolling_window_size = 10;
+    controller_nh.param(ns_ + "velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
+    ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
+                                     << velocity_rolling_window_size << ".");
+
+    odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
+
+    // Twist command related:
+    controller_nh.param(ns_ + "cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
+    ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
+                                     << cmd_vel_timeout_ << "s.");
+
+    controller_nh.param(ns_ + "allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
+    ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is "
+                                     << (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled"));
+
+    controller_nh.param(ns_ + "base_frame_id", base_frame_id_, base_frame_id_);
+    ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
+
+    controller_nh.param(ns_ + "odom_frame_id", odom_frame_id_, odom_frame_id_);
+    ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
+
+    controller_nh.param(ns_ + "enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
+    ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_ ? "enabled" : "disabled"));
+
+    // Velocity and acceleration limits:
+    controller_nh.param(ns_ + "linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
+    controller_nh.param(ns_ + "linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
+    controller_nh.param(ns_ + "linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits);
+    controller_nh.param(ns_ + "linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
+    controller_nh.param(ns_ + "linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
+    controller_nh.param(ns_ + "linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
+    controller_nh.param(ns_ + "linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);
+    controller_nh.param(ns_ + "linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk);
+    controller_nh.param(ns_ + "linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk);
+
+    controller_nh.param(ns_ + "angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
+    controller_nh.param(ns_ + "angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
+    controller_nh.param(ns_ + "angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits);
+    controller_nh.param(ns_ + "angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
+    controller_nh.param(ns_ + "angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
+    controller_nh.param(ns_ + "angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
+    controller_nh.param(ns_ + "angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);
+    controller_nh.param(ns_ + "angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk);
+    controller_nh.param(ns_ + "angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk);
+
+    // If either parameter is not available, we need to look up the value in the URDF
+    bool lookup_wheel_separation_h = !controller_nh.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
+    bool lookup_wheel_radius = !controller_nh.getParam(ns_ + "wheel_radius", wheel_radius_);
+
+    if (!setOdomParamsFromUrdf(root_nh,
+                               rear_wheel_name,
+                               front_steer_name,
+                               lookup_wheel_separation_h,
+                               lookup_wheel_radius))
+    {
+      return false;
+    }
+
+    // Regardless of how we got the separation and radius, use them
+    // to set the odometry parameters
+    const double ws_h = wheel_separation_h_multiplier_ * wheel_separation_h_;
+    const double wr = wheel_radius_multiplier_ * wheel_radius_;
+    odometry_.setWheelParams(ws_h, wr);
+    ROS_INFO_STREAM_NAMED(name_,
+                          "Odometry params : wheel separation height " << ws_h
+                                                                       << ", wheel radius " << wr);
+
+    setOdomPubFields(root_nh, controller_nh);
+
+    //-- rear wheel
+    //---- handles need to be previously registerd in steer_drive_test.cpp
+    ROS_INFO_STREAM_NAMED(name_,
+                          "Adding the rear wheel with joint name: " << rear_wheel_name);
+    rear_wheel_joint_ = vel_joint_if->getHandle(rear_wheel_name); // throws on failure
+    //-- front steer
+    ROS_INFO_STREAM_NAMED(name_,
+                          "Adding the front steer with joint name: " << front_steer_name);
+    front_steer_joint_ = pos_joint_if->getHandle(front_steer_name); // throws on failure
+    ROS_INFO_STREAM_NAMED(name_,
+                          "Adding the subscriber: cmd_vel");
+    sub_command_ = controller_nh.subscribe("cmd_vel", 1, &SteerDriveController::cmdVelCallback, this);
+    ROS_INFO_STREAM_NAMED(name_, "Finished controller initialization");
+
+    return true;
+  }
+
+  void SteerDriveController::update(const ros::Time &time, const ros::Duration &period)
+  {
+    // COMPUTE AND PUBLISH ODOMETRY
+    if (open_loop_)
+    {
+      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
+    }
+    else
+    {
+      double wheel_pos = rear_wheel_joint_.getPosition();
+      double steer_pos = front_steer_joint_.getPosition();
+
+      if (std::isnan(wheel_pos) || std::isnan(steer_pos))
+        return;
+
+      // Estimate linear and angular velocity using joint information
+      steer_pos = steer_pos * steer_pos_multiplier_;
+      odometry_.update(wheel_pos, steer_pos, time);
+    }
+
+    // Publish odometry message
+    if (last_state_publish_time_ + publish_period_ < time)
+    {
+      last_state_publish_time_ += publish_period_;
+      // Compute and store orientation info
+      const geometry_msgs::Quaternion orientation(
+          tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
+
+      // Populate odom message and publish
+      if (odom_pub_->trylock())
+      {
+        odom_pub_->msg_.header.stamp = time;
+        odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
+        odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
+        odom_pub_->msg_.pose.pose.orientation = orientation;
+        odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
+        odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
+        odom_pub_->unlockAndPublish();
+      }
+
+      // Publish tf /odom frame
+      if (enable_odom_tf_ && tf_odom_pub_->trylock())
+      {
+        geometry_msgs::TransformStamped &odom_frame = tf_odom_pub_->msg_.transforms[0];
+        odom_frame.header.stamp = time;
+        odom_frame.transform.translation.x = odometry_.getX();
+        odom_frame.transform.translation.y = odometry_.getY();
+        odom_frame.transform.rotation = orientation;
+        tf_odom_pub_->unlockAndPublish();
+      }
+    }
+
+    // MOVE ROBOT
+    // Retreive current velocity command and time step:
+    Commands curr_cmd = *(command_.readFromRT());
+    const double dt = (time - curr_cmd.stamp).toSec();
+
+    // Brake if cmd_vel has timeout:
+    if (dt > cmd_vel_timeout_)
+    {
+      curr_cmd.lin = 0.0;
+      curr_cmd.ang = 0.0;
+    }
+
+    // Limit velocities and accelerations:
+    const double cmd_dt(period.toSec());
+    // double old_lin = curr_cmd.lin;
+    limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
+    // double old_ang = curr_cmd.ang;
+    limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
+
+    last1_cmd_ = last0_cmd_;
+    last0_cmd_ = curr_cmd;
+
+    // Set Command
+    const double wheel_vel = curr_cmd.lin / wheel_radius_; // omega = linear_vel / radius
+    rear_wheel_joint_.setCommand(wheel_vel);
+    front_steer_joint_.setCommand(curr_cmd.ang);
+    // ROS_INFO("curr_cmd.lin = %f last0_cmd_.lin = %f last1_cmd_.lin = %f cmd_dt = %f", curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, old_lin);
+    // ROS_WARN("curr_cmd.ang = %f last0_cmd_.ang = %f last1_cmd_.ang = %f cmd_dt = %f", curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang,old_ang);
+  }
+
+  void SteerDriveController::starting(const ros::Time &time)
+  {
+    brake();
+
+    // Register starting time used to keep fixed rate
+    last_state_publish_time_ = time;
+
+    odometry_.init(time);
+  }
+
+  void SteerDriveController::stopping(const ros::Time & /*time*/)
+  {
+    brake();
+  }
+
+  void SteerDriveController::brake()
+  {
+    const double steer_pos = 0.0;
+    const double wheel_vel = 0.0;
+
+    rear_wheel_joint_.setCommand(steer_pos);
+    front_steer_joint_.setCommand(wheel_vel);
+  }
+
+  void SteerDriveController::cmdVelCallback(const geometry_msgs::Twist &command)
+  {
+    if (isRunning())
+    {
+      // check that we don't have multiple publishers on the command topic
+      if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
+      {
+        ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers() << " publishers. Only 1 publisher is allowed. Going to brake.");
+        brake();
+        return;
+      }
+
+      command_struct_.ang = command.angular.z;
+      command_struct_.lin = command.linear.x;
+      command_struct_.stamp = ros::Time::now();
+      command_.writeFromNonRT(command_struct_);
+      ROS_DEBUG_STREAM_NAMED(name_,
+                             "Added values to command. "
+                                 << "Ang: " << command_struct_.ang << ", "
+                                 << "Lin: " << command_struct_.lin << ", "
+                                 << "Stamp: " << command_struct_.stamp);
+    }
+    else
+    {
+      ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
+    }
+  }
+
+  bool SteerDriveController::setOdomParamsFromUrdf(ros::NodeHandle &root_nh,
+                                                   const std::string rear_wheel_name,
+                                                   const std::string front_steer_name,
+                                                   bool lookup_wheel_separation_h,
+                                                   bool lookup_wheel_radius)
+  {
+    if (!(lookup_wheel_separation_h || lookup_wheel_radius))
+    {
+      // Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
+      return true;
+    }
+
+    // Parse robot description
+    const std::string model_param_name = "robot_description";
+    bool res = root_nh.hasParam(model_param_name);
+    std::string robot_model_str = "";
+    if (!res || !root_nh.getParam(model_param_name, robot_model_str))
+    {
+      ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
+      return false;
+    }
+
+    urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(robot_model_str);
+
+    urdf::JointConstSharedPtr rear_wheel_joint = model->getJoint(rear_wheel_name);
+    urdf::JointConstSharedPtr front_steer_joint = model->getJoint(front_steer_name);
+
+    if (lookup_wheel_separation_h)
+    {
+      // Get wheel separation
+      if (!rear_wheel_joint)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, rear_wheel_name
+                                          << " couldn't be retrieved from model description");
+        return false;
+      }
+
+      if (!front_steer_joint)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, front_steer_name
+                                          << " couldn't be retrieved from model description");
+        return false;
+      }
+
+      ROS_INFO_STREAM("rear wheel to origin: "
+                      << rear_wheel_joint->parent_to_joint_origin_transform.position.x << ","
+                      << rear_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
+                      << rear_wheel_joint->parent_to_joint_origin_transform.position.z);
+
+      ROS_INFO_STREAM("front steer to origin: "
+                      << front_steer_joint->parent_to_joint_origin_transform.position.x << ","
+                      << front_steer_joint->parent_to_joint_origin_transform.position.y << ", "
+                      << front_steer_joint->parent_to_joint_origin_transform.position.z);
+
+      wheel_separation_h_ = fabs(
+          rear_wheel_joint->parent_to_joint_origin_transform.position.x - front_steer_joint->parent_to_joint_origin_transform.position.x);
+
+      ROS_INFO_STREAM("Calculated wheel_separation_h: " << wheel_separation_h_);
+    }
+
+    if (lookup_wheel_radius)
+    {
+      // Get wheel radius
+      if (!getWheelRadius(model->getLink(rear_wheel_joint->child_link_name), wheel_radius_))
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << rear_wheel_name << " wheel radius");
+        return false;
+      }
+      ROS_INFO_STREAM("Retrieved wheel_radius: " << wheel_radius_);
+    }
+
+    return true;
+  }
+
+  void SteerDriveController::setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
+  {
+    // Get and check params for covariances
+    XmlRpc::XmlRpcValue pose_cov_list;
+    controller_nh.getParam(ns_ + "pose_covariance_diagonal", pose_cov_list);
+    ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+    ROS_ASSERT(pose_cov_list.size() == 6);
+    for (int i = 0; i < pose_cov_list.size(); ++i)
+      ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+
+    XmlRpc::XmlRpcValue twist_cov_list;
+    controller_nh.getParam(ns_ + "twist_covariance_diagonal", twist_cov_list);
+    ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+    ROS_ASSERT(twist_cov_list.size() == 6);
+    for (int i = 0; i < twist_cov_list.size(); ++i)
+      ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+
+    // Setup odometry realtime publisher + odom message constant fields
+    odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, ns_ + "odom", 100));
+    odom_pub_->msg_.header.frame_id = odom_frame_id_;
+    odom_pub_->msg_.child_frame_id = base_frame_id_;
+    odom_pub_->msg_.pose.pose.position.z = 0;
+    odom_pub_->msg_.pose.covariance = boost::assign::list_of(static_cast<double>(pose_cov_list[0]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[1]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[2]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[3]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[4]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[5]));
+    odom_pub_->msg_.twist.twist.linear.y = 0;
+    odom_pub_->msg_.twist.twist.linear.z = 0;
+    odom_pub_->msg_.twist.twist.angular.x = 0;
+    odom_pub_->msg_.twist.twist.angular.y = 0;
+    odom_pub_->msg_.twist.covariance = boost::assign::list_of(static_cast<double>(twist_cov_list[0]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[1]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[2]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[3]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[4]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[5]));
+    tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
+    tf_odom_pub_->msg_.transforms.resize(1);
+    tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
+    tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
+    tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
+  }
+
+} // namespace steer_drive_controller

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/steer_drive_controller_plugins.xml

@@ -0,0 +1,9 @@
+<library path="lib/libsteer_drive_controller">
+
+  <class name="steer_drive_controller/SteerDriveController" type="steer_drive_controller::SteerDriveController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The SteerDriveController tracks velocity commands. It expects 1 VelocityJointInterface and 1 PositionJointInterfece types of hardware interfaces.
+    </description>
+  </class>
+
+</library>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/config/steerbot_controllers.yaml

@@ -0,0 +1,13 @@
+steerbot_controller:
+  type: "steer_drive_controller/SteerDriveController"
+  rear_wheel  : 'rear_wheel_joint'
+  front_steer : 'front_steer_joint'
+  publish_rate: 50.0 # defaults to 50
+
+  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
+
+  #wheel_separation_h : 0.4
+  #wheel_radius : 0.11
+  wheel_separation_h_multiplier: 0.257 # calibration parameter for odometory, needed for test.

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/config/steerbot_hw_sim.yaml

@@ -0,0 +1,9 @@
+steerbot_hw_sim:
+  rear_wheel  : 'rear_wheel_joint'
+  front_steer : 'front_steer_joint'
+
+  rear_wheels: ['rear_right_wheel_joint', 'rear_left_wheel_joint']
+  front_wheels: ['front_right_wheel_joint', 'front_left_wheel_joint']
+  front_steers: ['front_right_steer_joint', 'front_left_steer_joint']
+
+  wheel_separation_h : 0.20

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/include/steerbot.h

@@ -0,0 +1,434 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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.
+//////////////////////////////////////////////////////////////////////////////
+
+// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
+
+// ROS
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_srvs/Empty.h>
+
+// ros_control
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// NaN
+#include <limits>
+
+// ostringstream
+#include <sstream>
+
+enum INDEX_WHEEL {
+    INDEX_RIGHT = 0,
+    INDEX_LEFT = 1
+};
+
+class Steerbot : public hardware_interface::RobotHW
+{
+public:
+  Steerbot()
+  : running_(true)
+  , start_srv_(nh_.advertiseService("start", &Steerbot::startCallback, this))
+  , stop_srv_(nh_.advertiseService("stop", &Steerbot::stopCallback, this))
+  , ns_("steerbot_hw_sim/")
+  {
+    // Intialize raw data
+    this->cleanUp();
+    this->getJointNames(nh_);
+    this->registerHardwareInterfaces();
+
+    nh_.getParam(ns_ + "wheel_separation_w", wheel_separation_w_);
+    nh_.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
+    ROS_INFO_STREAM("wheel_separation_w in test steerbot= " << wheel_separation_w_);
+    ROS_INFO_STREAM("wheel_separation_h in test steerbot= " << wheel_separation_h_);
+  }
+
+  ros::Time getTime() const {return ros::Time::now();}
+  ros::Duration getPeriod() const {return ros::Duration(0.01);}
+
+  void read()
+  {
+    std::ostringstream os;
+    // directly get from controller
+    os << rear_wheel_jnt_vel_cmd_ << ", ";
+    os << front_steer_jnt_pos_cmd_ << ", ";
+
+    // convert to each joint velocity
+    //-- differential drive
+    for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
+    {
+      virtual_rear_wheel_jnt_vel_cmd_[i] = rear_wheel_jnt_vel_cmd_;
+      os << virtual_rear_wheel_jnt_vel_cmd_[i] << ", ";
+    }
+
+    //-- ackerman link
+    const double h = wheel_separation_h_;
+    const double w = wheel_separation_w_;
+    virtual_front_steer_jnt_pos_cmd_[INDEX_RIGHT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h + w/2.0*tan(front_steer_jnt_pos_cmd_));
+    virtual_front_steer_jnt_pos_cmd_[INDEX_LEFT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h - w/2.0*tan(front_steer_jnt_pos_cmd_));
+
+    for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
+    {
+      os << virtual_front_steer_jnt_pos_cmd_[i] << ", ";
+    }
+
+    if (rear_wheel_jnt_vel_cmd_ != 0.0 || front_steer_jnt_pos_cmd_ != 0.0)
+      ROS_INFO_STREAM("Commands for joints: " << os.str());
+
+  }
+
+  void write()
+  {
+    std::ostringstream os;
+    if (running_)
+    {
+      // wheels
+      rear_wheel_jnt_pos_ += rear_wheel_jnt_vel_*getPeriod().toSec();
+      rear_wheel_jnt_vel_ = rear_wheel_jnt_vel_cmd_;
+      for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
+      {
+        // Note that pos_[i] will be NaN for one more cycle after we start(),
+        // but that is consistent with the knowledge we have about the state
+        // of the robot.
+        virtual_rear_wheel_jnt_pos_[i] += virtual_rear_wheel_jnt_vel_[i]*getPeriod().toSec();
+        virtual_rear_wheel_jnt_vel_[i] = virtual_rear_wheel_jnt_vel_cmd_[i];
+      }
+
+      // steers
+      front_steer_jnt_pos_ = front_steer_jnt_pos_cmd_;
+      for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
+      {
+        virtual_front_steer_jnt_pos_[i] = virtual_front_steer_jnt_pos_cmd_[i];
+      }
+
+      // directly get from controller
+      os << rear_wheel_jnt_vel_cmd_ << ", ";
+      os << front_steer_jnt_pos_cmd_ << ", ";
+
+      // convert to each joint velocity
+      //-- differential drive
+      for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
+      {
+          os << virtual_rear_wheel_jnt_pos_[i] << ", ";
+      }
+
+      //-- ackerman link
+      for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
+      {
+          os << virtual_front_steer_jnt_pos_[i] << ", ";
+      }
+    }
+    else
+    {
+      // wheels
+      rear_wheel_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
+      rear_wheel_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
+      std::fill_n(virtual_rear_wheel_jnt_pos_.begin(), virtual_rear_wheel_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
+      std::fill_n(virtual_rear_wheel_jnt_vel_.begin(), virtual_rear_wheel_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
+      // steers
+      front_steer_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
+      front_steer_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
+      std::fill_n(virtual_front_steer_jnt_pos_.begin(), virtual_front_steer_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
+      std::fill_n(virtual_front_steer_jnt_vel_.begin(), virtual_front_steer_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
+
+      // wheels
+      os << rear_wheel_jnt_pos_ << ", ";
+      os << rear_wheel_jnt_vel_ << ", ";
+      for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
+      {
+          os << virtual_rear_wheel_jnt_pos_[i] << ", ";
+      }
+
+      // steers
+      os << front_steer_jnt_pos_ << ", ";
+      os << front_steer_jnt_vel_ << ", ";
+      for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
+      {
+          os << virtual_front_steer_jnt_pos_[i] << ", ";
+      }
+    }
+    ROS_INFO_STREAM("running_ = " << running_ << ". commands are " << os.str());
+  }
+
+  bool startCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
+  {
+    ROS_INFO_STREAM("running_ = " << running_ << ".");
+    running_ = true;
+    return true;
+  }
+
+  bool stopCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
+  {
+    ROS_INFO_STREAM("running_ = " << running_ << ".");
+    running_ = false;
+    return true;
+  }
+
+private:
+  
+  void cleanUp()
+  {
+    // wheel
+    //-- wheel joint names
+    rear_wheel_jnt_name_.empty();
+    virtual_rear_wheel_jnt_names_.clear();
+    //-- actual rear wheel joint
+    rear_wheel_jnt_pos_ = 0;
+    rear_wheel_jnt_vel_ = 0;
+    rear_wheel_jnt_eff_ = 0;
+    rear_wheel_jnt_vel_cmd_ = 0;
+    //-- virtual rear wheel joint
+    virtual_rear_wheel_jnt_pos_.clear();
+    virtual_rear_wheel_jnt_vel_.clear();
+    virtual_rear_wheel_jnt_eff_.clear();
+    virtual_rear_wheel_jnt_vel_cmd_.clear();
+    //-- virtual front wheel joint
+    virtual_front_wheel_jnt_pos_.clear();
+    virtual_front_wheel_jnt_vel_.clear();
+    virtual_front_wheel_jnt_eff_.clear();
+    virtual_front_wheel_jnt_vel_cmd_.clear();
+
+    // steer
+    //-- steer joint names
+    front_steer_jnt_name_.empty();
+    virtual_front_steer_jnt_names_.clear();
+    //-- front steer joint
+    front_steer_jnt_pos_ = 0;
+    front_steer_jnt_vel_ = 0;
+    front_steer_jnt_eff_ = 0;
+    front_steer_jnt_pos_cmd_ = 0;
+    //-- virtual wheel joint
+    virtual_front_steer_jnt_pos_.clear();
+    virtual_front_steer_jnt_vel_.clear();
+    virtual_front_steer_jnt_eff_.clear();
+    virtual_front_steer_jnt_pos_cmd_.clear();
+  }
+
+  void getJointNames(ros::NodeHandle &_nh)
+  {
+    this->getWheelJointNames(_nh);
+    this->getSteerJointNames(_nh);
+  }
+
+  void getWheelJointNames(ros::NodeHandle &_nh)
+  {
+    // wheel joint to get linear command
+    _nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_);
+
+    // virtual wheel joint for gazebo con_rol
+    _nh.getParam(ns_ + "rear_wheels", virtual_rear_wheel_jnt_names_);
+    int dof = virtual_rear_wheel_jnt_names_.size();
+    virtual_rear_wheel_jnt_pos_.resize(dof);
+    virtual_rear_wheel_jnt_vel_.resize(dof);
+    virtual_rear_wheel_jnt_eff_.resize(dof);
+    virtual_rear_wheel_jnt_vel_cmd_.resize(dof);
+
+    _nh.getParam(ns_ + "front_wheels", virtual_front_wheel_jnt_names_);
+    dof = virtual_front_wheel_jnt_names_.size();
+    virtual_front_wheel_jnt_pos_.resize(dof);
+    virtual_front_wheel_jnt_vel_.resize(dof);
+    virtual_front_wheel_jnt_eff_.resize(dof);
+    virtual_front_wheel_jnt_vel_cmd_.resize(dof);
+  }
+
+  void getSteerJointNames(ros::NodeHandle &_nh)
+  {
+    // steer joint to get angular command
+    _nh.getParam(ns_ + "front_steer", front_steer_jnt_name_);
+
+    // virtual steer joint for gazebo control
+    _nh.getParam(ns_ + "front_steers", virtual_front_steer_jnt_names_);
+
+    const int dof = virtual_front_steer_jnt_names_.size();
+    virtual_front_steer_jnt_pos_.resize(dof);
+    virtual_front_steer_jnt_vel_.resize(dof);
+    virtual_front_steer_jnt_eff_.resize(dof);
+    virtual_front_steer_jnt_pos_cmd_.resize(dof);
+  }
+
+  void registerHardwareInterfaces()
+  {
+    this->registerSteerInterface();
+    this->registerWheelInterface();
+
+    // register mapped interface to ros_control
+    registerInterface(&jnt_state_interface_);
+    registerInterface(&rear_wheel_jnt_vel_cmd_interface_);
+    registerInterface(&front_steer_jnt_pos_cmd_interface_);
+  }
+
+  void registerWheelInterface()
+  {
+    // actual wheel joints
+    this->registerInterfaceHandles(
+          jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
+          rear_wheel_jnt_name_, rear_wheel_jnt_pos_, rear_wheel_jnt_vel_, rear_wheel_jnt_eff_, rear_wheel_jnt_vel_cmd_);
+
+    // virtual rear wheel joints
+    for (int i = 0; i < virtual_rear_wheel_jnt_names_.size(); i++)
+    {
+      this->registerInterfaceHandles(
+            jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
+            virtual_rear_wheel_jnt_names_[i], virtual_rear_wheel_jnt_pos_[i], virtual_rear_wheel_jnt_vel_[i], virtual_rear_wheel_jnt_eff_[i], virtual_rear_wheel_jnt_vel_cmd_[i]);
+    }
+    // virtual front wheel joints
+    for (int i = 0; i < virtual_front_wheel_jnt_names_.size(); i++)
+    {
+      this->registerInterfaceHandles(
+            jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
+            virtual_front_wheel_jnt_names_[i], virtual_front_wheel_jnt_pos_[i], virtual_front_wheel_jnt_vel_[i], virtual_front_wheel_jnt_eff_[i], virtual_front_wheel_jnt_vel_cmd_[i]);
+    }
+  }
+
+  void registerSteerInterface()
+  {
+    // actual steer joints
+    this->registerInterfaceHandles(
+          jnt_state_interface_, front_steer_jnt_pos_cmd_interface_,
+          front_steer_jnt_name_, front_steer_jnt_pos_, front_steer_jnt_vel_, front_steer_jnt_eff_, front_steer_jnt_pos_cmd_);
+
+    // virtual steer joints
+    for (int i = 0; i < virtual_front_steer_jnt_names_.size(); i++)
+    {
+      this->registerInterfaceHandles(
+            jnt_state_interface_, front_steer_jnt_pos_cmd_interface_,
+            virtual_front_steer_jnt_names_[i], virtual_front_steer_jnt_pos_[i], virtual_front_steer_jnt_vel_[i], virtual_front_steer_jnt_eff_[i], virtual_front_steer_jnt_pos_cmd_[i]);
+    }
+  }
+
+  void registerInterfaceHandles(
+          hardware_interface::JointStateInterface& _jnt_state_interface,
+          hardware_interface::JointCommandInterface& _jnt_cmd_interface,
+          const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff,  double& _jnt_cmd)
+  {
+    // register joint (both JointState and CommandJoint)
+    this->registerJointStateInterfaceHandle(_jnt_state_interface, _jnt_name,
+                                            _jnt_pos, _jnt_vel, _jnt_eff);
+    this->registerCommandJointInterfaceHandle(_jnt_state_interface, _jnt_cmd_interface,
+                                              _jnt_name, _jnt_cmd);
+  }
+
+  void registerJointStateInterfaceHandle(
+      hardware_interface::JointStateInterface& _jnt_state_interface,
+      const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff)
+  {
+    hardware_interface::JointStateHandle state_handle(_jnt_name,
+                                                      &_jnt_pos,
+                                                      &_jnt_vel,
+                                                      &_jnt_eff);
+    _jnt_state_interface.registerHandle(state_handle);
+
+    ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the JointStateInterface");
+  }
+
+  void registerCommandJointInterfaceHandle(
+      hardware_interface::JointStateInterface& _jnt_state_interface,
+      hardware_interface::JointCommandInterface& _jnt_cmd_interface,
+      const std::string _jnt_name, double& _jnt_cmd)
+  {
+    // joint command
+    hardware_interface::JointHandle _handle(_jnt_state_interface.getHandle(_jnt_name),
+                                            &_jnt_cmd);
+    _jnt_cmd_interface.registerHandle(_handle);
+
+    ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the CommandJointInterface");
+  }
+
+private:
+  // common
+  hardware_interface::JointStateInterface jnt_state_interface_;// rear wheel
+  //-- actual joint(single actuator)
+  //---- joint name
+  std::string rear_wheel_jnt_name_;
+  //---- joint interface parameters
+  double rear_wheel_jnt_pos_;
+  double rear_wheel_jnt_vel_;
+  double rear_wheel_jnt_eff_;
+  //---- joint interface command
+  double rear_wheel_jnt_vel_cmd_;
+  //---- Hardware interface: joint
+  hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_;
+  //hardware_interface::JointStateInterface wheel_jnt_state_interface_;
+  //
+  //-- virtual joints(two rear wheels)
+  //---- joint name
+  std::vector<std::string> virtual_rear_wheel_jnt_names_;
+  //---- joint interface parameters
+  std::vector<double> virtual_rear_wheel_jnt_pos_;
+  std::vector<double> virtual_rear_wheel_jnt_vel_;
+  std::vector<double> virtual_rear_wheel_jnt_eff_;
+  //---- joint interface command
+  std::vector<double> virtual_rear_wheel_jnt_vel_cmd_;
+  //-- virtual joints(two front wheels)
+  //---- joint name
+  std::vector<std::string> virtual_front_wheel_jnt_names_;
+  //---- joint interface parameters
+  std::vector<double> virtual_front_wheel_jnt_pos_;
+  std::vector<double> virtual_front_wheel_jnt_vel_;
+  std::vector<double> virtual_front_wheel_jnt_eff_;
+  //---- joint interface command
+  std::vector<double> virtual_front_wheel_jnt_vel_cmd_;
+
+  // front steer
+  //-- actual joint(single actuator)
+  //---- joint name
+  std::string front_steer_jnt_name_;
+  //---- joint interface parameters
+  double front_steer_jnt_pos_;
+  double front_steer_jnt_vel_;
+  double front_steer_jnt_eff_;
+  //---- joint interface command
+  double front_steer_jnt_pos_cmd_;
+  //---- Hardware interface: joint
+  hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_;
+  //hardware_interface::JointStateInterface steer_jnt_state_interface_;
+  //
+  //-- virtual joints(two steers)
+  //---- joint name
+  std::vector<std::string> virtual_front_steer_jnt_names_;
+  //---- joint interface parameters
+  std::vector<double> virtual_front_steer_jnt_pos_;
+  std::vector<double> virtual_front_steer_jnt_vel_;
+  std::vector<double> virtual_front_steer_jnt_eff_;
+  //---- joint interface command
+  std::vector<double> virtual_front_steer_jnt_pos_cmd_;
+
+  // Wheel separation, wrt the midpoint of the wheel width:
+  double wheel_separation_w_;
+  // Wheel separation, wrt the midpoint of the wheel width:
+  double wheel_separation_h_;
+
+  std::string ns_;
+  bool running_;
+
+  ros::NodeHandle nh_;
+  ros::ServiceServer start_srv_;
+  ros::ServiceServer stop_srv_;
+};

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/include/test_common.h

@@ -0,0 +1,97 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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 Bence Magyar
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/tf.h>
+
+#include <std_srvs/Empty.h>
+
+// Floating-point value comparison threshold
+const double EPS = 0.01;
+const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision
+const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
+const double ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
+const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
+const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
+const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision
+
+class SteerDriveControllerTest : public ::testing::Test
+{
+public:
+
+  SteerDriveControllerTest()
+  : cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
+  , odom_sub(nh.subscribe("odom", 100, &SteerDriveControllerTest::odomCallback, this))
+  , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
+  , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
+  {
+  }
+
+  ~SteerDriveControllerTest()
+  {
+    odom_sub.shutdown();
+  }
+
+  nav_msgs::Odometry getLastOdom(){ return last_odom; }
+  void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
+  bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); }
+
+  void start(){ std_srvs::Empty srv; start_srv.call(srv); }
+  void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
+
+private:
+  ros::NodeHandle nh;
+  ros::Publisher cmd_pub;
+  ros::Subscriber odom_sub;
+  nav_msgs::Odometry last_odom;
+
+  ros::ServiceClient start_srv;
+  ros::ServiceClient stop_srv;
+
+  void odomCallback(const nav_msgs::Odometry& odom)
+  {
+    ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x
+                     << ", orient.z: " << odom.pose.pose.orientation.z
+                     << ", lin_est: " << odom.twist.twist.linear.x
+                     << ", ang_est: " << odom.twist.twist.angular.z);
+    last_odom = odom;
+  }
+};
+
+inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
+{
+  return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
+}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/launch/steer_drive_common.launch

@@ -0,0 +1,37 @@
+<launch>
+  <!-- Load steerbot model -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot.xacro' --inorder" />
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_controllers.yaml" />
+
+  <!-- Load steerbot config -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_hw_sim.yaml" />
+
+  <!-- Start steerbot -->
+  <node name="steer_drive_controller_steerbot"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_steerbot"/>
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="steerbot_controller" />
+
+  <!-- rqt_plot monitoring -->
+  <!--
+  <node name="steerbot_pos_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/steerbot_controller/odom/pose/pose/position/x" />
+  <node name="steerbot_vel_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/steerbot_controller/odom/twist/twist/linear/x" />
+  <node name="steerbot_ang_vel_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/steerbot_controller/odom/twist/twist/angular/z" />
+  -->
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/launch/view_steerbot.launch

@@ -0,0 +1,32 @@
+<launch>
+  <!-- start world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
+
+  <!-- load robot -->
+  <param name="robot_description"
+      command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot.xacro' --inorder" />
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_controllers.yaml" />
+
+  <!-- Load steerbot config -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_hw_sim.yaml" />
+
+  <!-- Start steerbot -->
+  <node name="steer_drive_controller_steerbot"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_steerbot"/>
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="steerbot_controller" />
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+	args="-urdf -model steerbot -param robot_description -z 0.5"/>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+  <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find steer_drive_controller)/test/common/rviz/display.rviz"/>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/rviz/display.rviz

@@ -0,0 +1,256 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_bottom_lrf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_top_3durg:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_top_3durg_rgbd_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_front_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_rear_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        merged_lrf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rear_bottom_lrf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_front_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_rear_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        steer:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        steer_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        steer_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        camera_link:
+          Value: true
+        front_bottom_lrf:
+          Value: true
+        front_top_3durg:
+          Value: true
+        front_top_3durg_rgbd_optical_frame:
+          Value: true
+        left_front_wheel:
+          Value: true
+        left_rear_wheel:
+          Value: true
+        merged_lrf:
+          Value: true
+        rear_bottom_lrf:
+          Value: true
+        right_front_wheel:
+          Value: true
+        right_rear_wheel:
+          Value: true
+        steer:
+          Value: true
+        steer_left:
+          Value: true
+        steer_right:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          camera_link:
+            {}
+          front_bottom_lrf:
+            {}
+          front_top_3durg:
+            front_top_3durg_rgbd_optical_frame:
+              {}
+          left_rear_wheel:
+            {}
+          merged_lrf:
+            {}
+          rear_bottom_lrf:
+            {}
+          right_rear_wheel:
+            {}
+          steer:
+            {}
+          steer_left:
+            left_front_wheel:
+              {}
+          steer_right:
+            right_front_wheel:
+              {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.57916
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.129039
+        Y: -0.142336
+        Z: 0.339095
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.420398
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 0.805398
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007510000003efc0100000002fb0000000800540069006d0065010000000000000751000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1873
+  X: 47
+  Y: 24

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/src/steerbot.cpp

@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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.
+//////////////////////////////////////////////////////////////////////////////
+
+// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
+
+// ROS
+#include <ros/ros.h>
+
+// ros_control
+#include <controller_manager/controller_manager.h>
+
+#include "./../include/steerbot.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "steerbot");
+  ros::NodeHandle nh;
+
+  Steerbot robot;
+  ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
+  controller_manager::ControllerManager cm(&robot, nh);
+
+  ros::Rate rate(1.0 / robot.getPeriod().toSec());
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  while(ros::ok())
+  {
+    ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
+    robot.read();
+    cm.update(robot.getTime(), robot.getPeriod());
+    robot.write();
+    rate.sleep();
+  }
+  spinner.stop();
+
+  return 0;
+}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/urdf/steerbot.xacro

@@ -0,0 +1,95 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<!--
+Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+-->
+  <xacro:include filename="$(find steer_drive_controller)/test/common/urdf/wheel.xacro"/>
+
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="length" value="0.5" /> 
+  <xacro:property name="width" value="0.3" /> 
+  <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+  <xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
+  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+  <xacro:property name="steer_offset" value="0.02" /> <!-- Link 1 -->
+
+  <!-- Links: inertial,visual,collision -->
+  <link name="base_link">
+    <inertial>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="5"/>
+      <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="${length} ${width} 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${length} ${width} 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <link name="base_footprint">
+    <visual>
+      <geometry>
+        <sphere radius="0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.00000001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+    <child link="base_link"/>
+    <parent link="base_footprint"/>
+  </joint>
+
+  <!-- joints for steer_drive_controller -->
+  <front_steer name="front" parent="base" radius="${wheel_radius}" thickness="${thickness}" 
+      length="${length}" width="${width}" axel_offset="${axel_offset}" steer_height="${wheel_radius+steer_offset}">
+    <origin xyz="${length/2-axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </front_steer>
+  <rear_wheel name="rear" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-length/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </rear_wheel>
+
+  <!-- Wheels -->
+  <front_wheel_lr name="front_right" parent="base" radius="${wheel_radius}" thickness="${thickness}" 
+      length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="1" steer_height="${wheel_radius+steer_offset}">
+    <origin xyz="${length/2-axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </front_wheel_lr>
+  <front_wheel_lr name="front_left" parent="base" radius="${wheel_radius}" thickness="${thickness}" 
+      length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="-1" steer_height="${wheel_radius+steer_offset}">
+    <origin xyz="${length/2-axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </front_wheel_lr>
+  <rear_wheel_lr name="rear_right" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-length/2+axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </rear_wheel_lr>
+  <rear_wheel_lr name="rear_left" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-length/2+axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </rear_wheel_lr>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/urdf/steerbot_sphere_wheels.xacro

@@ -0,0 +1,95 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<!--
+Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+-->
+  <xacro:include filename="$(find steer_drive_controller)/test/common/urdf/wheel_sphere.xacro"/>
+
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="length" value="0.5" /> 
+  <xacro:property name="width" value="0.3" /> 
+  <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+  <xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
+  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+  <xacro:property name="steer_offset" value="0.02" /> <!-- Link 1 -->
+
+  <!-- Links: inertial,visual,collision -->
+  <link name="base_link">
+    <inertial>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="5"/>
+      <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="${length} ${width} 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${length} ${width} 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <link name="base_footprint">
+    <visual>
+      <geometry>
+        <sphere radius="0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.00000001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+    <child link="base_link"/>
+    <parent link="base_footprint"/>
+  </joint>
+
+  <!-- joints for steer_drive_controller -->
+  <front_steer name="front" parent="base" radius="${wheel_radius}" thickness="${thickness}" 
+      length="${length}" width="${width}" axel_offset="${axel_offset}" steer_height="${wheel_radius+steer_offset}">
+    <origin xyz="${length/2-axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </front_steer>
+  <rear_wheel name="rear" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-length/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </rear_wheel>
+
+  <!-- Wheels -->
+  <front_wheel_lr name="front_right" parent="base" radius="${wheel_radius}" thickness="${thickness}" 
+      length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="1" steer_height="${wheel_radius+steer_offset}">
+    <origin xyz="${length/2-axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </front_wheel_lr>
+  <front_wheel_lr name="front_left" parent="base" radius="${wheel_radius}" thickness="${thickness}" 
+      length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="-1" steer_height="${wheel_radius+steer_offset}">
+    <origin xyz="${length/2-axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </front_wheel_lr>
+  <rear_wheel_lr name="rear_right" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-length/2+axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </rear_wheel_lr>
+  <rear_wheel_lr name="rear_left" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-length/2+axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </rear_wheel_lr>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/urdf/wheel.xacro

@@ -0,0 +1,206 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:property name="steer_radius" value="0.05" /> <!-- Link 1 -->
+  <xacro:property name="steer_thickness" value="0.02" /> <!-- Link 1 -->
+  <xacro:property name="steer_effort"   value="10.0"/>
+  <xacro:property name="steer_velocity" value="5.0"/>
+
+  <xacro:macro name="front_steer" params="name parent radius thickness length width axel_offset steer_height *origin">
+    <link name="${name}_steer_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_steer_joint" type="revolute">
+      <parent link="${parent}_link"/>
+      <child link="${name}_steer_link"/>
+      <origin xyz="${length/2-axel_offset} 0 ${steer_height}" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="${steer_effort}"
+             lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
+             velocity="${steer_velocity}"/>
+     </joint>
+
+    <gazebo reference="${name}_steer_link">
+      <material>Gazebo/Yellow</material>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="front_wheel_lr" params="name parent radius thickness length width axel_offset right_left steer_height *origin">
+    <link name="${name}_steer_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_steer_joint" type="revolute">
+      <parent link="${parent}_link"/>
+      <child link="${name}_steer_link"/>
+      <origin xyz="${length/2-axel_offset} ${right_left*(width/2+axel_offset)} ${steer_height}" rpy="0 0 0"/>
+      <!--xacro:insert_block name="origin"/-->
+      <axis xyz="0 0 1"/>
+      <limit effort="${steer_effort}"
+             lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
+             velocity="${steer_velocity}"/>
+     </joint>
+
+    <link name="${name}_wheel_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_wheel_joint" type="continuous">
+      <parent link="${name}_steer_link"/>
+      <child link="${name}_wheel_link"/>
+      <origin xyz="0 0 ${-steer_height}" rpy="${-90 * deg_to_rad} 0 0"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_steer_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_steer_joint_motor"/>
+      <joint name="${name}_steer_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+    <transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_wheel_joint_motor"/>
+      <joint name="${name}_wheel_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_steer_link">
+      <material>Gazebo/Blue</material>
+    </gazebo>
+    <gazebo reference="${name}_wheel_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="rear_wheel" params="name parent radius thickness *origin">
+    <link name="${name}_wheel_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_wheel_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_wheel_link"/>
+      <xacro:insert_block name="origin"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_wheel_joint_motor"/>
+      <joint name="${name}_wheel_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_wheel_link">
+      <material>Gazebo/Yellow</material>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="rear_wheel_lr" params="name parent radius thickness *origin">
+    <link name="${name}_wheel_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_wheel_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_wheel_link"/>
+      <xacro:insert_block name="origin"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_wheel_joint_motor"/>
+      <joint name="${name}_wheel_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_wheel_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+</robot>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/common/urdf/wheel_sphere.xacro

@@ -0,0 +1,206 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:property name="steer_radius" value="0.05" /> <!-- Link 1 -->
+  <xacro:property name="steer_thickness" value="0.02" /> <!-- Link 1 -->
+  <xacro:property name="steer_effort"   value="10.0"/>
+  <xacro:property name="steer_velocity" value="5.0"/>
+
+  <xacro:macro name="front_steer" params="name parent radius thickness length width axel_offset steer_height *origin">
+    <link name="${name}_steer_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_steer_joint" type="revolute">
+      <parent link="${parent}_link"/>
+      <child link="${name}_steer_link"/>
+      <origin xyz="${length/2-axel_offset} 0 ${steer_height}" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="${steer_effort}"
+             lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
+             velocity="${steer_velocity}"/>
+     </joint>
+
+    <gazebo reference="${name}_steer_link">
+      <material>Gazebo/Yellow</material>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="front_wheel_lr" params="name parent radius thickness length width axel_offset right_left steer_height *origin">
+    <link name="${name}_steer_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_steer_joint" type="revolute">
+      <parent link="${parent}_link"/>
+      <child link="${name}_steer_link"/>
+      <origin xyz="${length/2-axel_offset} ${right_left*(width/2+axel_offset)} ${steer_height}" rpy="0 0 0"/>
+      <!--xacro:insert_block name="origin"/-->
+      <axis xyz="0 0 1"/>
+      <limit effort="${steer_effort}"
+             lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
+             velocity="${steer_velocity}"/>
+     </joint>
+
+    <link name="${name}_wheel_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${wheel_radius}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${wheel_radius}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_wheel_joint" type="continuous">
+      <parent link="${name}_steer_link"/>
+      <child link="${name}_wheel_link"/>
+      <origin xyz="0 0 ${-steer_height}" rpy="${-90 * deg_to_rad} 0 0"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_steer_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_steer_joint_motor"/>
+      <joint name="${name}_steer_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+    <transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_wheel_joint_motor"/>
+      <joint name="${name}_wheel_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_steer_link">
+      <material>Gazebo/Blue</material>
+    </gazebo>
+    <gazebo reference="${name}_wheel_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="rear_wheel" params="name parent radius thickness *origin">
+    <link name="${name}_wheel_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${wheel_radius}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${wheel_radius}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_wheel_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_wheel_link"/>
+      <xacro:insert_block name="origin"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_wheel_joint_motor"/>
+      <joint name="${name}_wheel_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_wheel_link">
+      <material>Gazebo/Yellow</material>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="rear_wheel_lr" params="name parent radius thickness *origin">
+    <link name="${name}_wheel_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${wheel_radius}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${wheel_radius}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_wheel_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_wheel_link"/>
+      <xacro:insert_block name="origin"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_wheel_joint_motor"/>
+      <joint name="${name}_wheel_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_wheel_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+</robot>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame.test

@@ -0,0 +1,18 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+  <!-- Do not specific an odom_frame_id -->
+  <!-- <rosparam>
+    steerbot_controller:
+      odom_frame_id: odom
+  </rosparam> -->
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_default_odom_frame_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_default_odom_frame_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame_test.cpp

@@ -0,0 +1,73 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2015, Locus Robotics Corp.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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 Eric Tappan
+
+#include "../common/include/test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(SteerDriveControllerTest, testOdomFrame)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // set up tf listener
+  tf::TransformListener listener;
+  ros::Duration(2.0).sleep();
+  // check the original odom frame doesn't exist
+  EXPECT_TRUE(listener.frameExists("odom"));
+}
+
+TEST_F(SteerDriveControllerTest, testOdomTopic)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+
+  // get an odom message
+  nav_msgs::Odometry odom_msg = getLastOdom();
+  // check its frame_id
+  ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom");
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "steer_drive_controller_default_odom_frame_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_fail_test/steer_drive_controller_fail_test.cpp

@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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 Paul Mathieu
+
+#include "../common/include/test_common.h"
+
+// TEST CASES
+TEST_F(SteerDriveControllerTest, testWrongJointName)
+{
+  // the controller should never be alive
+  int secs = 0;
+  while(!isControllerAlive() && secs < 5)
+  {
+    ros::Duration(1.0).sleep();
+    secs++;
+  }
+  // give up and assume controller load failure after 5 seconds
+  EXPECT_GE(secs, 5);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "steer_drive_controller_fail_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_fail_test/steer_drive_controller_wrong.test

@@ -0,0 +1,16 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+
+  <!-- Override with wrong controller configuration -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_fail_test/steerbot_wrong.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_fail_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_fail_test/steerbot_wrong.yaml

@@ -0,0 +1,8 @@
+steerbot_controller:
+  type: "steer_drive_controller/SteerDriveController"
+  rear_wheel  : 'this_joint_does_not_exist'
+  front_steer : 'front_steer_joint'
+  publish_rate: 50.0 # defaults to 50
+  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_limits_test/steer_drive_controller_limits.test

@@ -0,0 +1,16 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+
+  <!-- Load diff-drive limits -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_limits_test/steerbot_limits.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_limits_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_limits_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_limits_test/steer_drive_controller_limits_test.cpp

@@ -0,0 +1,242 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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 Paul Mathieu
+
+#include "../common/include/test_common.h"
+
+// TEST CASES
+TEST_F(SteerDriveControllerTest, testLinearJerkLimits)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  // send a big command
+  cmd_vel.linear.x = 10.0;
+  publish(cmd_vel);
+  // wait for a while
+  ros::Duration(0.5).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot speed is now 0.37m.s-1
+  EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE);
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
+
+  cmd_vel.linear.x = 0.0;
+  publish(cmd_vel);
+}
+
+TEST_F(SteerDriveControllerTest, testLinearAccelerationLimits)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  // send a big command
+  cmd_vel.linear.x = 10.0;
+  publish(cmd_vel);
+  // wait for a while
+  ros::Duration(0.5).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE);
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
+
+  cmd_vel.linear.x = 0.0;
+  publish(cmd_vel);
+}
+
+TEST_F(SteerDriveControllerTest, testLinearVelocityLimits)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  // send a big command
+  cmd_vel.linear.x = 10.0;
+  publish(cmd_vel);
+  // wait for a while
+  ros::Duration(5.0).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot speed is now 1.0 m.s-1, the limit
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE);
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
+
+  cmd_vel.linear.x = 0.0;
+  publish(cmd_vel);
+}
+
+TEST_F(SteerDriveControllerTest, testAngularJerkLimits)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  // send a big command
+  cmd_vel.angular.z = 10.0;
+  // send linear command too 
+  // because sending only angular command doesn't actuate wheels for steer drive mechanism
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  // wait for a while
+  ros::Duration(0.5).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot speed is now 0.18rad.s-1
+  EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.18, JERK_ANGULAR_VELOCITY_TOLERANCE);
+  // check if the robot speed is now 0.1m.s-1
+  EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE);
+
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+}
+
+TEST_F(SteerDriveControllerTest, testAngularAccelerationLimits)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  // send a big command
+  cmd_vel.angular.z = 10.0;
+  // send linear command too 
+  // because sending only angular command doesn't actuate wheels for steer drive mechanism
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  // wait for a while
+  ros::Duration(0.5).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot speed is now 0.25rad.s-1, which is 0.5.s-2 * 0.5s
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.25 + VELOCITY_TOLERANCE);
+  // check if the robot speed is now 0.1m.s-1
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE);
+
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+}
+
+TEST_F(SteerDriveControllerTest, testAngularVelocityLimits)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  cmd_vel.angular.z = 10.0;
+  // send linear command too 
+  // because sending only angular command doesn't actuate wheels for steer drive mechanism
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  // wait for a while
+  ros::Duration(5.0).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot speed is now 0.5rad.s-1, the limit
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.5 + ANGULAR_VELOCITY_TOLERANCE);
+  // check if the robot speed is now 0.1m.s-1
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE);
+
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "steer_drive_controller_limits_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  //ros::Duration(0.5).sleep();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_limits_test/steerbot_limits.yaml

@@ -0,0 +1,19 @@
+steerbot_controller:
+  linear:
+    x:
+      has_velocity_limits: true
+      min_velocity: -0.5
+      max_velocity: 1.0
+      has_acceleration_limits: true
+      min_acceleration: -0.5
+      max_acceleration: 1.0
+      has_jerk_limits: true
+      max_jerk: 5.0
+  angular:
+    z:
+      has_velocity_limits: true
+      max_velocity: 0.5
+      has_acceleration_limits: true
+      max_acceleration: 0.5
+      has_jerk_limits: true
+      max_jerk: 2.5

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_nan_test/steer_drive_controller_nan.test

@@ -0,0 +1,14 @@
+
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_nan_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_nan_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_nan_test/steer_drive_controller_nan_test.cpp

@@ -0,0 +1,92 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2014, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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 Enrique Fernandez
+
+#include "../common/include/test_common.h"
+
+// NaN
+#include <limits>
+
+// TEST CASES
+TEST_F(SteerDriveControllerTest, testNaN)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  ros::Duration(2.0).sleep();
+
+  // send a command
+  cmd_vel.linear.x = 0.1;
+  ros::Duration(2.0).sleep();
+
+  // stop robot (will generate NaN)
+  stop();
+  ros::Duration(2.0).sleep();
+
+  nav_msgs::Odometry odom = getLastOdom();
+
+  EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
+  EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
+
+  // start robot
+  start();
+  ros::Duration(2.0).sleep();
+
+  odom = getLastOdom();
+
+  EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
+  EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
+  EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "steer_drive_controller_nan_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_no_steer_test/steer_drive_controller_no_steer.test

@@ -0,0 +1,16 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+
+  <!-- Override with wrong controller configuration -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_no_steer_test/steerbot_no_steer.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_no_steer_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_no_steer_test/steerbot_no_steer.yaml

@@ -0,0 +1,8 @@
+steerbot_controller:
+  type: "steer_drive_controller/SteerDriveController"
+  rear_wheel  : 'rear_wheel_joint'
+  front_steer : ''
+  publish_rate: 50.0 # defaults to 50
+  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_no_wheel_test/steer_drive_controller_no_wheel.test

@@ -0,0 +1,16 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+
+  <!-- Override with wrong controller configuration -->
+  <rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_no_wheel_test/steerbot_no_wheel.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_no_wheel_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_no_wheel_test/steerbot_no_wheel.yaml

@@ -0,0 +1,8 @@
+steerbot_controller:
+  type: "steer_drive_controller/SteerDriveController"
+  rear_wheel  : ''
+  front_steer : 'front_steer_joint'
+  publish_rate: 50.0 # defaults to 50
+  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame.test

@@ -0,0 +1,19 @@
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
+
+  <!-- Set odom_frame_id to something new -->
+  <rosparam>
+    steerbot_controller:
+      odom_frame_id: new_odom
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="steer_drive_controller_odom_frame_test"
+        pkg="steer_drive_controller"
+        type="steer_drive_controller_odom_frame_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
+    <remap from="odom" to="steerbot_controller/odom" />
+  </test>
+</launch>

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame_test.cpp

@@ -0,0 +1,88 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2015, Locus Robotics Corp.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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 Eric Tappan
+
+#include "../common/include/test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(SteerDriveControllerTest, testNoOdomFrame)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // set up tf listener
+  tf::TransformListener listener;
+  ros::Duration(2.0).sleep();
+  // check the original odom frame doesn't exist
+  EXPECT_FALSE(listener.frameExists("odom"));
+}
+
+TEST_F(SteerDriveControllerTest, testNewOdomFrame)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  // set up tf listener
+  tf::TransformListener listener;
+  ros::Duration(2.0).sleep();
+  // check the new_odom frame does exist
+  EXPECT_TRUE(listener.frameExists("new_odom"));
+}
+
+TEST_F(SteerDriveControllerTest, testOdomTopic)
+{
+  // wait for ROS
+  while(!isControllerAlive())
+  {
+    ros::Duration(0.1).sleep();
+  }
+
+  ros::Duration(2.0).sleep();
+  // get an odom message
+  nav_msgs::Odometry odom_msg = getLastOdom();
+  // check its frame_id
+  ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom");
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "steer_drive_controller_odom_frame_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}