HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
.gitignore vendored Executable file
View File

@@ -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

3
.gitmodules vendored Executable file
View File

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

9
.vscode/settings.json vendored Executable file
View File

@@ -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"
]
}

69
CMakeLists.txt Normal file
View File

@@ -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()

125
README.md Executable file
View File

@@ -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)

17
Setup.txt Executable file
View File

@@ -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

2
mir_robot/.dockerfilelintrc Executable file
View File

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

10
mir_robot/.flake8 Executable file
View File

@@ -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,

42
mir_robot/.github/workflows/github-actions.yml vendored Executable file
View File

@@ -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

1
mir_robot/.gitignore vendored Executable file
View File

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

25
mir_robot/.mdl_style.rb Executable file
View File

@@ -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

2
mir_robot/.mdlrc Executable file
View File

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

116
mir_robot/.pre-commit-config.yaml Executable file
View File

@@ -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

34
mir_robot/Dockerfile-noetic Executable file
View File

@@ -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"

27
mir_robot/LICENSE Executable file
View File

@@ -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.

388
mir_robot/README.md Executable file
View File

@@ -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) |

237
mir_robot/diff_drive_controller/CHANGELOG.rst Executable file
View File

@@ -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.

216
mir_robot/diff_drive_controller/CMakeLists.txt Executable file
View File

@@ -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}
)

5
mir_robot/diff_drive_controller/README.md Executable file
View File

@@ -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).

18
mir_robot/diff_drive_controller/cfg/DiffDriveController.cfg Executable file
View File

@@ -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"))

9
mir_robot/diff_drive_controller/diff_drive_controller_plugins.xml Executable file
View File

@@ -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>

312
mir_robot/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h Executable file
View File

@@ -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

210
mir_robot/diff_drive_controller/include/diff_drive_controller/odometry.h Executable file
View File

@@ -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_;
};
}

129
mir_robot/diff_drive_controller/include/diff_drive_controller/speed_limiter.h Executable file
View File

@@ -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

51
mir_robot/diff_drive_controller/package.xml Executable file
View File

@@ -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>

837
mir_robot/diff_drive_controller/src/diff_drive_controller.cpp Executable file
View File

@@ -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);

174
mir_robot/diff_drive_controller/src/odometry.cpp Executable file
View File

@@ -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

137
mir_robot/diff_drive_controller/src/speed_limiter.cpp Executable file
View File

@@ -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

18
mir_robot/diff_drive_controller/test/diff_drive_bad_urdf.test Executable file
View File

@@ -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>

35
mir_robot/diff_drive_controller/test/diff_drive_common.launch Executable file
View File

@@ -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>

14
mir_robot/diff_drive_controller/test/diff_drive_controller.test Executable file
View File

@@ -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>

17
mir_robot/diff_drive_controller/test/diff_drive_controller_limits.test Executable file
View File

@@ -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>

20
mir_robot/diff_drive_controller/test/diff_drive_controller_nan.test Executable file
View File

@@ -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>

21
mir_robot/diff_drive_controller/test/diff_drive_default_cmd_vel_out.test Executable file
View File

@@ -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>

69
mir_robot/diff_drive_controller/test/diff_drive_default_cmd_vel_out_test.cpp Executable file
View File

@@ -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;
}

20
mir_robot/diff_drive_controller/test/diff_drive_default_odom_frame.test Executable file
View File

@@ -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>

69
mir_robot/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp Executable file
View File

@@ -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;
}

21
mir_robot/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state.test Executable file
View File

@@ -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>

65
mir_robot/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state_test.cpp Executable file
View File

@@ -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;
}

20
mir_robot/diff_drive_controller/test/diff_drive_dyn_reconf.test Executable file
View File

@@ -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>

176
mir_robot/diff_drive_controller/test/diff_drive_dyn_reconf_test.cpp Executable file
View File

@@ -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;
}

60
mir_robot/diff_drive_controller/test/diff_drive_fail_test.cpp Executable file
View File

@@ -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;
}

19
mir_robot/diff_drive_controller/test/diff_drive_get_wheel_radius_fail.test Executable file
View File

@@ -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>

17
mir_robot/diff_drive_controller/test/diff_drive_left_right_multipliers.test Executable file
View File

@@ -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>

231
mir_robot/diff_drive_controller/test/diff_drive_limits_test.cpp Executable file
View File

@@ -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;
}

24
mir_robot/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers.test Executable file
View File

@@ -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>

72
mir_robot/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers_test.cpp Executable file
View File

@@ -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;
}

17
mir_robot/diff_drive_controller/test/diff_drive_multipliers.test Executable file
View File

@@ -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>

131
mir_robot/diff_drive_controller/test/diff_drive_nan_test.cpp Executable file
View File

@@ -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;
}

20
mir_robot/diff_drive_controller/test/diff_drive_odom_frame.test Executable file
View File

@@ -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>

83
mir_robot/diff_drive_controller/test/diff_drive_odom_frame_test.cpp Executable file
View File

@@ -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;
}

20
mir_robot/diff_drive_controller/test/diff_drive_odom_tf.test Executable file
View File

@@ -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>

57
mir_robot/diff_drive_controller/test/diff_drive_odom_tf_test.cpp Executable file
View File

@@ -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;
}

17
mir_robot/diff_drive_controller/test/diff_drive_open_loop.test Executable file
View File

@@ -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>

20
mir_robot/diff_drive_controller/test/diff_drive_pub_cmd_vel_out.test Executable file
View File

@@ -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>

74
mir_robot/diff_drive_controller/test/diff_drive_pub_cmd_vel_out_test.cpp Executable file
View File

@@ -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;
}

21
mir_robot/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state.test Executable file
View File

@@ -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>

65
mir_robot/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state_test.cpp Executable file
View File

@@ -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;
}

20
mir_robot/diff_drive_controller/test/diff_drive_radius_param.test Executable file
View File

@@ -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>

20
mir_robot/diff_drive_controller/test/diff_drive_radius_param_fail.test Executable file
View File

@@ -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>

20
mir_robot/diff_drive_controller/test/diff_drive_radius_sphere.test Executable file
View File

@@ -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>

22
mir_robot/diff_drive_controller/test/diff_drive_separation_param.test Executable file
View File

@@ -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>

154
mir_robot/diff_drive_controller/test/diff_drive_test.cpp Executable file
View File

@@ -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;
}

17
mir_robot/diff_drive_controller/test/diff_drive_timeout.test Executable file
View File

@@ -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>

70
mir_robot/diff_drive_controller/test/diff_drive_timeout_test.cpp Executable file
View File

@@ -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;
}

17
mir_robot/diff_drive_controller/test/diff_drive_wrong.test Executable file
View File

@@ -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>

93
mir_robot/diff_drive_controller/test/diffbot.cpp Executable file
View File

@@ -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;
}

143
mir_robot/diff_drive_controller/test/diffbot.h Executable file
View File

@@ -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_;
};

77
mir_robot/diff_drive_controller/test/diffbot.xacro Executable file
View File

@@ -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>

142
mir_robot/diff_drive_controller/test/diffbot_bad.xacro Executable file
View File

@@ -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>

8
mir_robot/diff_drive_controller/test/diffbot_controllers.yaml Executable file
View File

@@ -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

4
mir_robot/diff_drive_controller/test/diffbot_left_right_multipliers.yaml Executable file
View File

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

19
mir_robot/diff_drive_controller/test/diffbot_limits.yaml Executable file
View File

@@ -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

3
mir_robot/diff_drive_controller/test/diffbot_multipliers.yaml Executable file
View File

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

2
mir_robot/diff_drive_controller/test/diffbot_open_loop.yaml Executable file
View File

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

75
mir_robot/diff_drive_controller/test/diffbot_sphere_wheels.xacro Executable file
View File

@@ -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>

75
mir_robot/diff_drive_controller/test/diffbot_square_wheels.xacro Executable file
View File

@@ -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>

2
mir_robot/diff_drive_controller/test/diffbot_timeout.yaml Executable file
View File

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

8
mir_robot/diff_drive_controller/test/diffbot_wrong.yaml Executable file
View File

@@ -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

35
mir_robot/diff_drive_controller/test/skid_steer_common.launch Executable file
View File

@@ -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>

14
mir_robot/diff_drive_controller/test/skid_steer_controller.test Executable file
View File

@@ -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>

17
mir_robot/diff_drive_controller/test/skid_steer_no_wheels.test Executable file
View File

@@ -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>

93
mir_robot/diff_drive_controller/test/skidsteerbot.cpp Executable file
View File

@@ -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;
}

90
mir_robot/diff_drive_controller/test/skidsteerbot.xacro Executable file
View File

@@ -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>

8
mir_robot/diff_drive_controller/test/skidsteerbot_controllers.yaml Executable file
View File

@@ -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

8
mir_robot/diff_drive_controller/test/skidsteerbot_no_wheels.yaml Executable file
View File

@@ -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

43
mir_robot/diff_drive_controller/test/sphere_wheel.xacro Executable file
View File

@@ -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>

44
mir_robot/diff_drive_controller/test/square_wheel.xacro Executable file
View File

@@ -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>

162
mir_robot/diff_drive_controller/test/test_common.h Executable file
View File

@@ -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);
}

26
mir_robot/diff_drive_controller/test/view_diffbot.launch Executable file
View File

@@ -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>

26
mir_robot/diff_drive_controller/test/view_skidsteerbot.launch Executable file
View File

@@ -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>

43
mir_robot/diff_drive_controller/test/wheel.xacro Executable file
View File

@@ -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>

70
mir_robot/mir_actions/CHANGELOG.rst Executable file
View File

@@ -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

40
mir_robot/mir_actions/CMakeLists.txt Executable file
View File

@@ -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
)

130
mir_robot/mir_actions/action/MirMoveBase.action Executable file
View File

@@ -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

25
mir_robot/mir_actions/package.xml Executable file
View File

@@ -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>

133
mir_robot/mir_description/CHANGELOG.rst Executable file
View File

@@ -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

31
mir_robot/mir_description/CMakeLists.txt Executable file
View File

@@ -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)

44
mir_robot/mir_description/config/diffdrive_controller.yaml Executable file
View File

@@ -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

4
mir_robot/mir_description/config/joint_state_controller.yaml Executable file
View File

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

Some files were not shown because too many files have changed in this diff Show More