git commit -m "first commit"

.gitignore

@@ -0,0 +1,14 @@
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+

.gitmodules

@@ -0,0 +1,3 @@
+# [submodule "costmap_2d"]
+# 	path = navigations/costmap_2d
+# 	url = http://git.pnkx/HiepLM/costmap_2d.git

.vscode/settings.json

@@ -0,0 +1,9 @@
+{
+    "ros.distro": "noetic",
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

CMakeLists.txt

@@ -0,0 +1,69 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+project(Project)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching for catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()

README.md

@@ -0,0 +1,125 @@
+# Example dynamic_logistics_warehouse
+-----------------------------------------------
+* Terminal 1 : 
+
+```bash
+
+roslaunch mir_gazebo mir_empty_world.launch \
+world_name:=$(rospack find dynamic_logistics_warehouse)/worlds/warehouse.world robot_x:=9.0 robot_y:=25.0 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+```
+
+* Terminal 2 : 
+```bash
+### mapping:
+roslaunch mir_navigation hector_mapping.launch
+```
+
+```bash
+### localization:
+roslaunch mir_navigation amcl.launch initial_pose_x:=0.00 initial_pose_y:=0.00 \ initial_pose_a:=0.0 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+```
+* Terminal 3 : 
+
+```bash
+
+rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+```
+* Terminal 4 : 
+
+```bash
+# navigation:
+roslaunch mir_navigation start_planner.launch \
+map_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/warehouse.yaml \
+empty_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/map_empty.yaml \
+virtual_walls_map_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/virtual_walls.yaml \
+with_virtual_walls:=true \
+preferred_zones_map_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/preferred_zones.yaml \
+with_preferred_zones:=true \
+unpreferred_zones_map_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/unpreferred_zones.yaml \
+with_unpreferred_zones:=true \
+critical_zones_map_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/critical_zones.yaml \
+with_critical_zones:=true \
+direction_zones_map_file:=$(rospack find dynamic_logistics_warehouse)/maps/warehouse/direction_zones.yaml \
+with_direction_zones:=true 2> >(grep -v TF_REPEATED_DATA buffer_core)
+```
+```bash
+roslaunch mir_navigation move_base.xml with_virtual_walls:=false
+```
+
+# Default 
+
+```bash
+### gazebo:
+roslaunch mir_gazebo mir_maze_world.launch 2> >(grep -v TF_REPEATED_DATA buffer_core)
+rosservice call /gazebo/unpause_physics   # or click the "start" button in the Gazebo GUI
+
+### localization:
+roslaunch mir_navigation amcl.launch initial_pose_x:=10.0 initial_pose_y:=10.0  2> >(grep -v TF_REPEATED_DATA buffer_core)
+# or alternatively: roslaunch mir_gazebo fake_localization.launch delta_x:=-10.0 delta_y:=-10.0
+
+# navigation:
+roslaunch mir_navigation start_planner.launch \
+    map_file:=$(rospack find mir_gazebo)/maps/maze.yaml 
+
+roslaunch mir_navigation start_planner.launch \
+map_file:=$(rospack find mir_gazebo)/maps/warehouse/maze.yaml \
+virtual_walls_map_file:=$(rospack find mir_gazebo)/maps/warehouse/virtual_walls.yaml \
+with_virtual_walls:=true \
+preferred_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/preferred_zones.yaml \
+with_preferred_zones:=true \
+unpreferred_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/unpreferred_zones.yaml \
+with_unpreferred_zones:=true \
+critical_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/critical_zones.yaml \
+with_critical_zones:=true \
+direction_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/direction_zones.yaml \
+with_direction_zones:=true 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+# rviz
+rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz 2> >(grep -v TF_REPEATED_DATA buffer_core)
+```
+
+# Multilpe mir
+# start Gazebo + first MiR
+roslaunch mir_gazebo mir_maze_world.launch tf_prefix:=mir
+
+# first MiR: start localization, navigation + rviz
+roslaunch mir_navigation amcl.launch initial_pose_x:=10.0 initial_pose_y:=10.0 tf_prefix:=mir
+
+roslaunch mir_navigation start_planner.launch \
+map_file:=$(rospack find mir_gazebo)/maps/warehouse/maze.yaml \
+virtual_walls_map_file:=$(rospack find mir_gazebo)/maps/warehouse/virtual_walls.yaml \
+with_virtual_walls:=true \
+preferred_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/preferred_zones.yaml \
+with_preferred_zones:=true \
+unpreferred_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/unpreferred_zones.yaml \
+with_unpreferred_zones:=true \
+critical_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/critical_zones.yaml \
+with_critical_zones:=true \
+direction_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/direction_zones.yaml \
+with_direction_zones:=true prefix:=mir/ 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+ROS_NAMESPACE=mir rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+# spawn second MiR into Gazebo
+roslaunch mir_gazebo mir_gazebo_common.launch robot_x:=-2 robot_y:=-2 tf_prefix:=mir2 model_name:=mir2 __ns:=mir2
+
+# second MiR: start localization, navigation + rviz
+roslaunch mir_navigation amcl.launch initial_pose_x:=8.0 initial_pose_y:=8.0 tf_prefix:=mir2
+
+roslaunch mir_navigation start_planner.launch \
+map_file:=$(rospack find mir_gazebo)/maps/warehouse/maze.yaml \
+virtual_walls_map_file:=$(rospack find mir_gazebo)/maps/warehouse/virtual_walls.yaml \
+with_virtual_walls:=true \
+preferred_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/preferred_zones.yaml \
+with_preferred_zones:=true \
+unpreferred_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/unpreferred_zones.yaml \
+with_unpreferred_zones:=true \
+critical_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/critical_zones.yaml \
+with_critical_zones:=true \
+direction_zones_map_file:=$(rospack find mir_gazebo)/maps/warehouse/direction_zones.yaml \
+with_direction_zones:=true prefix:=mir2/ 2> >(grep -v TF_REPEATED_DATA buffer_core)
+
+ROS_NAMESPACE=mir2 rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz 2> >(grep -v TF_REPEATED_DATA buffer_core)

Setup.txt

@@ -0,0 +1,17 @@
+sudo apt install libsuitesparse-dev
+sudo apt install libspdlog-dev
+
+git clone https://github.com/gabime/spdlog.git -b master
+mkdir build
+cd build
+cmake .. -DSPDLOG_BUILD_SHARED=OFF -DSPDLOG_BUILD_STATIC=ON -DCMAKE_CXX_FLAGS=-fPIC
+make
+sudo rm /usr/local/lib/libspdlog.a
+sudo cp /path/to/spdlog/source/build/libspdlog.a /usr/local/lib/
+
+git clone https://github.com/RainerKuemmerle/g2o.git -b master
+mkdir build
+cd build
+cmake ../
+make
+sudo make install

mir_robot/.dockerfilelintrc

@@ -0,0 +1,2 @@
+rules:
+  apt-get-update_require_install: off

mir_robot/.flake8

@@ -0,0 +1,10 @@
+[flake8]
+# The line length here has to match the black config in pyproject.toml
+max-line-length = 120
+exclude =
+    .git,
+    __pycache__
+extend-ignore =
+    # See https://github.com/PyCQA/pycodestyle/issues/373
+    E203,
+    E741,

mir_robot/.github/workflows/github-actions.yml

@@ -0,0 +1,42 @@
+name: Build and run ROS tests
+on: [push, pull_request]
+jobs:
+  build:
+    strategy:
+      matrix:
+        rosdistro: [noetic]
+    runs-on: ubuntu-latest
+    container:
+      image: ros:${{ matrix.rosdistro }}-ros-core
+    steps:
+    - name: Install apt dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get install -y build-essential clang-format-10 file git python3-catkin-lint python3-catkin-tools python3-pip python3-rosdep
+    - name: Install pip dependencies
+      run: pip install pre-commit
+    - name: Checkout repository
+      uses: actions/checkout@v3
+      with:
+        path: src/mir_robot
+    - name: Use rosdep to install remaining dependencies
+      run: |
+        sudo rosdep init
+        rosdep update
+        rosdep install --from-paths src -i -y --rosdistro ${{ matrix.rosdistro }}
+    - name: Build
+      run: |
+        . /opt/ros/${{ matrix.rosdistro }}/setup.sh
+        catkin init
+        catkin config -j 1 -p 1
+        catkin build --limit-status-rate 0.1 --no-notify
+        catkin build --limit-status-rate 0.1 --no-notify --make-args tests
+    - name: Run tests
+      run: |
+        . devel/setup.sh
+        catkin run_tests
+        catkin_test_results
+    - name: Run pre-commit hooks
+      run: |
+        cd src/mir_robot
+        pre-commit run -a

mir_robot/.gitignore

@@ -0,0 +1 @@
+*.pyc

mir_robot/.mdl_style.rb

@@ -0,0 +1,25 @@
+# Style file for markdownlint
+
+all
+
+exclude_rule 'fenced-code-language' # Fenced code blocks should have a language specified
+exclude_rule 'first-line-h1' # First line in file should be a top level header
+
+exclude_rule 'ul-style'
+
+exclude_rule 'no-multiple-blanks'
+
+exclude_rule 'header-style'
+
+exclude_rule 'no-bare-urls'  # we need to use a bare URL in the README for the video to properly show on GitHub
+
+# Line lenght
+rule 'MD013', :line_length => 120, :code_blocks => false, :tables => false
+
+# Unordered list indentation
+rule 'MD007', :indent => 2
+
+# Ordered list item prefix
+rule 'MD029', :style => 'ordered'
+
+rule 'no-duplicate-header', :allow_different_nesting => true

mir_robot/.mdlrc

@@ -0,0 +1,2 @@
+git_recurse true
+style ".mdl_style.rb"

mir_robot/.pre-commit-config.yaml

@@ -0,0 +1,116 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.1.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-case-conflict
+      - id: check-executables-have-shebangs
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-shebang-scripts-are-executable
+      - id: check-symlinks
+      - id: check-vcs-permalinks
+      - id: check-xml
+      - id: check-yaml
+        exclude: mobipick_description/config/joint_limits.yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+        exclude: &excludes |
+          (?x)^(
+              .*\.blend|
+              .*\.dae|
+              .*\.mtl|
+              .*\.obj|
+              .*\.pgm|
+              .*\.step|
+              .*\.stl
+          )$
+      - id: fix-byte-order-marker
+      - id: mixed-line-ending
+        exclude: *excludes
+      - id: trailing-whitespace
+        exclude: *excludes
+
+  - repo: https://github.com/psf/black
+    rev: 22.3.0
+    hooks:
+      - id: black
+
+  - repo: https://github.com/PyCQA/flake8.git
+    rev: 5.0.4
+    hooks:
+    - id: flake8
+
+  - repo: https://github.com/detailyang/pre-commit-shell.git
+    rev: v1.0.6
+    hooks:
+    - id: shell-lint
+      args: [--external-sources]
+
+  - repo: https://github.com/markdownlint/markdownlint
+    rev: v0.11.0
+    hooks:
+    - id: markdownlint
+
+  - repo: https://github.com/pryorda/dockerfilelint-precommit-hooks
+    rev: v0.1.0
+    hooks:
+    - id: dockerfilelint
+
+  - repo: https://github.com/Lucas-C/pre-commit-hooks
+    rev: v1.3.1
+    hooks:
+      - id: insert-license
+        files: \.py$
+        exclude: |
+          (?x)^(
+              .*setup\.py|
+              .*__init__\.py|
+              .*test_copyright\.py|
+              .*test_pep257\.py|
+              mir_driver/nodes/tf_remove_child_frames\.py|
+              mir_navigation/mprim/genmprim_unicycle_highcost_5cm\.py
+          )$
+        args:
+          - --license-filepath
+          - LICENSE
+
+  - repo: https://github.com/pycqa/pydocstyle
+    rev: 6.1.1
+    hooks:
+      - id: pydocstyle
+        args:
+        - --ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404   # same as https://github.com/ament/ament_lint/blob/bbdaa17224f3b8dfece53662b65d7d18b7404b6a/ament_pep257/ament_pep257/main.py#L43-L55
+
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-10
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: [ "-fallback-style=none", "-i" ]
+      - id: catkin_lint
+        name: catkin_lint
+        description: Check package.xml and cmake files
+        entry: catkin_lint .
+        language: system
+        always_run: true
+        pass_filenames: false
+        args: [ "--strict", "--skip-pkg", "mobipick_moveit_config" ]   # have to skip this package because of the missing warehouse_ros_mongo dependency

mir_robot/Dockerfile-noetic

@@ -0,0 +1,34 @@
+FROM ros:noetic-ros-core
+
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends build-essential python3-rosdep python3-catkin-lint python3-catkin-tools \
+    && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/*
+
+# Install pre-commit hooks to /root/.cache/pre-commit/
+RUN apt-get update -qq \
+    && apt-get install -y -qq --no-install-recommends git python3-pip ruby shellcheck clang-format-10 python3-catkin-lint \
+    && rm -rf /var/lib/apt/lists/*
+RUN pip3 install pre-commit
+RUN mkdir -p /tmp/pre-commit
+COPY .pre-commit-config.yaml /tmp/pre-commit/
+RUN cd /tmp/pre-commit \
+    && git init \
+    && pre-commit install-hooks \
+    && rm -rf /tmp/pre-commit
+
+# Create ROS workspace
+COPY . /ws/src/mir_robot
+WORKDIR /ws
+
+# Use rosdep to install all dependencies (including ROS itself)
+RUN rosdep init \
+    && rosdep update \
+    && apt-get update \
+    && DEBIAN_FRONTEND=noninteractive rosdep install --from-paths src -i -y --rosdistro noetic \
+    && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/*
+
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \
+    catkin init && \
+    catkin config --install -j 1 -p 1 && \
+    catkin build --limit-status-rate 0.1 --no-notify && \
+    catkin build --limit-status-rate 0.1 --no-notify --make-args tests"

mir_robot/LICENSE

@@ -0,0 +1,27 @@
+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.

mir_robot/README.md

@@ -0,0 +1,388 @@
+mir_robot
+=========
+
+This repo contains a ROS driver and ROS configuration files (URDF description,
+Gazebo launch files, move_base config, bringup launch files, message and action
+descriptions) for the [MiR robots](http://www.mobile-industrial-robots.com/).
+This is a community project created by us ([DFKI](https://www.dfki.de/), the
+German Research Center for Artificial Intelligence) to use the MiR Robots with
+ROS. We are not affiliated with Mobile Industrial Robots. If you find a bug or
+missing feature in this software, please report it on the
+[issue tracker](https://github.com/DFKI-NI/mir_robot/issues).
+
+Supported MiR robots and software versions
+------------------------------------------
+
+This repo has been confirmed to work with the following robots:
+
+* MiR 100
+* MiR 200
+* MiR 500
+
+It probably also works with the MiR250 and MiR1000. If you can test it on one
+of those, please let us know if it works.
+
+This repo has been tested with the following MiR software versions:
+
+* 2.8.3.1
+* 2.13.4.1
+
+You can try if it works with other versions, but these are the ones that are
+known to work.
+
+
+Package overview
+----------------
+
+* `mir_actions`: Action definitions for the MiR robot
+* `mir_description`: URDF description of the MiR robot
+* `mir_dwb_critics`: Plugins for the dwb_local_planner used in Gazebo
+* `mir_driver`: A reverse ROS bridge for the MiR robot
+* `mir_gazebo`: Simulation specific launch and configuration files for the MiR robot
+* `mir_msgs`: Message definitions for the MiR robot
+* `mir_navigation`: move_base launch and configuration files
+
+
+Installation
+------------
+
+You can chose between binary and source install below. If you don't want to
+modify the source, the binary install is preferred (if `mir_robot` binary
+packages are available for your ROS distro). The instructions below use the ROS
+distro `noetic` as an example; if you use a different distro (e.g.  `melodic`),
+replace all occurrences of the string `noetic` by your distro name in the
+instructions.
+
+### Preliminaries
+
+If you haven't already installed ROS on your PC, you need to add the ROS apt
+repository. This step is necessary for either binary or source install.
+
+```
+sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+sudo apt-get update -qq
+```
+
+### Binary install
+
+For a binary install, it suffices to run this command:
+
+```bash
+sudo apt install ros-noetic-mir-robot
+```
+
+See the tables at the end of this README for a list of ROS distros for which
+binary packages are available.
+
+### Source install
+
+For a source install, run the commands below instead of the command from the
+"binary install" section.
+
+```bash
+# create a catkin workspace
+mkdir -p ~/catkin_ws/src
+cd ~/catkin_ws/src/
+
+# clone mir_robot into the catkin workspace
+git clone -b noetic https://github.com/DFKI-NI/mir_robot.git
+
+# use rosdep to install all dependencies (including ROS itself)
+sudo apt-get update -qq
+sudo apt-get install -qq -y python-rosdep
+sudo rosdep init
+rosdep update
+rosdep install --from-paths ./ -i -y --rosdistro noetic
+
+# build all packages in the catkin workspace
+source /opt/ros/noetic/setup.bash
+catkin_init_workspace
+cd ~/catkin_ws
+catkin_make -DCMAKE_BUILD_TYPE=RelWithDebugInfo
+```
+
+In case you encounter problems, please compare the commands above to the build
+step in [`.github/workflows/github-actions.yml`](.github/workflows/github-actions.yml); that should always have the most
+recent list of commands.
+
+You should add the following line to the end of your `~/.bashrc`, and then
+close and reopen all terminals:
+
+```bash
+source ~/catkin_ws/devel/setup.bash
+```
+
+Gazebo demo (existing map)
+--------------------------
+
+https://user-images.githubusercontent.com/320188/145610491-2afeb46c-3729-4106-ab9c-6681b5dd9d2e.mp4
+
+```bash
+### gazebo:
+roslaunch mir_gazebo mir_maze_world.launch
+rosservice call /gazebo/unpause_physics   # or click the "start" button in the Gazebo GUI
+
+### localization:
+roslaunch mir_navigation amcl.launch initial_pose_x:=10.0 initial_pose_y:=10.0
+# or alternatively: roslaunch mir_gazebo fake_localization.launch delta_x:=-10.0 delta_y:=-10.0
+
+# navigation:
+roslaunch mir_navigation start_planner.launch \
+    map_file:=$(rospack find mir_gazebo)/maps/maze.yaml \
+    virtual_walls_map_file:=$(rospack find mir_gazebo)/maps/maze_virtual_walls.yaml
+rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz
+```
+
+Now, you can use the "2D Nav Goal" tool in RViz to set a navigation goal for move_base.
+
+
+Gazebo demo (mapping)
+---------------------
+
+```bash
+### gazebo:
+roslaunch mir_gazebo mir_maze_world.launch
+rosservice call /gazebo/unpause_physics   # or click the "start" button in the Gazebo GUI
+
+### mapping:
+roslaunch mir_navigation hector_mapping.launch
+
+# navigation:
+roslaunch mir_navigation move_base.xml with_virtual_walls:=false
+rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz
+```
+
+Gazebo demo (MiR 250 in warehouse Gazebo world)
+-----------------------------------------------
+
+https://user-images.githubusercontent.com/320188/171613044-639f3ab2-fe84-4839-acfc-d0642f8869b3.mp4
+
+This repo contains URDF descriptions for the MiR 100 (default) and the MiR 250.
+You can switch to the MiR 250 by adding **`mir_type:=mir_250`** to the gazebo
+roslaunch command. You can also select another Gazebo world using the
+**`world_name`** argument. For example, the video above was generated using the
+following commands:
+
+```bash
+cd <your catkin workspace>
+git clone -b ros1 https://github.com/aws-robotics/aws-robomaker-small-warehouse-world.git
+catkin build
+
+roslaunch mir_gazebo mir_empty_world.launch \
+        world_name:=$(rospack find aws_robomaker_small_warehouse_world)/worlds/no_roof_small_warehouse.world \
+        mir_type:=mir_250
+```
+
+... and then running the remaining commands from the "mapping" section above.
+
+
+Gazebo demo (multiple robots)
+-----------------------------
+
+If you want to spawn multiple robots into Gazebo, you unfortunately have to
+hard-code the name of the second robot into the `mir_empty_world.launch` file,
+like this:
+
+```diff
+diff --git i/mir_gazebo/launch/mir_empty_world.launch w/mir_gazebo/launch/mir_empty_world.launch
+index 27b9159..7773fae 100644
+--- i/mir_gazebo/launch/mir_empty_world.launch
++++ w/mir_gazebo/launch/mir_empty_world.launch
+@@ -17,6 +17,10 @@
+       <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" />
+
++      <remap from="mir2/joint_states"                   to="mir2/mir/joint_states" />
++      <remap from="mir2/mobile_base_controller/cmd_vel" to="mir2/cmd_vel" />
++      <remap from="mir2/mobile_base_controller/odom"    to="mir2/odom" />
++
+       <include file="$(find gazebo_ros)/launch/empty_world.launch">
+         <arg name="world_name" value="$(arg world_name)"/>
+         <arg name="paused" value="true" />
+```
+
+Then you can run the simulation like this:
+
+```bash
+# start Gazebo + first MiR
+roslaunch mir_gazebo mir_maze_world.launch tf_prefix:=mir
+
+# first MiR: start localization, navigation + rviz
+roslaunch mir_navigation amcl.launch initial_pose_x:=10.0 initial_pose_y:=10.0 tf_prefix:=mir#
+roslaunch mir_navigation start_planner.launch \
+        map_file:=$(rospack find mir_gazebo)/maps/maze.yaml \
+        virtual_walls_map_file:=$(rospack find mir_gazebo)/maps/maze_virtual_walls.yaml prefix:=mir/
+ROS_NAMESPACE=mir rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz
+
+# spawn second MiR into Gazebo
+roslaunch mir_gazebo mir_gazebo_common.launch robot_x:=-2 robot_y:=-2 tf_prefix:=mir2 model_name:=mir2 __ns:=mir2
+
+# second MiR: start localization, navigation + rviz
+roslaunch mir_navigation amcl.launch initial_pose_x:=8.0 initial_pose_y:=8.0 tf_prefix:=mir2
+roslaunch mir_navigation start_planner.launch \
+        map_file:=$(rospack find mir_gazebo)/maps/maze.yaml \
+        virtual_walls_map_file:=$(rospack find mir_gazebo)/maps/maze_virtual_walls.yaml prefix:=mir2/
+ROS_NAMESPACE=mir2 rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz
+```
+
+
+Running the driver on the real robot
+------------------------------------
+
+### Start up the robot
+
+* switch on MiR base
+
+
+### Connect to the MiR web interface
+
+* connect to MiR_R??? wifi (password "mirex4you"), for example from your Android phone/tablet
+* disable other network connections (mobile data / LAN / etc.)
+* open mir.com (192.168.12.20) in Chrome (!)
+* log in (admin/mir4you)
+
+
+### Synchronize system time
+
+The internal robot PC's is not synchronized (for example via NTP), so it tends
+to get out of sync quickly (about 1 second per day!). This causes TF transforms
+timing out etc. and can be seen using `tf_monitor` (the "Max Delay" is about
+3.3 seconds, but should be less than 0.1 seconds):
+
+```
+$ rosrun tf tf_monitor
+Frames:
+Frame: /back_laser_link published by unknown_publisher Average Delay: 3.22686 Max Delay: 3.34766
+Frame: /base_footprint published by unknown_publisher Average Delay: 3.34273 Max Delay: 3.38062
+Frame: /base_link published by unknown_publisher Average Delay: 3.22751 Max Delay: 3.34844
+Frame: /front_laser_link published by unknown_publisher Average Delay: 3.22661 Max Delay: 3.34159
+Frame: /imu_link published by unknown_publisher Average Delay: 3.22739 Max Delay: 3.34738
+Frame: /odom published by unknown_publisher Average Delay: 3.16493 Max Delay: 3.28667
+[...]
+
+All Broadcasters:
+Node: unknown_publisher 418.344 Hz, Average Delay: 0.827575 Max Delay: 3.35237
+Node: unknown_publisher(static) 465.362 Hz, Average Delay: 0 Max Delay: 0
+```
+
+To fix this:
+
+* go to "Service" -> "Configuration" -> "System settings" -> "Time settings" -> "Set device time on robot"
+
+Afterwards, the ROS software on the robot will restart, so you'll have to start `move_base` again (see below).
+
+If you have an external PC on the MiR platform, you can use `chrony` to automatically synchronize system time (see below).
+
+
+### Start `move_base` on the robot
+
+* go to "Service" -> "Configuration" -> "Launch menu", start "Planner"; this starts `move_base` and `amcl` on the robot
+
+
+### Teleoperate the robot (optional)
+
+* go to "Manual", press yellow button (LEDs change from yellow to blue); now the robot can be teleoperated
+
+
+### Relocalize robot (optional)
+
+If the robot's localization is lost:
+
+* go to "Service" -> "Command view" -> "Set start position" and click + drag to current position of robot in the map
+* click "Adjust"
+
+
+### Start the ROS driver
+
+```bash
+roslaunch mir_driver mir.launch
+```
+
+Advanced
+--------
+
+### Installing chrony to synchronize system time automatically
+
+If you have an external PC integrated into your robot that is on the same wired
+network as the MiR PC, you can use `chrony` to automatically synchronize the
+MiR's system time. Unfortunately, this method is not easy to install.
+
+Let's call the external PC `external-pc`. That PC's clock is our reference
+clock. It is synced to an NTP clock whenever the `external-pc` has access to
+the internet. To implement this synchronization solution, install `chrony` on
+both the `external-pc` and the internal PC of the MiR, and set up the
+`external-pc` as the chrony server and the internal MiR PC as the chrony
+client. This way, the clocks on these systems always stay in sync without any
+manual interaction.
+
+To install things on the internal MiR PC:
+
+* connect a monitor and keyboard to the ports that are exposed on one corner of the MiR
+* boot into a live USB linux system
+* `chroot` into the MiR PC
+* download `chrony_2.1.1-1ubuntu0.1_amd64.deb`,
+  `libtomcrypt0_1.17-7ubuntu0.1_amd64.deb`, `libtommath0_0.42.0-1.2_amd64.deb`
+  and `timelimit_1.8-1_amd64.deb` from a PC that has internet and install them
+  in the `chroot` environment onto the MiR PC using `dpkg -i`
+* set up `/etc/chrony/chrony.conf`
+
+
+Troubleshooting
+---------------
+
+### Got a result when we were already in the DONE state
+
+Sometimes the move_base action will print the warning "Got a result when we
+were already in the DONE state". This is caused by a race condition between the
+`/move_base/result` and `/move_base/status` topics. When a status message with
+status `SUCCEEDED` arrives before the corresponding result message, this
+warning will be printed. It can be safely ignored.
+
+
+### Gazebo prints errors: "No p gain specified for pid."
+
+These errors are expected and can be ignored.
+
+Unfortunately, we cannot set the PID gains (to silence the error) due to the
+following behavior of Gazebo:
+
+1. When using the `PositionJointInterface`, you *must* set the PID values for the
+   joints using that interface, otherwise you will run into
+   [this bug](https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612).
+2. When using the `VelocityJointInterface`, if you omit the PID values, Gazebo
+   just perfectly follows the commanded velocities. If you specify PID values,
+   Gazebo will use a PID controller to approximate following the commanded
+   velocities, so you have to tune the PID controllers.
+
+Since we just want Gazebo to follow our commanded velocities, we cannot set the
+PID values for joints using the VelocityJointInterface, so the errors get
+printed (but can be ignored).
+
+
+GitHub Actions - Continuous Integration
+---------------------------------------
+
+| Noetic                                                                                                                                                                               |
+|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [![Build Status](https://github.com/DFKI-NI/mir_robot/actions/workflows/github-actions.yml/badge.svg)](https://github.com/DFKI-NI/mir_robot/actions/workflows/github-actions.yml/) |
+
+
+ROS Buildfarm
+-------------
+
+|                                                        | Melodic source deb                                                                                                                                                                      | Melodic binary deb                                                                                                                                                                                      | Noetic source deb                                                                                                                                                                     | Noetic binary deb                                                                                                                                                                                     |
+|--------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [mir_actions](http://wiki.ros.org/mir_actions)         | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_actions__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_actions__ubuntu_bionic__source/)         | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_actions__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_actions__ubuntu_bionic_amd64__binary/)         | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_actions__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_actions__ubuntu_focal__source/)         | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_actions__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_actions__ubuntu_focal_amd64__binary/)         |
+| [mir_description](http://wiki.ros.org/mir_description) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_description__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_description__ubuntu_bionic__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_description__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_description__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_description__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_description__ubuntu_focal__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_description__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_description__ubuntu_focal_amd64__binary/) |
+| [mir_driver](http://wiki.ros.org/mir_driver)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_driver__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_driver__ubuntu_bionic__source/)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_driver__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_driver__ubuntu_bionic_amd64__binary/)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_driver__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_driver__ubuntu_focal__source/)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_driver__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_driver__ubuntu_focal_amd64__binary/)           |
+| [mir_dwb_critics](http://wiki.ros.org/mir_dwb_critics) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_dwb_critics__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_dwb_critics__ubuntu_bionic__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_dwb_critics__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_dwb_critics__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_dwb_critics__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_dwb_critics__ubuntu_focal__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_dwb_critics__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_dwb_critics__ubuntu_focal_amd64__binary/) |
+| [mir_gazebo](http://wiki.ros.org/mir_gazebo)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_gazebo__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_gazebo__ubuntu_bionic__source/)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_gazebo__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_gazebo__ubuntu_bionic_amd64__binary/)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_gazebo__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_gazebo__ubuntu_focal__source/)           | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_gazebo__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_gazebo__ubuntu_focal_amd64__binary/)           |
+| [mir_msgs](http://wiki.ros.org/mir_msgs)               | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_msgs__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_msgs__ubuntu_bionic__source/)               | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_msgs__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_msgs__ubuntu_bionic_amd64__binary/)               | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_msgs__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_msgs__ubuntu_focal__source/)               | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_msgs__ubuntu_focal_amd64__binary/)               |
+| [mir_navigation](http://wiki.ros.org/mir_navigation)   | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_navigation__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_navigation__ubuntu_bionic__source/)   | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_navigation__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_navigation__ubuntu_bionic_amd64__binary/)   | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_navigation__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_navigation__ubuntu_focal__source/)   | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_navigation__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_navigation__ubuntu_focal_amd64__binary/)   |
+| [mir_robot](http://wiki.ros.org/mir_robot)             | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__mir_robot__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__mir_robot__ubuntu_bionic__source/)             | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__mir_robot__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__mir_robot__ubuntu_bionic_amd64__binary/)             | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__mir_robot__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__mir_robot__ubuntu_focal__source/)             | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__mir_robot__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__mir_robot__ubuntu_focal_amd64__binary/)             |
+| [sdc21x0](http://wiki.ros.org/sdc21x0)                 | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__sdc21x0__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__sdc21x0__ubuntu_bionic__source/)                 | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__sdc21x0__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__sdc21x0__ubuntu_bionic_amd64__binary/)                 | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__sdc21x0__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__sdc21x0__ubuntu_focal__source/)                 | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__sdc21x0__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__sdc21x0__ubuntu_focal_amd64__binary/)                 |
+
+|                                            | Melodic devel                                                                                                                                                    | Melodic doc                                                                                                                                                      | Noetic devel                                                                                                                                                   | Noetic doc                                                                                                                                                     |
+|--------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [mir_robot](http://wiki.ros.org/mir_robot) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__mir_robot__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__mir_robot__ubuntu_bionic_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdoc__mir_robot__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdoc__mir_robot__ubuntu_bionic_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__mir_robot__ubuntu_focal_amd64)](http://build.ros.org/job/Ndev__mir_robot__ubuntu_focal_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndoc__mir_robot__ubuntu_focal_amd64)](http://build.ros.org/job/Ndoc__mir_robot__ubuntu_focal_amd64) |

mir_robot/diff_drive_controller/CHANGELOG.rst

@@ -0,0 +1,237 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package diff_drive_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.21.2 (2023-09-03)
+-------------------
+
+0.21.1 (2023-01-31)
+-------------------
+* Don't hardcode plugin library path
+* Contributors: Jochen Sprickerhof
+
+0.21.0 (2022-10-21)
+-------------------
+* std::bind and placeholders instead of boost
+* use boost::placeholders::_1/_2 in remaining instances, include boost/bind/bind.hpp instead of boost/bind.hpp, eliminated unnecessary bind.hpp include
+* Contributors: Lucas Walter
+
+0.20.0 (2022-05-06)
+-------------------
+* Drop old C++ standard
+* Use new boost bind placeholders
+* Add <?xml version=1.0?> to every .launch and .test file
+* Contributors: Jochen Sprickerhof, Lucas Walter
+
+0.19.0 (2021-06-13)
+-------------------
+* fix NaN bug
+* fix test to expose NaN bug
+* Wait long enough for accumulator to be cleared
+* Add test for `#532 <https://github.com/ros-controls/ros_controllers/issues/532>`_
+  Close `#540 <https://github.com/ros-controls/ros_controllers/issues/540>`_
+* Contributors: Caio Amaral, Matt Reynolds, Melvin Wang
+
+0.18.1 (2020-12-03)
+-------------------
+* Fix null pointer checks in diff_drive_controller
+* Use version-agnostic FindBoost for headers
+* Contributors: David V. Lu, Matt Reynolds
+
+0.18.0 (2020-10-11)
+-------------------
+* Fix dependency on Boost
+* Apply consistent format to CMakeLists.txt
+* Update package.xml to format 3
+* Clean dependencies of diff_drive_controller package
+* Apply waitForController method to all diff_drive_controller tests
+* Move odom_pub setup to the end to allow consistent isControllerAlive check
+* Contributors: Mateus Amarante
+
+0.17.0 (2020-05-12)
+-------------------
+
+0.16.1 (2020-04-27)
+-------------------
+
+0.16.0 (2020-04-16)
+-------------------
+* Fix warning dynamic_reconfigure
+* Bump CMake version to prevent CMP0048
+* Add missing header guards
+* Replace header guard with #pragma once
+* Prefix every xacro tag with 'xacro:'
+* Modernize xacro
+  - Remove '--inorder'
+  - Use 'xacro' over 'xacro.py'
+* switch implementation of read and write methods of Diffbot class
+* Refactor nan test
+  EXPECT_NE(x, bool) -> EXPECT_TRUE/FALSE(x)
+  EXPECT_EQ(x, double) -> EXPECT_DOUBLE_EQ(x, double)
+  + clang default reformat
+* Check for nan cmd_vel
+* Contributors: Anas Abou Allaban, Bence Magyar, Franz, Matt Reynolds, Raffaello Bonghi
+
+0.15.1 (2020-03-09)
+-------------------
+* Use nullptr
+* add missing pluginlib deps.
+* Update null link pointer error message
+* Revert cmake include catkin_INCLUDE_DIRS as system
+* Use C++11 `std::this_thread::sleep_for`.
+* Contributors: Bence Magyar, Enrique Fernandez Perdomo, Matt Reynolds, Sean Yen
+
+0.15.0 (2019-03-26)
+-------------------
+* Default all controller builds to C++14
+* boost::chrono -> std::chrono
+* boost::assign -> C++ initializer list
+* boost::shared_ptr -> std::shared_ptr
+* Using left/right multiplies for desired vel
+* diff-drive publish joint trajectory controller state
+* fix install destination for libraries (`#403 <https://github.com/ros-controls/ros_controllers/issues/403>`_)
+* Contributors: Bence Magyar, Gennaro Raiola, James Xu, Jeremie Deray, Jordan Palacios
+
+0.14.3 (2019-02-09)
+-------------------
+* use operators instead of aliases
+* Fix typo descripion -> description
+* Contributors: Daniel Ingram, James Xu
+
+0.14.2 (2018-10-23)
+-------------------
+
+0.14.1 (2018-06-26)
+-------------------
+* Added 'multiplier' in DynamicParams ostream and changed boolean printing to 'enabled/disabled'
+* isPublishngCmdVelOut to check getNumPublisheres until timeout
+* Contributors: Kei Okada, Martin Ganeff
+
+0.14.0 (2018-04-27)
+-------------------
+* add dynamic_reconf to diff_drive_controller
+* migrate to new pluginlib headers
+* per wheel radius multiplier
+* fix xacro macro warning
+* [DiffDrive] Fix time-sensitive tests of diff_drive_controller
+* separate include_directories as SYSTEM to avoid unrelated compilation warnings
+* Contributors: Jeremie Deray, Mathias Lüdtke
+
+0.13.2 (2017-12-23)
+-------------------
+
+0.13.1 (2017-11-06)
+-------------------
+
+0.13.0 (2017-08-10)
+-------------------
+* Add test for allow_multiple_cmd_vel_publishers param
+* add check for multiple publishers on cmd_vel
+* Added tests for the odom_frame_id parameter.
+* Parameterized diff_drive_controller's odom_frame_id
+* Publish executed velocity if publish_cmd
+* refactor to remove code duplication
+* fixup pointer type for new convention
+* Allow diff_drive_controller to use spheres as well as cylinders for wheel collision geometry. Cylinders are not well behaved on Gazebo/ODE heightfields, using spheres works around the issue.
+* Contributors: Bence Magyar, Eric Tappan, Jeremie Deray, Karsten Knese, Tully Foote, mallanmba, tappan-at-git
+
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+* Add exporting include dirs
+* Contributors: Bence Magyar
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Add urdf compatibility header
+* Add --inorder to xacro calls
+* Add missing xacro tags
+* Use xacro instead of xacro.py
+* Disable angular jerk limit test
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Contributors: Bence Magyar
+
+0.11.2 (2016-08-16)
+-------------------
+
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
+0.10.0 (2015-11-20)
+-------------------
+* 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 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
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Enrique Fernandez, Paul Mathieu
+
+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.

mir_robot/diff_drive_controller/CMakeLists.txt

@@ -0,0 +1,216 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(diff_drive_controller)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS
+    controller_interface
+    control_msgs
+    dynamic_reconfigure
+    geometry_msgs
+    hardware_interface
+    nav_msgs
+    pluginlib
+    realtime_tools
+    tf
+    urdf
+)
+
+find_package(Boost REQUIRED)
+
+# Declare a catkin package
+generate_dynamic_reconfigure_options(cfg/DiffDriveController.cfg)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS
+    controller_interface
+    control_msgs
+    dynamic_reconfigure
+    geometry_msgs
+    hardware_interface
+    nav_msgs
+    realtime_tools
+    tf
+  LIBRARIES ${PROJECT_NAME}
+  DEPENDS Boost
+)
+
+
+###########
+## Build ##
+###########
+
+# Specify header include paths
+include_directories(
+  include ${catkin_INCLUDE_DIRS}
+  include ${Boost_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/diff_drive_controller.cpp
+  src/odometry.cpp
+  src/speed_limiter.cpp
+)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
+
+
+#############
+## Testing ##
+#############
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(controller_manager REQUIRED)
+  find_package(rosgraph_msgs REQUIRED)
+  find_package(std_srvs REQUIRED)
+  find_package(rostest REQUIRED)
+  
+  include_directories(
+    ${controller_manager_INCLUDE_DIRS}
+    ${rosgraph_msgs_INCLUDE_DIRS}
+    ${std_srvs_INCLUDE_DIRS}
+  )
+
+  set(test_LIBRARIES
+    ${controller_manager_LIBRARIES}
+    ${rosgraph_msgs_LIBRARIES}
+    ${std_srvs_LIBRARIES}
+  )
+
+  add_executable(diffbot test/diffbot.cpp)
+  target_link_libraries(diffbot ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_executable(skidsteerbot test/skidsteerbot.cpp)
+  target_link_libraries(skidsteerbot ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_dependencies(tests diffbot skidsteerbot)
+
+  add_rostest_gtest(diff_drive_test
+    test/diff_drive_controller.test
+    test/diff_drive_test.cpp
+  )
+  target_link_libraries(diff_drive_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_nan_test
+    test/diff_drive_controller_nan.test
+    test/diff_drive_nan_test.cpp
+  )
+  target_link_libraries(diff_drive_nan_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_limits_test
+    test/diff_drive_controller_limits.test
+    test/diff_drive_limits_test.cpp
+  )
+  target_link_libraries(diff_drive_limits_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_timeout_test
+    test/diff_drive_timeout.test
+    test/diff_drive_timeout_test.cpp
+  )
+  target_link_libraries(diff_drive_timeout_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest(test/diff_drive_multipliers.test)
+
+  add_rostest(test/diff_drive_left_right_multipliers.test)
+
+  add_rostest_gtest(diff_drive_fail_test
+    test/diff_drive_wrong.test
+    test/diff_drive_fail_test.cpp
+  )
+  target_link_libraries(diff_drive_fail_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_odom_tf_test
+    test/diff_drive_odom_tf.test
+    test/diff_drive_odom_tf_test.cpp
+  )
+  target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_default_cmd_vel_out_test
+    test/diff_drive_default_cmd_vel_out.test
+    test/diff_drive_default_cmd_vel_out_test.cpp
+  )
+  target_link_libraries(diff_drive_default_cmd_vel_out_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_pub_cmd_vel_out_test
+    test/diff_drive_pub_cmd_vel_out.test
+    test/diff_drive_pub_cmd_vel_out_test.cpp
+  )
+  target_link_libraries(diff_drive_pub_cmd_vel_out_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_default_wheel_joint_controller_state_test
+    test/diff_drive_default_wheel_joint_controller_state.test
+    test/diff_drive_default_wheel_joint_controller_state_test.cpp
+  )
+  target_link_libraries(diff_drive_default_wheel_joint_controller_state_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_publish_wheel_joint_controller_state_test
+    test/diff_drive_publish_wheel_joint_controller_state.test
+    test/diff_drive_publish_wheel_joint_controller_state_test.cpp
+  )
+  target_link_libraries(diff_drive_publish_wheel_joint_controller_state_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_default_odom_frame_test
+    test/diff_drive_default_odom_frame.test
+    test/diff_drive_default_odom_frame_test.cpp
+  )
+  target_link_libraries(diff_drive_default_odom_frame_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_odom_frame_test
+    test/diff_drive_odom_frame.test
+    test/diff_drive_odom_frame_test.cpp
+  )
+  target_link_libraries(diff_drive_odom_frame_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest_gtest(diff_drive_multiple_cmd_vel_publishers_test
+    test/diff_drive_multiple_cmd_vel_publishers.test
+    test/diff_drive_multiple_cmd_vel_publishers_test.cpp
+  )
+  target_link_libraries(diff_drive_multiple_cmd_vel_publishers_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+  add_rostest(test/diff_drive_open_loop.test)
+
+  add_rostest(test/skid_steer_controller.test)
+
+  add_rostest(test/skid_steer_no_wheels.test)
+
+  add_rostest(test/diff_drive_radius_sphere.test)
+
+  add_rostest(test/diff_drive_radius_param.test)
+
+  add_rostest(test/diff_drive_radius_param_fail.test)
+
+  add_rostest(test/diff_drive_separation_param.test)
+
+  add_rostest(test/diff_drive_bad_urdf.test)
+
+  add_rostest(test/diff_drive_get_wheel_radius_fail.test)
+
+  add_rostest_gtest(diff_drive_dyn_reconf_test
+    test/diff_drive_dyn_reconf.test
+    test/diff_drive_dyn_reconf_test.cpp
+  )
+  target_link_libraries(diff_drive_dyn_reconf_test ${catkin_LIBRARIES} ${test_LIBRARIES})
+
+endif()
+
+
+#############
+## Install ##
+#############
+
+# Install headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+# Install library
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+# Install plugins
+install(FILES diff_drive_controller_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

mir_robot/diff_drive_controller/README.md

@@ -0,0 +1,5 @@
+## Diff Drive Controller ##
+
+Controller for a differential drive mobile base.
+
+Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/diff_drive_controller).

mir_robot/diff_drive_controller/cfg/DiffDriveController.cfg

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+PACKAGE = 'diff_drive_controller'
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t
+
+gen = ParameterGenerator()
+
+# Kinematic parameters related
+gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
+gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)
+gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
+
+# Publication related
+gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0)
+gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", True)
+
+exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController"))

mir_robot/diff_drive_controller/diff_drive_controller_plugins.xml

@@ -0,0 +1,9 @@
+<library path="libdiff_drive_controller">
+
+  <class name="diff_drive_controller/DiffDriveController" type="diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The DiffDriveController tracks velocity commands. It expects 2 VelocityJointInterface type of hardware interfaces.
+    </description>
+  </class>
+
+</library>

mir_robot/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -0,0 +1,312 @@
+/*********************************************************************
+ * 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, Enrique Fernández
+ */
+
+#pragma once
+
+
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <controller_interface/controller.h>
+#include <diff_drive_controller/DiffDriveControllerConfig.h>
+#include <diff_drive_controller/odometry.h>
+#include <diff_drive_controller/speed_limiter.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <memory>
+#include <nav_msgs/Odometry.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <tf/tfMessage.h>
+
+namespace diff_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 or sphere in the urdf
+   *  - a wheel joint frame center's vertical projection on the floor must lie within the contact patch
+   */
+  class DiffDriveController
+      : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
+  {
+  public:
+    DiffDriveController();
+
+    /**
+     * \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,
+              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:
+    std::vector<hardware_interface::JointHandle> left_wheel_joints_;
+    std::vector<hardware_interface::JointHandle> right_wheel_joints_;
+
+    // Previous time
+    ros::Time time_previous_;
+
+    /// Previous velocities from the encoders:
+    std::vector<double> vel_left_previous_;
+    std::vector<double> vel_right_previous_;
+
+    /// Previous velocities from the encoders:
+    double vel_left_desired_previous_;
+    double vel_right_desired_previous_;
+
+    /// 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_;
+
+    /// Publish executed commands
+    std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> > cmd_vel_pub_;
+
+    /// Odometry related:
+    std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
+    Odometry odometry_;
+
+    /// Controller state publisher
+    std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> > controller_state_pub_;
+
+    /// Wheel separation, wrt the midpoint of the wheel width:
+    double wheel_separation_;
+
+    /// 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_multiplier_;
+    double left_wheel_radius_multiplier_;
+    double right_wheel_radius_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_;
+
+    /// Speed limiters:
+    Commands last1_cmd_;
+    Commands last0_cmd_;
+    SpeedLimiter limiter_lin_;
+    SpeedLimiter limiter_ang_;
+
+    /// Publish limited velocity:
+    bool publish_cmd_;
+
+    /// Publish wheel data:
+    bool publish_wheel_joint_controller_state_;
+
+    // A struct to hold dynamic parameters
+    // set from dynamic_reconfigure server
+    struct DynamicParams
+    {
+      bool update;
+
+      double left_wheel_radius_multiplier;
+      double right_wheel_radius_multiplier;
+      double wheel_separation_multiplier;
+
+      bool publish_cmd;
+
+      double publish_rate;
+      bool enable_odom_tf;
+
+      DynamicParams()
+        : left_wheel_radius_multiplier(1.0)
+        , right_wheel_radius_multiplier(1.0)
+        , wheel_separation_multiplier(1.0)
+        , publish_cmd(false)
+        , publish_rate(50)
+        , enable_odom_tf(true)
+      {}
+
+      friend std::ostream& operator<<(std::ostream& os, const DynamicParams& params)
+      {
+        os << "DynamicParams:\n"
+           //
+           << "\tOdometry parameters:\n"
+           << "\t\tleft wheel radius multiplier: "   << params.left_wheel_radius_multiplier  << "\n"
+           << "\t\tright wheel radius multiplier: "  << params.right_wheel_radius_multiplier << "\n"
+           << "\t\twheel separation multiplier: "    << params.wheel_separation_multiplier   << "\n"
+           //
+           << "\tPublication parameters:\n"
+           << "\t\tPublish executed velocity command: " << (params.publish_cmd?"enabled":"disabled") << "\n"
+           << "\t\tPublication rate: " << params.publish_rate                 << "\n"
+           << "\t\tPublish frame odom on tf: " << (params.enable_odom_tf?"enabled":"disabled");
+
+        return os;
+      }
+    };
+
+    realtime_tools::RealtimeBuffer<DynamicParams> dynamic_params_;
+
+    /// Dynamic Reconfigure server
+    typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> ReconfigureServer;
+    std::shared_ptr<ReconfigureServer> dyn_reconf_server_;
+    boost::recursive_mutex dyn_reconf_server_mutex_;
+
+  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 Get the wheel names from a wheel param
+     * \param [in]  controller_nh Controller node handler
+     * \param [in]  wheel_param   Param name
+     * \param [out] wheel_names   Vector with the whel names
+     * \return true if the wheel_param is available and the wheel_names are
+     *        retrieved successfully from the param server; false otherwise
+     */
+    bool getWheelNames(ros::NodeHandle& controller_nh,
+                       const std::string& wheel_param,
+                       std::vector<std::string>& wheel_names);
+
+    /**
+     * \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& left_wheel_name,
+                               const std::string& right_wheel_name,
+                               bool lookup_wheel_separation,
+                               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);
+
+    /**
+     * \brief Callback for dynamic_reconfigure server
+     * \param config The config set from dynamic_reconfigure server
+     * \param level not used at this time.
+     * \see dyn_reconf_server_
+     */
+    void reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/);
+
+    /**
+     * \brief Update the dynamic parameters in the RT loop
+     */
+    void updateDynamicParams();
+
+    /**
+     * \brief
+     * \param time Current time
+     * \param period Time since the last called to update
+     * \param curr_cmd Current velocity command
+     * \param wheel_separation wheel separation with multiplier
+     * \param left_wheel_radius left wheel radius with multiplier
+     * \param right_wheel_radius right wheel radius with multiplier
+     */
+    void publishWheelData(const ros::Time& time,
+                          const ros::Duration& period,
+                          Commands& curr_cmd,
+                          double wheel_separation,
+                          double left_wheel_radius,
+                          double right_wheel_radius);
+  };
+
+} // namespace diff_drive_controller

mir_robot/diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -0,0 +1,210 @@
+/*********************************************************************
+ * 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
+ */
+
+#pragma once
+
+
+#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 diff_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 left_pos  Left  wheel position [rad]
+     * \param right_pos Right wheel position [rad]
+     * \param time      Current time
+     * \return true if the odometry is actually updated
+     */
+    bool update(double left_pos, double right_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   Separation between left and right wheels [m]
+     * \param left_wheel_radius  Left wheel radius [m]
+     * \param right_wheel_radius Right wheel radius [m]
+     */
+    void setWheelParams(double wheel_separation, double left_wheel_radius, double right_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_;
+    double left_wheel_radius_;
+    double right_wheel_radius_;
+
+    /// Previou wheel position/state [rad]:
+    double left_wheel_old_pos_;
+    double right_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_;
+  };
+}

mir_robot/diff_drive_controller/include/diff_drive_controller/speed_limiter.h

@@ -0,0 +1,129 @@
+/*********************************************************************
+ * 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
+ */
+
+#pragma once
+
+
+namespace diff_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

mir_robot/diff_drive_controller/package.xml

@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<?xml-model
+  href="http://download.ros.org/schema/package_format3.xsd"
+  schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>diff_drive_controller</name>
+  <version>0.21.2</version>
+  <description>Controller for a differential drive mobile base.</description>
+
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+  <maintainer email="enrique.fernandez.perdomo@gmail.com">Enrique Fernandez</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/ros-controls/ros_controllers/wiki</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros_controllers</url>
+
+  <author email="bence.magyar@pal-robotics.com">Bence Magyar</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>control_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>geometry_msgs</depend>
+  <depend>hardware_interface</depend>
+  <depend>nav_msgs</depend>
+  <depend>realtime_tools</depend>
+  <depend>tf</depend>
+
+  <build_depend>boost</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>urdf</build_depend>
+
+  <build_export_depend>boost</build_export_depend>
+
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>urdf</exec_depend>
+
+  <test_depend>controller_manager</test_depend>
+  <test_depend>rosgraph_msgs</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>rostopic</test_depend>
+  <test_depend>std_srvs</test_depend>
+  <test_depend>xacro</test_depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/diff_drive_controller_plugins.xml"/>
+  </export>
+</package>

mir_robot/diff_drive_controller/src/diff_drive_controller.cpp

@@ -0,0 +1,837 @@
+/*********************************************************************
+ * 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, Enrique Fernández
+ */
+
+#include <cmath>
+#include <diff_drive_controller/diff_drive_controller.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <tf/transform_datatypes.h>
+#include <urdf/urdfdom_compatibility.h>
+#include <urdf_parser/urdf_parser.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 that a link exists and has a geometry collision.
+* \param link The link
+* \return true if the link has a collision element with geometry
+*/
+static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr& link)
+{
+  if (!link)
+  {
+    ROS_ERROR("Link pointer is 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;
+  }
+  return true;
+}
+
+/*
+ * \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 (!hasCollisionGeometry(link))
+  {
+    return false;
+  }
+
+  if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
+  {
+    ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry");
+    return false;
+  }
+
+  return true;
+}
+
+/*
+ * \brief Check if the link is modeled as a sphere
+ * \param link Link
+ * \return true if the link is modeled as a Sphere; false otherwise
+ */
+static bool isSphere(const urdf::LinkConstSharedPtr& link)
+{
+  if (!hasCollisionGeometry(link))
+  {
+    return false;
+  }
+
+  if (link->collision->geometry->type != urdf::Geometry::SPHERE)
+  {
+    ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere 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 otherwise
+ */
+static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
+{
+  if (isCylinder(wheel_link))
+  {
+    wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
+    return true;
+  }
+  else if (isSphere(wheel_link))
+  {
+    wheel_radius = (static_cast<urdf::Sphere*>(wheel_link->collision->geometry.get()))->radius;
+    return true;
+  }
+
+  ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!");
+  return false;
+}
+
+namespace diff_drive_controller{
+
+  DiffDriveController::DiffDriveController()
+    : open_loop_(false)
+    , command_struct_()
+    , wheel_separation_(0.0)
+    , wheel_radius_(0.0)
+    , wheel_separation_multiplier_(1.0)
+    , left_wheel_radius_multiplier_(1.0)
+    , right_wheel_radius_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)
+    , publish_cmd_(false)
+    , publish_wheel_joint_controller_state_(false)
+  {
+  }
+
+  bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw,
+            ros::NodeHandle& root_nh,
+            ros::NodeHandle &controller_nh)
+  {
+    const std::string complete_ns = controller_nh.getNamespace();
+    std::size_t id = complete_ns.find_last_of("/");
+    name_ = complete_ns.substr(id + 1);
+
+    // Get joint names from the parameter server
+    std::vector<std::string> left_wheel_names, right_wheel_names;
+    if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) ||
+        !getWheelNames(controller_nh, "right_wheel", right_wheel_names))
+    {
+      return false;
+    }
+
+    if (left_wheel_names.size() != right_wheel_names.size())
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+          "#left wheels (" << left_wheel_names.size() << ") != " <<
+          "#right wheels (" << right_wheel_names.size() << ").");
+      return false;
+    }
+    else
+    {
+      wheel_joints_size_ = left_wheel_names.size();
+
+      left_wheel_joints_.resize(wheel_joints_size_);
+      right_wheel_joints_.resize(wheel_joints_size_);
+    }
+
+    // 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("open_loop", open_loop_, open_loop_);
+
+    controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
+    ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by "
+                          << wheel_separation_multiplier_ << ".");
+
+    if (controller_nh.hasParam("wheel_radius_multiplier"))
+    {
+      double wheel_radius_multiplier;
+      controller_nh.getParam("wheel_radius_multiplier", wheel_radius_multiplier);
+
+      left_wheel_radius_multiplier_  = wheel_radius_multiplier;
+      right_wheel_radius_multiplier_ = wheel_radius_multiplier;
+    }
+    else
+    {
+      controller_nh.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
+      controller_nh.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
+    }
+
+    ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by "
+                          << left_wheel_radius_multiplier_ << ".");
+    ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by "
+                          << right_wheel_radius_multiplier_ << ".");
+
+    int velocity_rolling_window_size = 10;
+    controller_nh.param("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("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("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("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("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("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("linear/x/has_velocity_limits"    , limiter_lin_.has_velocity_limits    , limiter_lin_.has_velocity_limits    );
+    controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
+    controller_nh.param("linear/x/has_jerk_limits"        , limiter_lin_.has_jerk_limits        , limiter_lin_.has_jerk_limits        );
+    controller_nh.param("linear/x/max_velocity"           , limiter_lin_.max_velocity           ,  limiter_lin_.max_velocity          );
+    controller_nh.param("linear/x/min_velocity"           , limiter_lin_.min_velocity           , -limiter_lin_.max_velocity          );
+    controller_nh.param("linear/x/max_acceleration"       , limiter_lin_.max_acceleration       ,  limiter_lin_.max_acceleration      );
+    controller_nh.param("linear/x/min_acceleration"       , limiter_lin_.min_acceleration       , -limiter_lin_.max_acceleration      );
+    controller_nh.param("linear/x/max_jerk"               , limiter_lin_.max_jerk               ,  limiter_lin_.max_jerk              );
+    controller_nh.param("linear/x/min_jerk"               , limiter_lin_.min_jerk               , -limiter_lin_.max_jerk              );
+
+    controller_nh.param("angular/z/has_velocity_limits"    , limiter_ang_.has_velocity_limits    , limiter_ang_.has_velocity_limits    );
+    controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
+    controller_nh.param("angular/z/has_jerk_limits"        , limiter_ang_.has_jerk_limits        , limiter_ang_.has_jerk_limits        );
+    controller_nh.param("angular/z/max_velocity"           , limiter_ang_.max_velocity           ,  limiter_ang_.max_velocity          );
+    controller_nh.param("angular/z/min_velocity"           , limiter_ang_.min_velocity           , -limiter_ang_.max_velocity          );
+    controller_nh.param("angular/z/max_acceleration"       , limiter_ang_.max_acceleration       ,  limiter_ang_.max_acceleration      );
+    controller_nh.param("angular/z/min_acceleration"       , limiter_ang_.min_acceleration       , -limiter_ang_.max_acceleration      );
+    controller_nh.param("angular/z/max_jerk"               , limiter_ang_.max_jerk               ,  limiter_ang_.max_jerk              );
+    controller_nh.param("angular/z/min_jerk"               , limiter_ang_.min_jerk               , -limiter_ang_.max_jerk              );
+
+    // Publish limited velocity:
+    controller_nh.param("publish_cmd", publish_cmd_, publish_cmd_);
+
+    // Publish wheel data:
+    controller_nh.param("publish_wheel_joint_controller_state", publish_wheel_joint_controller_state_, publish_wheel_joint_controller_state_);
+
+    // If either parameter is not available, we need to look up the value in the URDF
+    bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_);
+    bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
+
+    if (!setOdomParamsFromUrdf(root_nh,
+                              left_wheel_names[0],
+                              right_wheel_names[0],
+                              lookup_wheel_separation,
+                              lookup_wheel_radius))
+    {
+      return false;
+    }
+
+    // Regardless of how we got the separation and radius, use them
+    // to set the odometry parameters
+    const double ws  = wheel_separation_multiplier_   * wheel_separation_;
+    const double lwr = left_wheel_radius_multiplier_  * wheel_radius_;
+    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+    odometry_.setWheelParams(ws, lwr, rwr);
+    ROS_INFO_STREAM_NAMED(name_,
+                          "Odometry params : wheel separation " << ws
+                          << ", left wheel radius "  << lwr
+                          << ", right wheel radius " << rwr);
+
+    if (publish_cmd_)
+    {
+      cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(controller_nh, "cmd_vel_out", 100));
+    }
+
+    // Wheel joint controller state:
+    if (publish_wheel_joint_controller_state_)
+    {
+      controller_state_pub_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState>(controller_nh, "wheel_joint_controller_state", 100));
+
+      const size_t num_wheels = wheel_joints_size_ * 2;
+
+      controller_state_pub_->msg_.joint_names.resize(num_wheels);
+
+      controller_state_pub_->msg_.desired.positions.resize(num_wheels);
+      controller_state_pub_->msg_.desired.velocities.resize(num_wheels);
+      controller_state_pub_->msg_.desired.accelerations.resize(num_wheels);
+      controller_state_pub_->msg_.desired.effort.resize(num_wheels);
+
+      controller_state_pub_->msg_.actual.positions.resize(num_wheels);
+      controller_state_pub_->msg_.actual.velocities.resize(num_wheels);
+      controller_state_pub_->msg_.actual.accelerations.resize(num_wheels);
+      controller_state_pub_->msg_.actual.effort.resize(num_wheels);
+
+      controller_state_pub_->msg_.error.positions.resize(num_wheels);
+      controller_state_pub_->msg_.error.velocities.resize(num_wheels);
+      controller_state_pub_->msg_.error.accelerations.resize(num_wheels);
+      controller_state_pub_->msg_.error.effort.resize(num_wheels);
+
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
+      {
+        controller_state_pub_->msg_.joint_names[i] = left_wheel_names[i];
+        controller_state_pub_->msg_.joint_names[i + wheel_joints_size_] = right_wheel_names[i];
+      }
+
+      vel_left_previous_.resize(wheel_joints_size_, 0.0);
+      vel_right_previous_.resize(wheel_joints_size_, 0.0);
+    }
+
+    setOdomPubFields(root_nh, controller_nh);
+
+    // Get the joint object to use in the realtime loop
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      ROS_INFO_STREAM_NAMED(name_,
+                            "Adding left wheel with joint name: " << left_wheel_names[i]
+                            << " and right wheel with joint name: " << right_wheel_names[i]);
+      left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]);  // throws on failure
+      right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]);  // throws on failure
+    }
+
+    sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
+
+    // Initialize dynamic parameters
+    DynamicParams dynamic_params;
+    dynamic_params.left_wheel_radius_multiplier  = left_wheel_radius_multiplier_;
+    dynamic_params.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
+    dynamic_params.wheel_separation_multiplier   = wheel_separation_multiplier_;
+
+    dynamic_params.publish_rate = publish_rate;
+    dynamic_params.enable_odom_tf = enable_odom_tf_;
+
+    dynamic_params_.writeFromNonRT(dynamic_params);
+
+    // Initialize dynamic_reconfigure server
+    DiffDriveControllerConfig config;
+    config.left_wheel_radius_multiplier  = left_wheel_radius_multiplier_;
+    config.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
+    config.wheel_separation_multiplier   = wheel_separation_multiplier_;
+
+    config.publish_rate = publish_rate;
+    config.enable_odom_tf = enable_odom_tf_;
+
+    dyn_reconf_server_ = std::make_shared<ReconfigureServer>(dyn_reconf_server_mutex_, controller_nh);
+
+    // Update parameters
+    dyn_reconf_server_mutex_.lock();
+    dyn_reconf_server_->updateConfig(config);
+    dyn_reconf_server_mutex_.unlock();
+
+    dyn_reconf_server_->setCallback(
+        std::bind(&DiffDriveController::reconfCallback, this, std::placeholders::_1, std::placeholders::_2));
+    return true;
+  }
+
+  void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
+  {
+    // update parameter from dynamic reconf
+    updateDynamicParams();
+
+    // Apply (possibly new) multipliers:
+    const double ws  = wheel_separation_multiplier_   * wheel_separation_;
+    const double lwr = left_wheel_radius_multiplier_  * wheel_radius_;
+    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+
+    odometry_.setWheelParams(ws, lwr, rwr);
+
+    // COMPUTE AND PUBLISH ODOMETRY
+    if (open_loop_)
+    {
+      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
+    }
+    else
+    {
+      double left_pos  = 0.0;
+      double right_pos = 0.0;
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
+      {
+        const double lp = left_wheel_joints_[i].getPosition();
+        const double rp = right_wheel_joints_[i].getPosition();
+        if (std::isnan(lp) || std::isnan(rp))
+          return;
+
+        left_pos  += lp;
+        right_pos += rp;
+      }
+      left_pos  /= wheel_joints_size_;
+      right_pos /= wheel_joints_size_;
+
+      // Estimate linear and angular velocity using joint information
+      odometry_.update(left_pos, right_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());
+
+    limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
+    limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
+
+    last1_cmd_ = last0_cmd_;
+    last0_cmd_ = curr_cmd;
+
+    // Publish limited velocity:
+    if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
+    {
+      cmd_vel_pub_->msg_.header.stamp = time;
+      cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.lin;
+      cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.ang;
+      cmd_vel_pub_->unlockAndPublish();
+    }
+
+    // Compute wheels velocities:
+    const double vel_left  = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/lwr;
+    const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/rwr;
+
+    // Set wheels velocities:
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      left_wheel_joints_[i].setCommand(vel_left);
+      right_wheel_joints_[i].setCommand(vel_right);
+    }
+
+    publishWheelData(time, period, curr_cmd, ws, lwr, rwr);
+    time_previous_ = time;
+  }
+
+  void DiffDriveController::starting(const ros::Time& time)
+  {
+    brake();
+
+    // Register starting time used to keep fixed rate
+    last_state_publish_time_ = time;
+    time_previous_ = time;
+
+    odometry_.init(time);
+  }
+
+  void DiffDriveController::stopping(const ros::Time& /*time*/)
+  {
+    brake();
+  }
+
+  void DiffDriveController::brake()
+  {
+    const double vel = 0.0;
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      left_wheel_joints_[i].setCommand(vel);
+      right_wheel_joints_[i].setCommand(vel);
+    }
+  }
+
+  void DiffDriveController::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;
+      }
+
+      if(!std::isfinite(command.angular.z) || !std::isfinite(command.linear.x))
+      {
+        ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring.");
+        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 DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh,
+                              const std::string& wheel_param,
+                              std::vector<std::string>& wheel_names)
+  {
+      XmlRpc::XmlRpcValue wheel_list;
+      if (!controller_nh.getParam(wheel_param, wheel_list))
+      {
+        ROS_ERROR_STREAM_NAMED(name_,
+            "Couldn't retrieve wheel param '" << wheel_param << "'.");
+        return false;
+      }
+
+      if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
+      {
+        if (wheel_list.size() == 0)
+        {
+          ROS_ERROR_STREAM_NAMED(name_,
+              "Wheel param '" << wheel_param << "' is an empty list");
+          return false;
+        }
+
+        for (int i = 0; i < wheel_list.size(); ++i)
+        {
+          if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
+          {
+            ROS_ERROR_STREAM_NAMED(name_,
+                "Wheel param '" << wheel_param << "' #" << i <<
+                " isn't a string.");
+            return false;
+          }
+        }
+
+        wheel_names.resize(wheel_list.size());
+        for (int i = 0; i < wheel_list.size(); ++i)
+        {
+          wheel_names[i] = static_cast<std::string>(wheel_list[i]);
+        }
+      }
+      else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
+      {
+        wheel_names.push_back(wheel_list);
+      }
+      else
+      {
+        ROS_ERROR_STREAM_NAMED(name_,
+            "Wheel param '" << wheel_param <<
+            "' is neither a list of strings nor a string.");
+        return false;
+      }
+
+      return true;
+  }
+
+  bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
+                             const std::string& left_wheel_name,
+                             const std::string& right_wheel_name,
+                             bool lookup_wheel_separation,
+                             bool lookup_wheel_radius)
+  {
+    if (!(lookup_wheel_separation || 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 description couldn't be retrieved from param server.");
+      return false;
+    }
+
+    urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str));
+
+    urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name));
+    urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name));
+
+    if (!left_wheel_joint)
+    {
+      ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
+                             << " couldn't be retrieved from model description");
+      return false;
+    }
+
+    if (!right_wheel_joint)
+    {
+      ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
+                             << " couldn't be retrieved from model description");
+      return false;
+    }
+
+    if (lookup_wheel_separation)
+    {
+      // Get wheel separation
+      ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
+                      << left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
+                      << left_wheel_joint->parent_to_joint_origin_transform.position.z);
+      ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
+                      << right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
+                      << right_wheel_joint->parent_to_joint_origin_transform.position.z);
+
+      wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
+                                             right_wheel_joint->parent_to_joint_origin_transform.position);
+
+    }
+
+    if (lookup_wheel_radius)
+    {
+      // Get wheel radius
+      if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  void DiffDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
+  {
+    // Get and check params for covariances
+    XmlRpc::XmlRpcValue pose_cov_list;
+    controller_nh.getParam("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("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, "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 = {
+        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 = {
+        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_;
+  }
+
+  void DiffDriveController::reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/)
+  {
+    DynamicParams dynamic_params;
+    dynamic_params.left_wheel_radius_multiplier  = config.left_wheel_radius_multiplier;
+    dynamic_params.right_wheel_radius_multiplier = config.right_wheel_radius_multiplier;
+    dynamic_params.wheel_separation_multiplier   = config.wheel_separation_multiplier;
+
+    dynamic_params.publish_rate = config.publish_rate;
+
+    dynamic_params.enable_odom_tf = config.enable_odom_tf;
+
+    dynamic_params_.writeFromNonRT(dynamic_params);
+
+    ROS_INFO_STREAM_NAMED(name_, "Dynamic Reconfigure:\n" << dynamic_params);
+  }
+
+  void DiffDriveController::updateDynamicParams()
+  {
+    // Retreive dynamic params:
+    const DynamicParams dynamic_params = *(dynamic_params_.readFromRT());
+
+    left_wheel_radius_multiplier_  = dynamic_params.left_wheel_radius_multiplier;
+    right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier;
+    wheel_separation_multiplier_   = dynamic_params.wheel_separation_multiplier;
+
+    publish_period_ = ros::Duration(1.0 / dynamic_params.publish_rate);
+    enable_odom_tf_ = dynamic_params.enable_odom_tf;
+  }
+
+  void DiffDriveController::publishWheelData(const ros::Time& time, const ros::Duration& period, Commands& curr_cmd,
+          double wheel_separation, double left_wheel_radius, double right_wheel_radius)
+  {
+    if (publish_wheel_joint_controller_state_ && controller_state_pub_->trylock())
+    {
+      const double cmd_dt(period.toSec());
+
+      // Compute desired wheels velocities, that is before applying limits:
+      const double vel_left_desired  = (curr_cmd.lin - curr_cmd.ang * wheel_separation / 2.0) / left_wheel_radius;
+      const double vel_right_desired = (curr_cmd.lin + curr_cmd.ang * wheel_separation / 2.0) / right_wheel_radius;
+      controller_state_pub_->msg_.header.stamp = time;
+
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
+      {
+        const double control_duration = (time - time_previous_).toSec();
+
+        const double left_wheel_acc = (left_wheel_joints_[i].getVelocity() - vel_left_previous_[i]) / control_duration;
+        const double right_wheel_acc = (right_wheel_joints_[i].getVelocity() - vel_right_previous_[i]) / control_duration;
+
+        // Actual
+        controller_state_pub_->msg_.actual.positions[i]     = left_wheel_joints_[i].getPosition();
+        controller_state_pub_->msg_.actual.velocities[i]    = left_wheel_joints_[i].getVelocity();
+        controller_state_pub_->msg_.actual.accelerations[i] = left_wheel_acc;
+        controller_state_pub_->msg_.actual.effort[i]        = left_wheel_joints_[i].getEffort();
+
+        controller_state_pub_->msg_.actual.positions[i + wheel_joints_size_]     = right_wheel_joints_[i].getPosition();
+        controller_state_pub_->msg_.actual.velocities[i + wheel_joints_size_]    = right_wheel_joints_[i].getVelocity();
+        controller_state_pub_->msg_.actual.accelerations[i + wheel_joints_size_] = right_wheel_acc;
+        controller_state_pub_->msg_.actual.effort[i+ wheel_joints_size_]         = right_wheel_joints_[i].getEffort();
+
+        // Desired
+        controller_state_pub_->msg_.desired.positions[i]    += vel_left_desired * cmd_dt;
+        controller_state_pub_->msg_.desired.velocities[i]    = vel_left_desired;
+        controller_state_pub_->msg_.desired.accelerations[i] = (vel_left_desired - vel_left_desired_previous_) * cmd_dt;
+        controller_state_pub_->msg_.desired.effort[i]        = std::numeric_limits<double>::quiet_NaN();
+
+        controller_state_pub_->msg_.desired.positions[i + wheel_joints_size_]    += vel_right_desired * cmd_dt;
+        controller_state_pub_->msg_.desired.velocities[i + wheel_joints_size_]    = vel_right_desired;
+        controller_state_pub_->msg_.desired.accelerations[i + wheel_joints_size_] = (vel_right_desired - vel_right_desired_previous_) * cmd_dt;
+        controller_state_pub_->msg_.desired.effort[i+ wheel_joints_size_]         = std::numeric_limits<double>::quiet_NaN();
+
+        // Error
+        controller_state_pub_->msg_.error.positions[i]     = controller_state_pub_->msg_.desired.positions[i] -
+                                                                              controller_state_pub_->msg_.actual.positions[i];
+        controller_state_pub_->msg_.error.velocities[i]    = controller_state_pub_->msg_.desired.velocities[i] -
+                                                                              controller_state_pub_->msg_.actual.velocities[i];
+        controller_state_pub_->msg_.error.accelerations[i] = controller_state_pub_->msg_.desired.accelerations[i] -
+                                                                              controller_state_pub_->msg_.actual.accelerations[i];
+        controller_state_pub_->msg_.error.effort[i]        = controller_state_pub_->msg_.desired.effort[i] -
+                                                                              controller_state_pub_->msg_.actual.effort[i];
+
+        controller_state_pub_->msg_.error.positions[i + wheel_joints_size_]     = controller_state_pub_->msg_.desired.positions[i + wheel_joints_size_] -
+                                                                                                   controller_state_pub_->msg_.actual.positions[i + wheel_joints_size_];
+        controller_state_pub_->msg_.error.velocities[i + wheel_joints_size_]    = controller_state_pub_->msg_.desired.velocities[i + wheel_joints_size_] -
+                                                                                                   controller_state_pub_->msg_.actual.velocities[i + wheel_joints_size_];
+        controller_state_pub_->msg_.error.accelerations[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.accelerations[i + wheel_joints_size_] -
+                                                                                                   controller_state_pub_->msg_.actual.accelerations[i + wheel_joints_size_];
+        controller_state_pub_->msg_.error.effort[i+ wheel_joints_size_]         = controller_state_pub_->msg_.desired.effort[i + wheel_joints_size_] -
+                                                                                                   controller_state_pub_->msg_.actual.effort[i + wheel_joints_size_];
+
+        // Save previous velocities to compute acceleration
+        vel_left_previous_[i] = left_wheel_joints_[i].getVelocity();
+        vel_right_previous_[i] = right_wheel_joints_[i].getVelocity();
+        vel_left_desired_previous_ = vel_left_desired;
+        vel_right_desired_previous_ = vel_right_desired;
+      }
+
+      controller_state_pub_->unlockAndPublish();
+    }
+  }
+
+} // namespace diff_drive_controller
+
+PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase);

mir_robot/diff_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 <diff_drive_controller/odometry.h>
+
+namespace diff_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_(0.0)
+  , left_wheel_radius_(0.0)
+  , right_wheel_radius_(0.0)
+  , left_wheel_old_pos_(0.0)
+  , right_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_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2))
+  {
+  }
+
+  void Odometry::init(const ros::Time& time)
+  {
+    // Reset accumulators and timestamp:
+    resetAccumulators();
+    timestamp_ = time;
+  }
+
+  bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
+  {
+    /// Get current wheel joint positions:
+    const double left_wheel_cur_pos  = left_pos  * left_wheel_radius_;
+    const double right_wheel_cur_pos = right_pos * right_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_;
+
+    /// Update old position with current:
+    left_wheel_old_pos_  = left_wheel_cur_pos;
+    right_wheel_old_pos_ = right_wheel_cur_pos;
+
+    /// Compute linear and angular diff:
+    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
+    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
+
+    /// 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, double left_wheel_radius, double right_wheel_radius)
+  {
+    wheel_separation_   = wheel_separation;
+    left_wheel_radius_  = left_wheel_radius;
+    right_wheel_radius_ = right_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

mir_robot/diff_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 <diff_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 diff_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

mir_robot/diff_drive_controller/test/diff_drive_bad_urdf.test

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override robot_description with bad urdf -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_bad.xacro'" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_fail_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_common.launch

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Use simulation time -->
+  <rosparam param="use_sim_time">True</rosparam>
+
+  <!-- Load diffbot model -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro'" />
+
+  <!-- Start diffbot -->
+  <node name="diffbot"
+      pkg="diff_drive_controller"
+      type="diffbot"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_controllers.yaml" />
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="diffbot_controller" />
+
+  <!-- rqt_plot monitoring -->
+  <!--
+  <node name="diffbot_pos_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/diffbot_controller/odom/pose/pose/position/x" />
+
+  <node name="diffbot_vel_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/diffbot_controller/odom/twist/twist/linear/x" />
+  -->
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_controller.test

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_controller_limits.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Load diff-drive limits -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_limits.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_limits_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_limits_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_controller_nan.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <rosparam>
+    diffbot_controller:
+      publish_cmd: True
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_nan_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_nan_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+    <remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_default_cmd_vel_out.test

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- default value -->
+  <!-- <rosparam>
+    diffbot_controller:
+      publish_cmd: False
+  </rosparam> -->
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_default_cmd_vel_out_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_default_cmd_vel_out_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+    <remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_default_cmd_vel_out_test.cpp

@@ -0,0 +1,69 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2017, PAL Robotics.
+//
+// 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 Jeremie Deray
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testDefaultCmdVelOutTopic)
+{
+  // wait for ROS
+  waitForController();
+  // msgs are published in the same loop
+  // thus if odom is published cmd_vel_out
+  // should be as well (if enabled)
+  waitForOdomMsgs();
+
+  EXPECT_FALSE(isPublishingCmdVelOut());
+
+  // 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(0.1).sleep();
+
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  ros::Duration(0.1).sleep();
+
+  EXPECT_FALSE(isPublishingCmdVelOut());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_default_cmd_vel_out_topic_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_default_odom_frame.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Do not specific an odom_frame_id -->
+  <!-- <rosparam>
+    diffbot_controller:
+      odom_frame_id: odom
+  </rosparam> -->
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_default_odom_frame_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_default_odom_frame_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp

@@ -0,0 +1,69 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testOdomFrame)
+{
+  // wait for ROS
+  waitForController();
+
+  // 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(DiffDriveControllerTest, testOdomTopic)
+{
+  // wait for ROS
+  waitForController();
+  waitForOdomMsgs();
+
+  // 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, "diff_drive_default_odom_frame_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state.test

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- default value -->
+  <!-- <rosparam>
+    diffbot_controller:
+      publish_wheel_joint_controller_state: False
+  </rosparam> -->
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_default_wheel_joint_controller_state_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_default_wheel_joint_controller_state_test"
+        time-limit="30.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+    <remap from="wheel_joint_controller_state" to="diffbot_controller/wheel_joint_controller_state" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state_test.cpp

@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2018, PAL Robotics.
+//
+// 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 Jeremie Deray
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testDefaultJointTrajectoryControllerStateTopic)
+{
+  // wait for ROS
+  waitForController();
+
+  EXPECT_FALSE(isPublishingJointTrajectoryControllerState());
+
+  // 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(0.1).sleep();
+
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  ros::Duration(0.1).sleep();
+
+  EXPECT_FALSE(isPublishingJointTrajectoryControllerState());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_default_wheel_joint_controller_state_topic_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_dyn_reconf.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Make sure to disable odom tf -->
+  <rosparam>
+    diffbot_controller:
+      enable_odom_tf: False
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_dyn_reconf_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_dyn_reconf_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_dyn_reconf_test.cpp

@@ -0,0 +1,176 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2018, 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 Jeremie Deray
+
+#include "test_common.h"
+#include <tf/transform_listener.h>
+
+#include <dynamic_reconfigure/server.h>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testDynReconfServerAlive)
+{
+  // wait for ROS
+  waitForController();
+
+  // Expect server is alive
+  EXPECT_TRUE(ros::service::exists("diffbot_controller/set_parameters", true));
+
+  dynamic_reconfigure::ReconfigureRequest  srv_req;
+  dynamic_reconfigure::ReconfigureResponse srv_resp;
+
+  // Expect server is callable (get-fashion)
+  EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
+
+  EXPECT_EQ(1, srv_resp.config.bools.size());
+
+  if (!srv_resp.config.bools.empty())
+  {
+    EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
+    // expect false since it is set to false in the .test
+    EXPECT_EQ(false, srv_resp.config.bools[0].value);
+  }
+
+  EXPECT_EQ(4, srv_resp.config.doubles.size());
+
+  if (srv_resp.config.doubles.size() >= 4)
+  {
+    EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
+    EXPECT_EQ(1, srv_resp.config.doubles[0].value);
+
+    EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
+    EXPECT_EQ(1, srv_resp.config.doubles[1].value);
+
+    EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
+    EXPECT_EQ(1, srv_resp.config.doubles[2].value);
+
+    EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
+    EXPECT_EQ(50, srv_resp.config.doubles[3].value);
+  }
+
+  dynamic_reconfigure::DoubleParameter double_param;
+  double_param.name = "left_wheel_radius_multiplier";
+  double_param.value = 0.95;
+
+  srv_req.config.doubles.push_back(double_param);
+
+  double_param.name = "right_wheel_radius_multiplier";
+  double_param.value = 0.95;
+
+  srv_req.config.doubles.push_back(double_param);
+
+  double_param.name = "wheel_separation_multiplier";
+  double_param.value = 0.95;
+
+  srv_req.config.doubles.push_back(double_param);
+
+  double_param.name = "publish_rate";
+  double_param.value = 150;
+
+  srv_req.config.doubles.push_back(double_param);
+
+  dynamic_reconfigure::BoolParameter bool_param;
+  bool_param.name = "enable_odom_tf";
+  bool_param.value = false;
+
+  srv_req.config.bools.push_back(bool_param);
+
+  // Expect server is callable (set-fashion)
+  EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
+
+  EXPECT_EQ(1, srv_resp.config.bools.size());
+
+  if (!srv_resp.config.bools.empty())
+  {
+    EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
+    EXPECT_EQ(false, srv_resp.config.bools[0].value);
+  }
+
+  EXPECT_EQ(4, srv_resp.config.doubles.size());
+
+  if (srv_resp.config.doubles.size() >= 4)
+  {
+    EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
+    EXPECT_EQ(0.95, srv_resp.config.doubles[0].value);
+
+    EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
+    EXPECT_EQ(0.95, srv_resp.config.doubles[1].value);
+
+    EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
+    EXPECT_EQ(0.95, srv_resp.config.doubles[2].value);
+
+    EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
+    EXPECT_EQ(150, srv_resp.config.doubles[3].value);
+  }
+}
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testDynReconfEnableTf)
+{
+  // wait for ROS
+  while(!isControllerAlive() && ros::ok())
+  {
+    ros::Duration(0.1).sleep();
+  }
+  if (!ros::ok())
+    FAIL() << "Something went wrong while executing test";
+
+  // set up tf listener
+  tf::TransformListener listener;
+  ros::Duration(2.0).sleep();
+  // check the odom frame doesn't exist
+  EXPECT_FALSE(listener.frameExists("odom"));
+
+  dynamic_reconfigure::ReconfigureRequest  srv_req;
+  dynamic_reconfigure::ReconfigureResponse srv_resp;
+
+  dynamic_reconfigure::BoolParameter bool_param;
+  bool_param.name = "enable_odom_tf";
+  bool_param.value = true;
+
+  srv_req.config.bools.push_back(bool_param);
+
+  EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
+
+  ros::Duration(2.0).sleep();
+  // check the odom frame doesn't exist
+  EXPECT_TRUE(listener.frameExists("odom"));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_dyn_reconf_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_fail_test.cpp

@@ -0,0 +1,60 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testWrongJointName)
+{
+  // the controller should never be alive
+  int secs = 0;
+  while(!isControllerAlive() && ros::ok() && secs < 5)
+  {
+    ros::Duration(1.0).sleep();
+    secs++;
+  }
+  if (!ros::ok())
+    FAIL() << "Something went wrong while executing test.";
+
+  // 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, "diff_drive_fail_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_get_wheel_radius_fail.test

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override with wrong controller configuration -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_wrong.yaml" />
+  <param name="diffbot_controller/wheel_separation" value="0.08"/>
+
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_fail_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_left_right_multipliers.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Load diff drive parameter multipliers -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_left_right_multipliers.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_limits_test.cpp

@@ -0,0 +1,231 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testLinearJerkLimits)
+{
+  // wait for ROS
+  waitForController();
+
+  // 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(DiffDriveControllerTest, 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(DiffDriveControllerTest, 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);
+}
+
+/* This test has been failing on Travis for a long time due to timing issues however it works well when ran manually
+TEST_F(DiffDriveControllerTest, 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;
+  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.7rad.s-1
+  EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.7, JERK_ANGULAR_VELOCITY_TOLERANCE);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
+
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+}
+*/
+
+TEST_F(DiffDriveControllerTest, 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;
+  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 1.0rad.s-1, which is 2.0rad.s-2 * 0.5s
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 1.0 + VELOCITY_TOLERANCE);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
+
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+}
+
+TEST_F(DiffDriveControllerTest, 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();
+  // send a big command
+  cmd_vel.angular.z = 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 2.0rad.s-1, the limit
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 2.0 + VELOCITY_TOLERANCE);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
+
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_limits_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  //ros::Duration(0.5).sleep();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers.test

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override robot_description with sphere wheel urdf -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
+  <!-- Set allow_multiple_publishers to false -->
+  <param name="diffbot_controller/allow_multiple_cmd_vel_publishers" value="false"/>
+
+  <node name="cmd_vel_publisher" pkg="rostopic" type="rostopic" args="pub -r 2 /diffbot_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0}}'" />
+
+  <node name="cmd_vel_publisher2" pkg="rostopic" type="rostopic" args="pub -r 2 /diffbot_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 2.0}}'" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_allow_multiple_cmd_vel_publishers_param_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_multiple_cmd_vel_publishers_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers_test.cpp

@@ -0,0 +1,72 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 Bence Magyar
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, breakWithMultiplePublishers)
+{
+  // wait for ROS
+  waitForController();
+  waitForOdomMsgs();
+
+  nav_msgs::Odometry old_odom = getLastOdom();
+  //TODO: we should be programatically publish from 2 different nodes
+  // not the current hacky solution with the launch files
+  ros::Duration(1.0).sleep(); 
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
+  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
+  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
+  EXPECT_NEAR(sqrt(dx*dx + dy*dy), 0.0, POSITION_TOLERANCE);
+  EXPECT_LT(fabs(dz), EPS);
+
+  // convert to rpy and test that way
+  double roll_old, pitch_old, yaw_old;
+  double roll_new, pitch_new, yaw_new;
+  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
+  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
+  EXPECT_LT(fabs(roll_new - roll_old), EPS);
+  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
+  EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_multiple_publishers_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_multipliers.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Load diff drive parameter multipliers -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_multipliers.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_nan_test.cpp

@@ -0,0 +1,131 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+
+// NaN
+#include <limits>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testNaN) {
+  // wait for ROS
+  waitForController();
+
+  // 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_FALSE(std::isnan(odom.twist.twist.linear.x));
+  EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.position.x));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.position.y));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w));
+
+  // start robot
+  start();
+  ros::Duration(2.0).sleep();
+
+  odom = getLastOdom();
+
+  EXPECT_FALSE(std::isnan(odom.twist.twist.linear.x));
+  EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.position.x));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.position.y));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z));
+  EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w));
+}
+
+TEST_F(DiffDriveControllerTest, testNaNCmd) {
+  // 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(0.1).sleep();
+
+  // send NaN
+  for (int i = 0; i < 10; ++i) {
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = NAN;
+    cmd_vel.angular.z = 0.0;
+    publish(cmd_vel);
+    geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
+    EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x));
+    EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z));
+    ros::Duration(0.1).sleep();
+  }
+  for (int i = 0; i < 10; ++i) {
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = 0.0;
+    cmd_vel.angular.z = NAN;
+    publish(cmd_vel);
+    geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
+    EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x));
+    EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z));
+    ros::Duration(0.1).sleep();
+  }
+
+  nav_msgs::Odometry odom = getLastOdom();
+  EXPECT_DOUBLE_EQ(odom.twist.twist.linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(odom.pose.pose.position.x, 0.0);
+  EXPECT_DOUBLE_EQ(odom.pose.pose.position.y, 0.0);
+
+  geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
+  EXPECT_DOUBLE_EQ(odom_msg.twist.linear.x, 0.0);
+}
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_nan_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_odom_frame.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Set odom_frame_id to something new -->
+  <rosparam>
+    diffbot_controller:
+      odom_frame_id: new_odom
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_odom_frame_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_odom_frame_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_odom_frame_test.cpp

@@ -0,0 +1,83 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testNoOdomFrame)
+{
+  // wait for ROS
+  waitForController();
+
+  // 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(DiffDriveControllerTest, testNewOdomFrame)
+{
+  // wait for ROS
+  waitForController();
+
+  // 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(DiffDriveControllerTest, testOdomTopic)
+{
+  // wait for ROS
+  waitForController();
+
+  // wait until we received first odom msg
+  waitForOdomMsgs();
+
+  // 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, "diff_drive_odom_frame_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_odom_tf.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Make sure to disable odom tf -->
+  <rosparam>
+    diffbot_controller:
+      enable_odom_tf: False
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_odom_tf_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_odom_tf_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_odom_tf_test.cpp

@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 Bence Magyar
+
+#include "test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testNoOdomFrame)
+{
+  // wait for ROS
+  waitForController();
+  
+  // set up tf listener
+  tf::TransformListener listener;
+  ros::Duration(2.0).sleep();
+  // check the odom frame doesn't exist
+  EXPECT_FALSE(listener.frameExists("odom"));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_odom_tf_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_open_loop.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Load diff drive parameter open loop -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_open_loop.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_pub_cmd_vel_out.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <rosparam>
+    diffbot_controller:
+      publish_cmd: True
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_pub_cmd_vel_out_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_pub_cmd_vel_out_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+    <remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_pub_cmd_vel_out_test.cpp

@@ -0,0 +1,74 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2017, PAL Robotics.
+//
+// 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 Jeremie Deray
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testCmdVelOutTopic)
+{
+  // wait for ROS
+  waitForController();
+  // msgs are published in the same loop
+  // thus if odom is published cmd_vel_out
+  // should be as well (if enabled)
+  waitForOdomMsgs();
+
+  EXPECT_TRUE(isPublishingCmdVelOut());
+
+  // 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(0.1).sleep();
+
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  ros::Duration(0.1).sleep();
+
+  EXPECT_TRUE(isPublishingCmdVelOut());
+
+  // get a twist message
+  geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
+
+  EXPECT_GT(fabs(odom_msg.twist.linear.x), 0);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_pub_cmd_vel_out_topic_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state.test

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- default value -->
+   <rosparam>
+    diffbot_controller:
+      publish_wheel_joint_controller_state: True
+  </rosparam>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_publish_wheel_joint_controller_state__test"
+        pkg="diff_drive_controller"
+        type="diff_drive_publish_wheel_joint_controller_state_test"
+        time-limit="30.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+    <remap from="wheel_joint_controller_state" to="diffbot_controller/wheel_joint_controller_state" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state_test.cpp

@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2018, PAL Robotics.
+//
+// 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 Jeremie Deray
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testPublishJointTrajectoryControllerStateTopic)
+{
+  // wait for ROS
+  waitForController();
+
+  EXPECT_TRUE(isPublishingJointTrajectoryControllerState());
+
+  // 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(0.1).sleep();
+
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  ros::Duration(0.1).sleep();
+
+  EXPECT_TRUE(isPublishingJointTrajectoryControllerState());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_publish_wheel_joint_controller_state_topic_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_radius_param.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override robot_description with square wheel urdf -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_square_wheels.xacro'" />
+  <!-- Provide the radius, since the bot's wheels are boxes, not cylinders or spheres -->
+  <param name="diffbot_controller/wheel_radius" value="0.11"/>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_wheel_radius_param_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_radius_param_fail.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override robot_description with bad urdf -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_square_wheels.xacro'" />
+  <!-- Don't provide the radius parameter, so the controller should break -->
+  <!-- <param name="diffbot_controller/wheel_radius" value="0.11"/> -->
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_wheel_radius_param_fail_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_radius_sphere.test

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override robot_description with sphere wheel urdf -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
+  <!-- Don't provide the radius, since controller should accept cylinders *or* spheres -->
+  <!-- param name="diffbot_controller/wheel_radius" value="0.11"/ -->
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_wheel_radius_param_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_separation_param.test

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override robot_description with sphere wheel urdf -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
+  <!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
+  <param name="diffbot_controller/wheel_radius" value="0.11"/>
+  <!-- Provide the wheel separation -->
+  <param name="diffbot_controller/wheel_separation" value="1.0"/>
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_wheel_separation_param_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_test.cpp

@@ -0,0 +1,154 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testForward)
+{
+  // wait for ROS
+  waitForController();
+
+  // 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 velocity command of 0.1 m/s
+  cmd_vel.linear.x = 0.1;
+  publish(cmd_vel);
+  // wait for 10s
+  ros::Duration(10.0).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
+
+  const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.toSec();
+
+  // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
+  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
+  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
+  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
+  EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE);
+  EXPECT_LT(fabs(dz), EPS);
+
+  // convert to rpy and test that way
+  double roll_old, pitch_old, yaw_old;
+  double roll_new, pitch_new, yaw_new;
+  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
+  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
+  EXPECT_LT(fabs(roll_new - roll_old), EPS);
+  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
+  EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
+  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
+
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
+}
+
+TEST_F(DiffDriveControllerTest, testTurn)
+{
+  // wait for ROS
+  waitForController();
+
+  // 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 velocity command
+  cmd_vel.angular.z = M_PI/10.0;
+  publish(cmd_vel);
+  // wait for 10s
+  ros::Duration(10.0).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot rotated PI around z, changes in the other fields should be ~~0
+  EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS);
+  EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS);
+  EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
+
+  const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
+  const double expected_rotation = cmd_vel.angular.z * actual_elapsed_time.toSec();
+
+  // convert to rpy and test that way
+  double roll_old, pitch_old, yaw_old;
+  double roll_new, pitch_new, yaw_new;
+  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
+  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
+  EXPECT_LT(fabs(roll_new - roll_old), EPS);
+  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
+  EXPECT_NEAR(fabs(yaw_new - yaw_old), expected_rotation, ORIENTATION_TOLERANCE);
+
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
+
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
+  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
+  EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), expected_rotation/actual_elapsed_time.toSec(), EPS);
+}
+
+TEST_F(DiffDriveControllerTest, testOdomFrame)
+{
+  // wait for ROS
+  waitForController();
+
+  // set up tf listener
+  tf::TransformListener listener;
+  ros::Duration(2.0).sleep();
+  // check the odom frame exist
+  EXPECT_TRUE(listener.frameExists("odom"));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  //ros::Duration(0.5).sleep();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_timeout.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Load diff-drive cmd_vel_timeout -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_timeout.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_timeout_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_timeout_test"
+        time-limit="20.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diff_drive_timeout_test.cpp

@@ -0,0 +1,70 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testTimeout)
+{
+  // wait for ROS
+  waitForController();
+
+  // zero everything before test
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.angular.z = 0.0;
+  publish(cmd_vel);
+  // give some time to the controller to react to the command
+  ros::Duration(0.1).sleep();
+  // get initial odom
+  nav_msgs::Odometry old_odom = getLastOdom();
+  // send a velocity command of 1 m/s
+  cmd_vel.linear.x = 1.0;
+  publish(cmd_vel);
+  // wait a bit
+  ros::Duration(3.0).sleep();
+
+  nav_msgs::Odometry new_odom = getLastOdom();
+
+  // check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance
+  EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "diff_drive_test");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}

mir_robot/diff_drive_controller/test/diff_drive_wrong.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+  <!-- Override with wrong controller configuration -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_wrong.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="diff_drive_fail_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+    <remap from="odom" to="diffbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/diffbot.cpp

@@ -0,0 +1,93 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+
+#include "diffbot.h"
+#include <chrono>
+#include <thread>
+#include <controller_manager/controller_manager.h>
+#include <ros/ros.h>
+#include <rosgraph_msgs/Clock.h>
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "diffbot");
+  ros::NodeHandle nh;
+
+  // This should be set in launch files as well
+  nh.setParam("/use_sim_time", true);
+
+  Diffbot<> robot;
+  ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
+  controller_manager::ControllerManager cm(&robot, nh);
+
+  ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
+  std::chrono::system_clock::time_point end   = std::chrono::system_clock::now();
+
+  ros::Time internal_time(0);
+  const ros::Duration dt = robot.getPeriod();
+  double elapsed_secs = 0;
+
+  while(ros::ok())
+  {
+    begin = std::chrono::system_clock::now();
+
+    robot.read();
+    cm.update(internal_time, dt);
+
+    robot.write();
+
+    end = std::chrono::system_clock::now();
+
+    elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double> >((end - begin)).count();
+
+    if (dt.toSec() - elapsed_secs < 0.0)
+    {
+      ROS_WARN_STREAM_THROTTLE(
+            0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
+    }
+    else
+    {
+      ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
+      std::this_thread::sleep_for(std::chrono::duration<double>(dt.toSec() - elapsed_secs));
+    }
+
+    rosgraph_msgs::Clock clock;
+    clock.clock = ros::Time(internal_time);
+    clock_publisher.publish(clock);
+    internal_time += dt;
+  }
+  spinner.stop();
+
+  return 0;
+}

mir_robot/diff_drive_controller/test/diffbot.h

@@ -0,0 +1,143 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+
+#pragma once
+
+
+// ROS
+#include <ros/ros.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>
+
+template <unsigned int NUM_JOINTS = 2>
+class Diffbot : public hardware_interface::RobotHW
+{
+public:
+  Diffbot()
+  : running_(true)
+  , start_srv_(nh_.advertiseService("start", &Diffbot::start_callback, this))
+  , stop_srv_(nh_.advertiseService("stop", &Diffbot::stop_callback, this))
+  {
+    // Intialize raw data
+    std::fill_n(pos_, NUM_JOINTS, 0.0);
+    std::fill_n(vel_, NUM_JOINTS, 0.0);
+    std::fill_n(eff_, NUM_JOINTS, 0.0);
+    std::fill_n(cmd_, NUM_JOINTS, 0.0);
+
+    // Connect and register the joint state and velocity interface
+    for (unsigned int i = 0; i < NUM_JOINTS; ++i)
+    {
+      std::ostringstream os;
+      os << "wheel_" << i << "_joint";
+
+      hardware_interface::JointStateHandle state_handle(os.str(), &pos_[i], &vel_[i], &eff_[i]);
+      jnt_state_interface_.registerHandle(state_handle);
+
+      hardware_interface::JointHandle vel_handle(jnt_state_interface_.getHandle(os.str()), &cmd_[i]);
+      jnt_vel_interface_.registerHandle(vel_handle);
+    }
+
+    registerInterface(&jnt_state_interface_);
+    registerInterface(&jnt_vel_interface_);
+  }
+
+  ros::Time getTime() const {return ros::Time::now();}
+  ros::Duration getPeriod() const {return ros::Duration(0.01);}
+
+  void read()
+  {
+    // Read the joint state of the robot into the hardware interface
+    if (running_)
+    {
+      for (unsigned int i = 0; i < NUM_JOINTS; ++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.
+        pos_[i] += vel_[i]*getPeriod().toSec(); // update position
+        vel_[i] = cmd_[i]; // might add smoothing here later
+      }
+    }
+    else
+    {
+      std::fill_n(pos_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
+      std::fill_n(vel_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
+    }
+  }
+
+  void write()
+  {
+    // Write the commands to the joints
+    std::ostringstream os;
+    for (unsigned int i = 0; i < NUM_JOINTS - 1; ++i)
+    {
+      os << cmd_[i] << ", ";
+    }
+    os << cmd_[NUM_JOINTS - 1];
+
+    ROS_INFO_STREAM("Commands for joints: " << os.str());
+  }
+
+  bool start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
+  {
+    running_ = true;
+    return true;
+  }
+
+  bool stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
+  {
+    running_ = false;
+    return true;
+  }
+
+private:
+  hardware_interface::JointStateInterface    jnt_state_interface_;
+  hardware_interface::VelocityJointInterface jnt_vel_interface_;
+  double cmd_[NUM_JOINTS];
+  double pos_[NUM_JOINTS];
+  double vel_[NUM_JOINTS];
+  double eff_[NUM_JOINTS];
+  bool running_;
+
+  ros::NodeHandle nh_;
+  ros::ServiceServer start_srv_;
+  ros::ServiceServer stop_srv_;
+};

mir_robot/diff_drive_controller/test/diffbot.xacro

@@ -0,0 +1,77 @@
+<?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 diff_drive_controller)/test/wheel.xacro"/>
+
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+  <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 -->
+
+  <!-- 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="${width} 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${width} 0.1 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>
+
+
+  <!-- Wheels -->
+  <xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

mir_robot/diff_drive_controller/test/diffbot_bad.xacro

@@ -0,0 +1,142 @@
+<?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
+-->
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="PI" value="3.1415926535897931"/>
+  <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+  <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 -->
+
+  <!-- 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="${width} 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${width} 0.1 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>
+
+
+  <link name="wheel1">
+    <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 ${PI/2} 0"/>
+      <geometry>
+        <sphere radius="${wheel_radius}"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
+      <geometry>
+        <sphere radius="${wheel_radius}"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_w1" type="continuous">
+    <parent link="base_link"/>
+    <child link="wheel1"/>
+    <origin xyz="${width/2+axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+
+  <link name="wheel2">
+    <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 ${PI/2} 0"/>
+      <geometry>
+        <sphere radius="${wheel_radius}"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
+      <geometry>
+        <sphere radius="${wheel_radius}"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_w2" type="continuous">
+    <parent link="base_link"/>
+    <child link="wheel2"/>
+    <origin xyz="${-width/2-axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+
+  <!-- Transmission is important to link the joints and the controller -->
+  <transmission name="joint_w1_trans" type="SimpleTransmission">
+    <actuator name="joint_w1_motor" />
+    <joint name="joint_w1" />
+    <mechanicalReduction>1</mechanicalReduction>
+    <motorTorqueConstant>1</motorTorqueConstant>
+  </transmission>
+
+  <transmission name="joint_w2_trans" type="SimpleTransmission">
+    <actuator name="joint_w2_motor" />
+    <joint name="joint_w2" />
+    <mechanicalReduction>1</mechanicalReduction>
+    <motorTorqueConstant>1</motorTorqueConstant>
+  </transmission>
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="wheel1">
+    <material>Gazebo/Red</material>
+  </gazebo>
+
+  <gazebo reference="wheel2">
+    <material>Gazebo/Blue</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

mir_robot/diff_drive_controller/test/diffbot_controllers.yaml

@@ -0,0 +1,8 @@
+diffbot_controller:
+  type: "diff_drive_controller/DiffDriveController"
+  left_wheel: 'wheel_0_joint'
+  right_wheel: 'wheel_1_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

mir_robot/diff_drive_controller/test/diffbot_left_right_multipliers.yaml

@@ -0,0 +1,4 @@
+diffbot_controller:
+  wheel_separation_multiplier: 2.3
+  left_wheel_radius_multiplier: 1.4
+  right_wheel_radius_multiplier: 1.4

mir_robot/diff_drive_controller/test/diffbot_limits.yaml

@@ -0,0 +1,19 @@
+diffbot_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: 2.0
+      has_acceleration_limits: true
+      max_acceleration: 2.0
+      has_jerk_limits: true
+      max_jerk: 10.0

mir_robot/diff_drive_controller/test/diffbot_multipliers.yaml

@@ -0,0 +1,3 @@
+diffbot_controller:
+  wheel_separation_multiplier: 2.3
+  wheel_radius_multiplier: 1.4

mir_robot/diff_drive_controller/test/diffbot_open_loop.yaml

@@ -0,0 +1,2 @@
+diffbot_controller:
+  open_loop: true

mir_robot/diff_drive_controller/test/diffbot_sphere_wheels.xacro

@@ -0,0 +1,75 @@
+<?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 diff_drive_controller)/test/sphere_wheel.xacro"/>
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+  <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+  <!-- 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="${width} 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${width} 0.1 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>
+
+
+  <!-- Wheels -->
+  <xacro:sphere_wheel name="wheel_0" parent="base" radius="${wheel_radius}">
+    <origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:sphere_wheel>
+  <xacro:sphere_wheel name="wheel_1" parent="base" radius="${wheel_radius}">
+    <origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:sphere_wheel>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

mir_robot/diff_drive_controller/test/diffbot_square_wheels.xacro

@@ -0,0 +1,75 @@
+<?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 diff_drive_controller)/test/square_wheel.xacro"/>
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+  <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+  <!-- 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="${width} 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${width} 0.1 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>
+
+
+  <!-- Wheels -->
+  <xacro:square_wheel name="wheel_0" parent="base" radius="${wheel_radius}">
+    <origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:square_wheel>
+  <xacro:square_wheel name="wheel_1" parent="base" radius="${wheel_radius}">
+    <origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:square_wheel>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

mir_robot/diff_drive_controller/test/diffbot_timeout.yaml

@@ -0,0 +1,2 @@
+diffbot_controller:
+  cmd_vel_timeout: 0.5

mir_robot/diff_drive_controller/test/diffbot_wrong.yaml

@@ -0,0 +1,8 @@
+diffbot_controller:
+  type: "diff_drive_controller/DiffDriveController"
+  left_wheel: 'this_joint_does_not_exist'
+  right_wheel: 'right_wheel_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

mir_robot/diff_drive_controller/test/skid_steer_common.launch

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Use simulation time -->
+  <rosparam param="use_sim_time">True</rosparam>
+
+  <!-- Load skidsteerbot model -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
+
+  <!-- Start skidsteerbot -->
+  <node name="skidsteerbot"
+      pkg="diff_drive_controller"
+      type="skidsteerbot"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_controllers.yaml" />
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="skidsteerbot_controller" />
+
+  <!-- rqt_plot monitoring -->
+  <!--
+  <node name="skidsteerbot_pos_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/skidsteerbot_controller/odom/pose/pose/position/x" />
+
+  <node name="skidsteerbot_vel_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/skidsteerbot_controller/odom/twist/twist/linear/x" />
+  -->
+</launch>

mir_robot/diff_drive_controller/test/skid_steer_controller.test

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/skid_steer_common.launch" />
+
+  <!-- Controller test -->
+  <test test-name="skid_steer_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="skidsteerbot_controller/cmd_vel" />
+    <remap from="odom" to="skidsteerbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/skid_steer_no_wheels.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find diff_drive_controller)/test/skid_steer_common.launch" />
+
+  <!-- Override with wrong controller configuration -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_no_wheels.yaml" />
+
+  <!-- Controller test -->
+  <test test-name="skid_steer_fail_test"
+        pkg="diff_drive_controller"
+        type="diff_drive_fail_test"
+        time-limit="10.0">
+    <remap from="cmd_vel" to="skidsteerbot_controller/cmd_vel" />
+    <remap from="odom" to="skidsteerbot_controller/odom" />
+  </test>
+</launch>

mir_robot/diff_drive_controller/test/skidsteerbot.cpp

@@ -0,0 +1,93 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+
+#include "diffbot.h"
+#include <chrono>
+#include <thread>
+#include <controller_manager/controller_manager.h>
+#include <ros/ros.h>
+#include <rosgraph_msgs/Clock.h>
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "skidsteerbot");
+  ros::NodeHandle nh;
+
+  // This should be set in launch files as well
+  nh.setParam("/use_sim_time", true);
+
+  Diffbot<6> robot;
+  ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
+  controller_manager::ControllerManager cm(&robot, nh);
+
+  ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
+
+  //ros::Rate rate(1.0 / robot.getPeriod().toSec());
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
+  std::chrono::system_clock::time_point end   = std::chrono::system_clock::now();
+
+  ros::Time internal_time(0);
+  const ros::Duration dt = robot.getPeriod();
+  double elapsed_secs = 0;
+
+  while(ros::ok())
+  {
+    begin = std::chrono::system_clock::now();
+
+    robot.read();
+    cm.update(internal_time, dt);
+    robot.write();
+
+    end = std::chrono::system_clock::now();
+
+    elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double> >((end - begin)).count();
+
+    if (dt.toSec() - elapsed_secs < 0.0)
+    {
+      ROS_WARN_STREAM_THROTTLE(
+            0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
+    }
+    else
+    {
+      ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
+      std::this_thread::sleep_for(std::chrono::duration<double>(dt.toSec() - elapsed_secs));
+    }
+
+    rosgraph_msgs::Clock clock;
+    clock.clock = ros::Time(internal_time);
+    clock_publisher.publish(clock);
+    internal_time += dt;
+  }
+  spinner.stop();
+
+  return 0;
+}

mir_robot/diff_drive_controller/test/skidsteerbot.xacro

@@ -0,0 +1,90 @@
+<?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 diff_drive_controller)/test/wheel.xacro"/>
+
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+  <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="y_offset" value="0.35" /> <!-- Offset for the wheels on the same side -->
+
+  <!-- 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="${width} 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${width} 0.1 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>
+
+
+  <!-- Wheels -->
+  <xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${width/2+axel_offset} ${y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_2" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${width/2+axel_offset} ${-y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_3" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-width/2+axel_offset} ${y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_4" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_5" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-width/2+axel_offset} ${-y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>

mir_robot/diff_drive_controller/test/skidsteerbot_controllers.yaml

@@ -0,0 +1,8 @@
+skidsteerbot_controller:
+  type: "diff_drive_controller/DiffDriveController"
+  left_wheel: ['wheel_0_joint', 'wheel_1_joint', 'wheel_2_joint']
+  right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_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

mir_robot/diff_drive_controller/test/skidsteerbot_no_wheels.yaml

@@ -0,0 +1,8 @@
+skidsteerbot_controller:
+  type: "diff_drive_controller/DiffDriveController"
+  left_wheel: []
+  right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_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

mir_robot/diff_drive_controller/test/sphere_wheel.xacro

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="sphere_wheel" params="name parent radius *origin">
+    <link name="${name}_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}_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_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}_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_joint_motor"/>
+      <joint name="${name}_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+</robot>

mir_robot/diff_drive_controller/test/square_wheel.xacro

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="square_wheel" params="name parent radius *origin">
+    <link name="${name}_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>
+          <!-- use boxes for wheels, scale radius to account for "circumference" difference -->
+          <box size="${wheel_radius*1.11} ${wheel_radius*1.11} ${wheel_radius*1.11} "/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="${wheel_radius*1.11} ${wheel_radius*1.11} ${wheel_radius*1.11} "/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_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}_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_joint_motor"/>
+      <joint name="${name}_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+</robot>

mir_robot/diff_drive_controller/test/test_common.h

@@ -0,0 +1,162 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+
+#pragma once
+
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <control_msgs/JointTrajectoryControllerState.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 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 DiffDriveControllerTest : public ::testing::Test
+{
+public:
+
+  DiffDriveControllerTest()
+  : received_first_odom(false)
+  , cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
+  , odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this))
+  , vel_out_sub(nh.subscribe("cmd_vel_out", 100, &DiffDriveControllerTest::cmdVelOutCallback, this))
+  , joint_traj_controller_state_sub(nh.subscribe("wheel_joint_controller_state", 100, &DiffDriveControllerTest::jointTrajectoryControllerStateCallback, this))
+  , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
+  , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
+  {
+  }
+
+  ~DiffDriveControllerTest()
+  {
+    odom_sub.shutdown();
+    joint_traj_controller_state_sub.shutdown();
+  }
+
+  nav_msgs::Odometry getLastOdom(){ return last_odom; }
+  geometry_msgs::TwistStamped getLastCmdVelOut(){ return last_cmd_vel_out; }
+  control_msgs::JointTrajectoryControllerState getLastJointTrajectoryControllerState(){ return last_joint_traj_controller_state; }
+  void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
+  bool isControllerAlive()const{ return (odom_sub.getNumPublishers() > 0); }
+  bool isPublishingCmdVelOut(const ros::Duration &timeout=ros::Duration(1)) const
+  {
+    ros::Time start = ros::Time::now();
+    int get_num_publishers = vel_out_sub.getNumPublishers();
+    while ( (get_num_publishers == 0) && (ros::Time::now() < start + timeout) ) {
+      ros::Duration(0.1).sleep();
+      get_num_publishers = vel_out_sub.getNumPublishers();
+    }
+    return (get_num_publishers > 0);
+  }
+  bool isPublishingJointTrajectoryControllerState(){ return (joint_traj_controller_state_sub.getNumPublishers() > 0); }
+  bool hasReceivedFirstOdom()const{ return received_first_odom; }
+
+  void start(){ std_srvs::Empty srv; start_srv.call(srv); }
+  void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
+
+  void waitForController() const
+  {
+    while(!isControllerAlive() && ros::ok())
+    {
+      ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller.");
+      ros::Duration(0.1).sleep();
+    }
+    if (!ros::ok())
+      FAIL() << "Something went wrong while executing test.";
+  }
+
+  void waitForOdomMsgs() const
+  {
+    while(!hasReceivedFirstOdom() && ros::ok())
+    {
+      ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published.");
+      ros::Duration(0.01).sleep();
+    }
+    if (!ros::ok())
+      FAIL() << "Something went wrong while executing test.";
+  }
+
+private:
+  bool received_first_odom;
+  ros::NodeHandle nh;
+  ros::Publisher cmd_pub;
+  ros::Subscriber odom_sub;
+  ros::Subscriber vel_out_sub;
+  nav_msgs::Odometry last_odom;
+  geometry_msgs::TwistStamped last_cmd_vel_out;
+  ros::Subscriber joint_traj_controller_state_sub;
+  control_msgs::JointTrajectoryControllerState last_joint_traj_controller_state;
+
+  ros::ServiceClient start_srv;
+  ros::ServiceClient stop_srv;
+
+  void odomCallback(const nav_msgs::Odometry& odom)
+  {
+    ROS_INFO_STREAM("Callback received: 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;
+    received_first_odom = true;
+  }
+
+  void jointTrajectoryControllerStateCallback(const control_msgs::JointTrajectoryControllerState& joint_traj_controller_state)
+  {
+    ROS_INFO_STREAM("Joint trajectory controller state callback.");
+    ROS_DEBUG_STREAM("Joint trajectory controller state callback received:\n" <<
+                     joint_traj_controller_state);
+
+    last_joint_traj_controller_state = joint_traj_controller_state;
+  }
+
+  void cmdVelOutCallback(const geometry_msgs::TwistStamped& cmd_vel_out)
+  {
+    ROS_INFO_STREAM("Callback received: lin: " << cmd_vel_out.twist.linear.x
+                     << ", ang: " << cmd_vel_out.twist.angular.z);
+    last_cmd_vel_out = cmd_vel_out;
+  }
+};
+
+inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
+{
+  return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
+}

mir_robot/diff_drive_controller/test/view_diffbot.launch

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- start world -->
+  <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
+  <!-- load robot -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro'" />
+
+  <!-- Start diffbot -->
+  <node name="diffbot"
+      pkg="diff_drive_controller"
+      type="diffbot"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_controllers.yaml" />
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="diffbot_controller" />
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model diffbot -z 0.5" respawn="false" output="screen" />
+
+</launch>

mir_robot/diff_drive_controller/test/view_skidsteerbot.launch

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- start world -->
+  <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
+  <!-- load robot -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
+
+  <!-- Start skidsteerbot -->
+  <node name="skidsteerbot"
+      pkg="diff_drive_controller"
+      type="skidsteerbot"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_controllers.yaml" />
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="skidsteerbot_controller" />
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model skidsteerbot -z 0.5" respawn="false" output="screen" />
+
+</launch>

mir_robot/diff_drive_controller/test/wheel.xacro

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="wheel" params="name parent radius thickness *origin">
+    <link name="${name}_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}_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_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}_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_joint_motor"/>
+      <joint name="${name}_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+</robot>

mir_robot/mir_actions/CHANGELOG.rst

@@ -0,0 +1,70 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mir_actions
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.7 (2023-01-20)
+------------------
+* Update MirMoveBase action to 2.10.3.1
+* Don't set cmake_policy CMP0048
+* Contributors: Martin Günther
+
+1.1.6 (2022-06-02)
+------------------
+* 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)
+------------------
+
+1.1.3 (2021-06-11)
+------------------
+* Merge branch 'melodic-2.8' into noetic
+* Remove RelativeMove action
+  It was removed in MiR software 2.4.0.
+* Update mir_actions to MiR 2.8.3
+* Adjust to changed MirMoveBase action (MiR >= 2.4.0)
+  See `#45 <https://github.com/DFKI-NI/mir_robot/issues/45>`_.
+* Contributors: Martin Günther
+
+1.1.2 (2021-05-12)
+------------------
+
+1.1.1 (2021-02-11)
+------------------
+* 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)
+------------------
+* Update mir_msgs and mir_actions to MiR 2.3.1
+* Contributors: Martin Günther
+
+1.0.3 (2019-03-04)
+------------------
+
+1.0.2 (2018-07-30)
+------------------
+
+1.0.1 (2018-07-17)
+------------------
+
+1.0.0 (2018-07-12)
+------------------
+* Initial release
+* Contributors: Martin Günther

mir_robot/mir_actions/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(mir_actions)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  geometry_msgs
+  message_generation
+  mir_msgs
+  nav_msgs
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate actions in the 'action' folder
+add_action_files(
+  FILES
+    MirMoveBase.action
+)
+
+# Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+    geometry_msgs
+    mir_msgs
+    nav_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS
+    actionlib
+    geometry_msgs
+    message_runtime
+    mir_msgs
+    nav_msgs
+)

mir_robot/mir_actions/action/MirMoveBase.action

@@ -0,0 +1,130 @@
+#goal definition
+#move type
+int16 BASE_MOVE = 0
+int16 GLOBAL_MOVE = 1
+int16 RELATIVE_MOVE = 2
+int16 RELATIVE_MARKER_MOVE = 3
+int16 DOCKING_MOVE = 4
+int16 DOCKING_GLOBAL_MOVE = 5
+int16 PATH_TYPE = 6
+int16 move_task
+
+#shared parameters
+geometry_msgs/PoseStamped target_pose
+string target_guid
+
+#global move parameters
+float64 goal_dist_threshold
+float64 goal_orientation_threshold
+nav_msgs/Path path
+float32 max_plan_time
+bool clear_costmaps
+bool pause_command
+bool continue_command
+
+#relative move parameters
+float64 yaw
+bool collision_detection
+bool collision_avoidance
+float64 disable_collision_check_dist
+float64 max_linear_speed
+float64 max_rotational_speed
+float64 pid_dist_offset
+float64 target_offset
+bool only_collision_detection
+float64 timeout
+
+#docking move parameters
+int32 pattern_type
+int32 pattern_value
+bool only_track
+bool same_goal
+string pose_frame
+geometry_msgs/Pose2D pose
+geometry_msgs/Pose2D offset
+float64 bar_length
+float64 bar_distance
+float64 shelf_leg_asymmetry_x
+float64 tolerance
+
+#Path type
+mir_msgs/MirLocalPlannerPathTypes path_type
+geometry_msgs/PoseStamped start_pose
+# float64 timeout
+
+
+---
+#result definition
+
+#shared states
+int16 UNDEFINED = 0
+int16 GOAL_REACHED = 1
+int16 FAILED = -1
+
+#global move states
+int16 MARKER_VISIBLE = 2
+int16 FAILED_NO_PATH = -2
+int16 FAILED_GOAL_IN_STATIC_OBSTACLE = -3
+int16 FAILED_GOAL_IN_FORBIDDEN_AREA = -4
+int16 FAILED_GOAL_IN_DYNAMIC_OBSTACLE = -5
+int16 FAILED_ROBOT_IN_COLLISION = -6
+int16 FAILED_ROBOT_IN_FORBIDDEN_AREA = -7
+int16 FAILED_UNKNOWN_TRAILER = -8
+int16 FAILED_TO_PASS_GLOBAL_PLAN = -9
+int16 FAILED_NO_VALID_RECOVERY_CONTROL = -10
+int16 FAILED_UNKNOWN_PLANNER_ERROR = -11
+int16 FAILED_ROBOT_OSCILLATING = -12
+int16 FAILED_SOFTWARE_ERROR = -13
+
+#relative move states
+int16 FAILED_TIMEOUT = -14
+int16 FAILED_COLLISION = -15
+int16 INVALID_GOAL = -16
+
+#docking move states
+int16 FAILED_MARKER_TRACKING_ERROR = -17
+
+#shared results
+int16 end_state
+geometry_msgs/PoseStamped end_pose
+
+#docking results
+geometry_msgs/Pose2D pose
+
+#feedback for UI
+string message
+
+---
+#feedback
+#shared
+int8 NOT_READY = -1
+int8 UNKNOWN = -2
+int8 WAITING_FOR_FLEET = -3
+int8 COLLISION = -4
+
+#global move states
+int8 PLANNING = 0
+int8 CONTROLLING = 1
+int8 CLEARING = 2
+
+#relative move states
+int8 DOCKING = 3
+
+#shared feedback
+int8 state
+
+#global move feedback
+geometry_msgs/PoseStamped base_position
+
+#relative move feedback
+geometry_msgs/PoseStamped current_goal
+geometry_msgs/PoseStamped dist_to_goal
+
+#docking move feedback
+geometry_msgs/Pose2D pose
+bool marker_inversion
+
+#path_types
+    #reverse_trolly
+int8 MOVING_FORWARD = 10
+int8 MOVING_BACKWARD = 11

mir_robot/mir_actions/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mir_actions</name>
+  <version>1.1.7</version>
+  <description>Action definitions for the MiR 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/mir_robot</url>
+  <url type="repository">https://github.com/DFKI-NI/mir_robot</url>
+  <url type="bugtracker">https://github.com/DFKI-NI/mir_robot/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>actionlib</depend>
+  <depend>geometry_msgs</depend>
+  <depend>mir_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+</package>

mir_robot/mir_description/CHANGELOG.rst

@@ -0,0 +1,133 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mir_description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.7 (2023-01-20)
+------------------
+* Don't set cmake_policy CMP0048
+* Contributors: Martin Günther
+
+1.1.6 (2022-06-02)
+------------------
+* URDF: Downsize inertia box, move to lower back
+* URDF: Pull out inertia properties
+* URDF: Update masses according to data sheet
+* URDF: Add mir_250
+* Add arg mir_type to launch files and urdfs
+* Add mir_250 meshes
+* URDF: Make wheels black
+* Add mir_100_v1.urdf.xacro for backwards compatibility
+* Rename mir_100 -> mir
+* Refactor URDF to prepare for MiR250 support
+* Gazebo: Don't manually specify wheel params for diffdrive controller
+* Simplify mir_100 collision mesh further
+* Contributors: Martin Günther
+
+1.1.5 (2022-02-11)
+------------------
+* Remove xacro comment to work around xacro bug
+  Since xacro 1.14.11, xacro now also evaluates expressions in comments
+  and throws an error if the substition argument is undefined. In xacro
+  1.14.12, this error was changed to a warning.
+  This commit removes that warning.
+  Workaround for https://github.com/ros/xacro/issues/309 .
+* xacro: drop --inorder option
+  In-order processing became default in ROS Melodic.
+* Add gazebo_plugins to dependency list (`#103 <https://github.com/DFKI-NI/mir_robot/issues/103>`_)
+  This is needed for the ground truth pose via p3d plugin.
+* Contributors: Martin Günther, moooeeeep
+
+1.1.4 (2021-12-10)
+------------------
+* Replace gazebo_plugins IMU with hector_gazebo_plugins
+* Use cylinders instead of STLs for wheel collision geometries
+  Fixes `#99 <https://github.com/DFKI-NI/mir_robot/issues/99>`_.
+* mir_debug_urdf.launch: Fix GUI display
+* 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)
+------------------
+* 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
+* Fix typo in robot_namespace
+* Add missing 'xacro:' xml namespace prefixes
+  Macro calls without 'xacro:' prefix are deprecated in Melodic and will
+  be forbidden in Noetic.
+* Contributors: Martin Günther
+
+1.1.0 (2020-06-30)
+------------------
+* Initial release into noetic
+* Contributors: Martin Günther
+
+1.0.6 (2020-06-30)
+------------------
+* Update to non-deprecated robot_state_publisher node
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.0.5 (2020-05-01)
+------------------
+* Switch from Gazebo GPU laser to normal laser plugin
+  The GPU laser plugin has caused multiple people problems before, because
+  it is not compatible with all GPUS: `#1 <https://github.com/DFKI-NI/mir_robot/issues/1>`_
+  `#32 <https://github.com/DFKI-NI/mir_robot/issues/32>`_
+  `#46 <https://github.com/DFKI-NI/mir_robot/issues/46>`_
+  `#52 <https://github.com/DFKI-NI/mir_robot/issues/52>`_
+  The normal laser plugin directly uses the physics engine, so it doesn't
+  depend on any specific GPU. Also, it doesn't slow down the simulation
+  noticeably (maybe 1-2%).
+* Contributors: Martin Günther
+
+1.0.4 (2019-05-06)
+------------------
+* Add legacyModeNS param to gazebo_ros_control plugin
+  This enables the new behavior of the plugin (pid_gains parameter are now
+  in the proper namespace).
+* re-added gazebo friction parameters for the wheels (`#19 <https://github.com/DFKI-NI/mir_robot/issues/19>`_)
+* Contributors: Martin Günther, niniemann
+
+1.0.3 (2019-03-04)
+------------------
+* 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
+* removed prefix from plugin frameName in sick urdf
+  The gazebo plugins automatically use tf_prefix, even if none is set
+  (in that case it defaults to the robot namespace). That's why we can
+  remove the prefix from the plugins configuration, assuming that the
+  robot namespace will be equal to the prefix.
+* 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
+* workaround eval in xacro for indigo support
+* adds tf_prefix argument to imu.gazebo.urdf.xacro
+* Add TFs for ultrasound sensors
+* Contributors: Martin Günther, Nils Niemann
+
+1.0.2 (2018-07-30)
+------------------
+
+1.0.1 (2018-07-17)
+------------------
+* gazebo: Remove leading slashes in TF frames
+  TF2 doesn't like it (e.g., robot_localization).
+* Contributors: Martin Günther
+
+1.0.0 (2018-07-12)
+------------------
+* Initial release
+* Contributors: Martin Günther

mir_robot/mir_description/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(mir_description)
+
+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
+  meshes
+  rviz
+  urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

mir_robot/mir_description/config/diffdrive_controller.yaml

@@ -0,0 +1,44 @@
+# -----------------------------------
+mobile_base_controller:
+  type        : "diff_drive_controller/DiffDriveController"
+  left_wheel  : '$(arg prefix)left_wheel_joint'
+  right_wheel : '$(arg prefix)right_wheel_joint'
+  publish_rate: 41.2               # this is what the real MiR platform publishes (default: 50)
+  # These covariances are exactly what the real MiR platform publishes
+  pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+  twist_covariance_diagonal: [0.1, 0.1, 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 : 0.445208
+  #wheel_radius : 0.0625
+
+  # Wheel separation and radius multipliers
+  wheel_separation_multiplier: 1.0 # default: 1.0
+  wheel_radius_multiplier    : 1.0 # default: 1.0
+
+  # Velocity commands timeout [s], default 0.5
+  cmd_vel_timeout: 0.5
+
+  # 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    : false
+      max_velocity           : 2.0  # m/s; move_base max_vel_x: 0.8
+      has_acceleration_limits: true
+      max_acceleration       : 3.0  # m/s^2; move_base acc_lim_x: 1.5
+  angular:
+    z:
+      has_velocity_limits    : false
+      max_velocity           : 2.0  # rad/s; move_base max_rot_vel: 1.0
+      has_acceleration_limits: true
+      max_acceleration       : 2.0  # rad/s^2; move_base acc_lim_th: 2.0

mir_robot/mir_description/config/joint_state_controller.yaml

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